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

Support optional dual key for constrained Jacobian and nonlinear factors. Default boost::none for unconstrained factors.

release/4.3a0
thduynguyen 2014-09-13 01:34:33 -04:00
parent 5e8c36b3ca
commit 4ca9d5757f
4 changed files with 811 additions and 588 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
gtsam/linear/JacobianFactor-inl.h
View File

@ -26,55 +26,56 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename TERMS> template<typename TERMS>
JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model) JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b,
{ const SharedDiagonal& model, const boost::optional<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms(terms, b, model); fillTerms(terms, b, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<typename KEYS> template<typename KEYS>
JacobianFactor::JacobianFactor( JacobianFactor::JacobianFactor(const KEYS& keys,
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) : const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model,
Base(keys), Ab_(augmentedMatrix) const boost::optional<Key>& dualKey) :
{ Base(keys), Ab_(augmentedMatrix), dualKey_(dualKey) {
// Check noise model dimension // Check noise model dimension
if(model && (DenseIndex)model->dim() != augmentedMatrix.rows()) if (model && (DenseIndex) model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Check number of variables // Check number of variables
if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) if ((DenseIndex) Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument( throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n" "Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks."); "one for the RHS vector must equal the number of provided matrix blocks.");
// Check RHS dimension // Check RHS dimension
if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument( throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided matrix block\n" "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."); "must be the RHS vector, but the last provided block had more than one column.");
// Take noise model // Take noise model
model_ = model; model_ = model;
} }
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal { namespace internal {
static inline DenseIndex getColsJF(const std::pair<Key,Matrix>& p) { static inline DenseIndex getColsJF(const std::pair<Key, Matrix>& p) {
return p.second.cols(); return p.second.cols();
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<typename TERMS> template<typename TERMS>
void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel) void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b,
{ const SharedDiagonal& noiseModel) {
using boost::adaptors::transformed; using boost::adaptors::transformed;
namespace br = boost::range; namespace br = boost::range;
// Check noise model dimension // Check noise model dimension
if(noiseModel && (DenseIndex)noiseModel->dim() != b.size()) if (noiseModel && (DenseIndex) noiseModel->dim() != b.size())
throw InvalidNoiseModel(b.size(), noiseModel->dim()); throw InvalidNoiseModel(b.size(), noiseModel->dim());
// Resize base class key vector // Resize base class key vector
@ -85,15 +86,17 @@ namespace gtsam {
// transforms the list of terms into a list of variable dimensions, by extracting the // 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 // number of columns in each matrix. This is done to avoid separately allocating an
// array of dimensions before constructing the VerticalBlockMatrix. // array of dimensions before constructing the VerticalBlockMatrix.
Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true); Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(),
true);
// Check and add terms // Check and add terms
DenseIndex i = 0; // For block index DenseIndex i = 0; // For block index
for(typename TERMS::const_iterator termIt = terms.begin(); termIt != terms.end(); ++termIt) { for (typename TERMS::const_iterator termIt = terms.begin();
termIt != terms.end(); ++termIt) {
const std::pair<Key, Matrix>& term = *termIt; const std::pair<Key, Matrix>& term = *termIt;
// Check block rows // Check block rows
if(term.second.rows() != Ab_.rows()) if (term.second.rows() != Ab_.rows())
throw InvalidMatrixBlock(Ab_.rows(), term.second.rows()); throw InvalidMatrixBlock(Ab_.rows(), term.second.rows());
// Assign key and matrix // Assign key and matrix
@ -101,7 +104,7 @@ namespace gtsam {
Ab_(i) = term.second; Ab_(i) = term.second;
// Increment block index // Increment block index
++ i; ++i;
} }
// Assign RHS vector // Assign RHS vector
@ -109,7 +112,7 @@ namespace gtsam {
// Assign noise model // Assign noise model
model_ = noiseModel; model_ = noiseModel;
} }
} // gtsam } // gtsam

83
gtsam/linear/JacobianFactor.cpp
View File

@ -59,7 +59,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor() : JacobianFactor::JacobianFactor() :
Ab_(cref_list_of<1>(1), 0) { Ab_(cref_list_of<1>(1), 0), dualKey_(boost::none) {
getb().setZero(); getb().setZero();
} }
@ -77,26 +77,30 @@ JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const Vector& b_in) : 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; getb() = b_in;
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b, JacobianFactor::JacobianFactor(Key i1, const Matrix& A1, const Vector& b,
const SharedDiagonal& model) { const SharedDiagonal& model, const boost::optional<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model); fillTerms(cref_list_of<1>(make_pair(i1, A1)), b, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, 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<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms(cref_list_of<2>(make_pair(i1, A1))(make_pair(i2, A2)), b, model); 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, JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
const Matrix& A2, Key i3, const Matrix& A3, const Vector& b, const Matrix& A2, Key i3, const Matrix& A3, const Vector& b,
const SharedDiagonal& model) { const SharedDiagonal& model, const boost::optional<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms( fillTerms(
cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)), cref_list_of<3>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)),
b, model); 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, JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, 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<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms( fillTerms(
cref_list_of<4>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) cref_list_of<4>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3))(
(make_pair(i4, A4)), b, model); make_pair(i4, A4)), b, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
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 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<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms( fillTerms(
cref_list_of<5>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) 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); make_pair(i4, A4))(make_pair(i5, A5)), b, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
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 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, Key i5, const Matrix& A5, Key i6, const Matrix& A6, const Vector& b,
const SharedDiagonal& model) { const SharedDiagonal& model, const boost::optional<Key>& dualKey) :
dualKey_(dualKey) {
fillTerms( fillTerms(
cref_list_of<6>(make_pair(i1, A1))(make_pair(i2, A2))(make_pair(i3, A3)) 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); make_pair(i4, A4))(make_pair(i5, A5))(make_pair(i6, A6)), b, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor) : JacobianFactor::JacobianFactor(const HessianFactor& factor) :
Base(factor), Ab_( Base(factor), Ab_(
VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(), VerticalBlockMatrix::LikeActiveViewOf(factor.matrixObject(),
factor.rows())) { factor.rows())), dualKey_(boost::none) {
// Copy Hessian into our matrix and then do in-place Cholesky // Copy Hessian into our matrix and then do in-place Cholesky
Ab_.full() = factor.matrixObject().full(); Ab_.full() = factor.matrixObject().full();
@ -214,14 +223,14 @@ boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
"Unable to determine dimensionality for all variables"); "Unable to determine dimensionality for all variables");
} }
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors){
m += factor->rows(); m += factor->rows();
} }
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
BOOST_FOREACH(DenseIndex d, varDims) { BOOST_FOREACH(DenseIndex d, varDims) {
assert(d != numeric_limits<DenseIndex>::max()); assert(d != numeric_limits<DenseIndex>::max());
} }
#endif #endif
return boost::make_tuple(varDims, m, n); return boost::make_tuple(varDims, m, n);
@ -233,7 +242,7 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
gttic(Convert_to_Jacobians); gttic(Convert_to_Jacobians);
FastVector<JacobianFactor::shared_ptr> jacobians; FastVector<JacobianFactor::shared_ptr> jacobians;
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors){
if (factor) { if (factor) {
if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast< if (JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<
JacobianFactor>(factor)) JacobianFactor>(factor))
@ -241,7 +250,7 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
else else
jacobians.push_back(boost::make_shared<JacobianFactor>(*factor)); jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
} }
} }
return jacobians; return jacobians;
} }
} }
@ -249,7 +258,8 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph, JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
boost::optional<const Ordering&> ordering, boost::optional<const Ordering&> ordering,
boost::optional<const VariableSlots&> variableSlots) { boost::optional<const VariableSlots&> variableSlots) :
dualKey_(boost::none) {
gttic(JacobianFactor_combine_constructor); gttic(JacobianFactor_combine_constructor);
// Compute VariableSlots if one was not provided // 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" "The ordering provided to the JacobianFactor combine constructor\n"
"contained extra variables that did not appear in the factors to combine."); "contained extra variables that did not appear in the factors to combine.");
// Add the remaining slots // Add the remaining slots
BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots) { BOOST_FOREACH(VariableSlots::const_iterator item, unorderedSlots){
orderedSlots.push_back(item); orderedSlots.push_back(item);
} }
} else { } else {
// If no ordering is provided, arrange the slots as they were, which will be sorted // If no ordering is provided, arrange the slots as they were, which will be sorted
// numerically since VariableSlots uses a map sorting on Key. // numerically since VariableSlots uses a map sorting on Key.
for (VariableSlots::const_iterator item = variableSlots->begin(); for (VariableSlots::const_iterator item = variableSlots->begin();
item != variableSlots->end(); ++item) item != variableSlots->end(); ++item)
orderedSlots.push_back(item); orderedSlots.push_back(item);
} }
gttoc(Order_slots); gttoc(Order_slots);
// Count dimensions // Count dimensions
@ -321,7 +331,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
// Loop over slots in combined factor and copy blocks from source factors // Loop over slots in combined factor and copy blocks from source factors
gttic(copy_blocks); gttic(copy_blocks);
size_t combinedSlot = 0; size_t combinedSlot = 0;
BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots) { BOOST_FOREACH(VariableSlots::const_iterator varslot, orderedSlots){
JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot)); JacobianFactor::ABlock destSlot(this->getA(this->begin() + combinedSlot));
// Loop over source jacobians // Loop over source jacobians
DenseIndex nextRow = 0; DenseIndex nextRow = 0;
@ -342,7 +352,7 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
} }
} }
++combinedSlot; ++combinedSlot;
} }
gttoc(copy_blocks); gttoc(copy_blocks);
// Copy the RHS vectors and sigmas // Copy the RHS vectors and sigmas
@ -592,7 +602,8 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const double* x,
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues JacobianFactor::gradientAtZero(const boost::optional<Vector&> dual) const { VectorValues JacobianFactor::gradientAtZero(
const boost::optional<Vector&> dual) const {
VectorValues g; VectorValues g;
Vector b = getb(); Vector b = getb();
// Gradient is really -A'*b / sigma^2 // Gradient is really -A'*b / sigma^2
@ -601,7 +612,8 @@ VectorValues JacobianFactor::gradientAtZero(const boost::optional<Vector&> dual)
// scale b by the dual vector if it exists // scale b by the dual vector if it exists
if (dual) { if (dual) {
if (dual->size() != b_sigma.size()) 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); b_sigma = b_sigma.cwiseProduct(*dual);
} }
this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 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"); //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<Matrix, Vector> JacobianFactor::jacobian() const { pair<Matrix, Vector> JacobianFactor::jacobian() const {
pair<Matrix, Vector> result = jacobianUnweighted(); pair<Matrix, Vector> result = jacobianUnweighted();
@ -742,8 +761,8 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd) const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
- Ab_.rowStart() - frontalDim; - Ab_.rowStart() - frontalDim;
const DenseIndex remainingRows = const DenseIndex remainingRows =
model_ ? model_ ? std::min(model_->sigmas().size() - frontalDim,
std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) : maxRemainingRows) :
maxRemainingRows; maxRemainingRows;
Ab_.rowStart() += frontalDim; Ab_.rowStart() += frontalDim;
Ab_.rowEnd() = Ab_.rowStart() + remainingRows; Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
@ -760,7 +779,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
model_ = noiseModel::Constrained::MixedSigmas(sigmas); model_ = noiseModel::Constrained::MixedSigmas(sigmas);
else else
model_ = noiseModel::Diagonal::Sigmas(sigmas, false); // I don't want to be smart here 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); gttoc(remaining_factor);

205
gtsam/linear/JacobianFactor.h
View File

@ -27,20 +27,21 @@
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations
class HessianFactor; class HessianFactor;
class VariableSlots; class VariableSlots;
class GaussianFactorGraph; class GaussianFactorGraph;
class GaussianConditional; class GaussianConditional;
class HessianFactor; class HessianFactor;
class VectorValues; class VectorValues;
class Ordering; class Ordering;
class JacobianFactor; class JacobianFactor;
GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>,
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); boost::shared_ptr<JacobianFactor> >
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
/** /**
* A Gaussian factor in the squared-error form. * A Gaussian factor in the squared-error form.
* *
* JacobianFactor implements a * JacobianFactor implements a
@ -79,29 +80,30 @@ namespace gtsam {
* and the negative log-likelihood represented by this factor would be * 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] * \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 class GTSAM_EXPORT JacobianFactor: public GaussianFactor {
{ public:
public:
typedef JacobianFactor This; ///< Typedef to this class typedef JacobianFactor This; ///< Typedef to this class
typedef GaussianFactor Base; ///< Typedef to base class typedef GaussianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected: protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
boost::optional<Key> dualKey_; // Key of the dual variable associated with this factor if it is a constraint
public: public:
typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock; typedef VerticalBlockMatrix::constBlock constABlock;
typedef ABlock::ColXpr BVector; typedef ABlock::ColXpr BVector;
typedef constABlock::ConstColXpr constBVector; typedef constABlock::ConstColXpr constBVector;
/** Convert from other GaussianFactor */ /** Convert from other GaussianFactor */
explicit JacobianFactor(const GaussianFactor& gf); explicit JacobianFactor(const GaussianFactor& gf);
/** Copy constructor */ /** Copy constructor */
JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {} JacobianFactor(const JacobianFactor& jf) :
Base(jf), Ab_(jf.Ab_), model_(jf.model_), dualKey_(jf.dualKey_) {
}
/** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */
explicit JacobianFactor(const HessianFactor& hf); explicit JacobianFactor(const HessianFactor& hf);
@ -113,60 +115,66 @@ namespace gtsam {
explicit JacobianFactor(const Vector& b_in); explicit JacobianFactor(const Vector& b_in);
/** Construct unary factor */ /** Construct unary factor */
JacobianFactor(Key i1, const Matrix& A1, JacobianFactor(Key i1, const Matrix& A1, const Vector& b,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const SharedDiagonal& model = SharedDiagonal(),
const boost::optional<Key>& dualKey = boost::none);
/** Construct binary factor */ /** Construct binary factor */
JacobianFactor(Key i1, const Matrix& A1, JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2,
Key i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model = SharedDiagonal(),
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const boost::optional<Key>& dualKey = boost::none);
/** Construct ternary factor */ /** Construct ternary factor */
JacobianFactor(Key i1, const Matrix& A1, Key i2, JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A2, Key i3, const Matrix& A3, const Matrix& A3, const Vector& b, const SharedDiagonal& model =
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); SharedDiagonal(), const boost::optional<Key>& dualKey = boost::none);
/** Construct four-ary factor */ /** Construct four-ary factor */
JacobianFactor(Key i1, const Matrix& A1, Key i2, JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, const Matrix& A3, Key i4, const Matrix& A4, const Vector& b,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const SharedDiagonal& model = SharedDiagonal(),
const boost::optional<Key>& dualKey = boost::none);
/** Construct five-ary factor */ /** Construct five-ary factor */
JacobianFactor(Key i1, const Matrix& A1, Key i2, JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5,
Key i5, const Matrix& A5, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); const Vector& b, const SharedDiagonal& model = SharedDiagonal(),
const boost::optional<Key>& dualKey = boost::none);
/** Construct six-ary factor */ /** Construct six-ary factor */
JacobianFactor(Key i1, const Matrix& A1, Key i2, JacobianFactor(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3,
const Matrix& A2, Key i3, const Matrix& A3, Key i4, const Matrix& A4, const Matrix& A3, Key i4, const Matrix& A4, Key i5, const Matrix& A5,
Key i5, const Matrix& A5, Key i6, const Matrix& A6, Key i6, const Matrix& A6, const Vector& b, const SharedDiagonal& model =
const Vector& b, const SharedDiagonal& model = SharedDiagonal()); SharedDiagonal(), const boost::optional<Key>& dualKey = boost::none);
/** Construct an n-ary factor /** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */ * collection of keys and matrices making up the factor. */
template<typename TERMS> template<typename TERMS>
JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal()); JacobianFactor(const TERMS& terms, const Vector& b,
const SharedDiagonal& model = SharedDiagonal(),
const boost::optional<Key>& dualKey = boost::none);
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together /** 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 * 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 * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */ * factor. */
template<typename KEYS> template<typename KEYS>
JacobianFactor( JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix,
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal()); const SharedDiagonal& sigmas = SharedDiagonal(),
const boost::optional<Key>& dualKey = boost::none);
/** /**
* Build a dense joint factor from all the factors in a factor graph. If a VariableSlots * 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 * structure computed for \c graph is already available, providing it will reduce the amount of
* computation performed. */ * computation performed. */
explicit JacobianFactor( explicit JacobianFactor(const GaussianFactorGraph& graph,
const GaussianFactorGraph& graph,
boost::optional<const Ordering&> ordering = boost::none, boost::optional<const Ordering&> ordering = boost::none,
boost::optional<const VariableSlots&> variableSlots = boost::none); boost::optional<const VariableSlots&> variableSlots = boost::none);
/** Virtual destructor */ /** Virtual destructor */
virtual ~JacobianFactor() {} virtual ~JacobianFactor() {
}
/** Clone this JacobianFactor */ /** Clone this JacobianFactor */
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
@ -175,8 +183,8 @@ namespace gtsam {
} }
// Implementing Testable interface // Implementing Testable interface
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "", const KeyFormatter& formatter =
const KeyFormatter& formatter = DefaultKeyFormatter) const; DefaultKeyFormatter) const;
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
@ -205,7 +213,7 @@ namespace gtsam {
virtual void hessianDiagonal(double* d) const; virtual void hessianDiagonal(double* d) const;
/// Return the block diagonal of the Hessian for this factor /// Return the block diagonal of the Hessian for this factor
virtual std::map<Key,Matrix> hessianBlockDiagonal() const; virtual std::map<Key, Matrix> hessianBlockDiagonal() const;
/** /**
* @brief Returns (dense) A,b pair associated with factor, bakes in the weights * @brief Returns (dense) A,b pair associated with factor, bakes in the weights
@ -228,10 +236,14 @@ namespace gtsam {
Matrix augmentedJacobianUnweighted() const; Matrix augmentedJacobianUnweighted() const;
/** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
const VerticalBlockMatrix& matrixObject() const { return Ab_; } const VerticalBlockMatrix& matrixObject() const {
return Ab_;
}
/** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
VerticalBlockMatrix& matrixObject() { return Ab_; } VerticalBlockMatrix& matrixObject() {
return Ab_;
}
/** /**
* Construct the corresponding anti-factor to negate information * Construct the corresponding anti-factor to negate information
@ -241,75 +253,111 @@ namespace gtsam {
virtual GaussianFactor::shared_ptr negate() const; virtual GaussianFactor::shared_ptr negate() const;
/** Check if the factor is empty. TODO: How should this be defined? */ /** Check if the factor is empty. TODO: How should this be defined? */
virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } virtual bool empty() const {
return size() == 0 /*|| rows() == 0*/;
}
/** is noise model constrained ? */ /** is noise model constrained ? */
bool isConstrained() const { return model_ && model_->isConstrained(); } bool isConstrained() const {
return model_ && model_->isConstrained();
}
/** Return the dimension of the variable pointed to by the given key iterator /** 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? * todo: Remove this in favor of keeping track of dimensions with variables?
*/ */
virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } virtual DenseIndex getDim(const_iterator variable) const {
return Ab_(variable - begin()).cols();
}
/** /**
* return the number of rows in the corresponding linear system * return the number of rows in the corresponding linear system
*/ */
size_t rows() const { return Ab_.rows(); } size_t rows() const {
return Ab_.rows();
}
/** /**
* return the number of columns in the corresponding linear system * return the number of columns in the corresponding linear system
*/ */
size_t cols() const { return Ab_.cols(); } size_t cols() const {
return Ab_.cols();
}
/** get a copy of model */ /** get a copy of model */
const SharedDiagonal& get_model() const { return model_; } const SharedDiagonal& get_model() const {
return model_;
}
/** get a copy of model (non-const version) */ /** get a copy of model (non-const version) */
SharedDiagonal& get_model() { return model_; } SharedDiagonal& get_model() {
return model_;
}
/** Get a view of the r.h.s. vector b, not weighted by noise */ /** Get a view of the r.h.s. vector b, not weighted by noise */
const constBVector getb() const { return Ab_(size()).col(0); } 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 */ /** 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()); } constABlock getA(const_iterator variable) const {
return Ab_(variable - begin());
}
/** Get a view of the A matrix, not weighted by noise */ /** Get a view of the A matrix, not weighted by noise */
constABlock getA() const { return Ab_.range(0, size()); } constABlock getA() const {
return Ab_.range(0, size());
}
/** Get a view of the r.h.s. vector b (non-const version) */ /** Get a view of the r.h.s. vector b (non-const version) */
BVector getb() { return Ab_(size()).col(0); } BVector getb() {
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) */ /** 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()); } ABlock getA(iterator variable) {
return Ab_(variable - begin());
}
/** Get a view of the A matrix */ /** Get a view of the A matrix */
ABlock getA() { return Ab_.range(0, size()); } ABlock getA() {
return Ab_.range(0, size());
}
/** Return A*x */ /** Return A*x */
Vector operator*(const VectorValues& x) const; Vector operator*(const VectorValues& x) const;
/** x += A'*e. If x is initially missing any values, they are created and assumed to start as /** x += A'*e. If x is initially missing any values, they are created and assumed to start as
* zero vectors. */ * zero vectors. */
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; void transposeMultiplyAdd(double alpha, const Vector& e,
VectorValues& x) const;
/** y += alpha * A'*A*x */ /** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const;
void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const; void multiplyHessianAdd(double alpha, const double* x, double* y,
std::vector<size_t> keys) const;
void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; void multiplyHessianAdd(double alpha, const double* x, double* y) const {
}
;
/// A'*b for Jacobian /// A'*b for Jacobian
VectorValues gradientAtZero(const boost::optional<Vector&> dual = boost::none) const; VectorValues gradientAtZero(
const boost::optional<Vector&> dual = boost::none) const;
/* ************************************************************************* */ /* ************************************************************************* */
virtual void gradientAtZero(double* d) const; virtual void gradientAtZero(double* d) const;
/// Compute the gradient wrt a key at any values
Vector gradient(Key key, const VectorValues& x) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const; JacobianFactor whiten() const;
/** Eliminate the requested variables. */ /** Eliminate the requested variables. */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > std::pair<boost::shared_ptr<GaussianConditional>,
boost::shared_ptr<JacobianFactor> >
eliminate(const Ordering& keys); eliminate(const Ordering& keys);
/** set noiseModel correctly */ /** set noiseModel correctly */
@ -327,7 +375,8 @@ namespace gtsam {
* @return The conditional and remaining factor * @return The conditional and remaining factor
* *
* \addtogroup LinearSolving */ * \addtogroup LinearSolving */
friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>,
boost::shared_ptr<JacobianFactor> >
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys); EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
/** /**
@ -337,13 +386,21 @@ namespace gtsam {
*/ */
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals); boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
protected: /// 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 /// Internal function to fill blocks and set dimensions
template<typename TERMS> template<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); void fillTerms(const TERMS& terms, const Vector& b,
const SharedDiagonal& noiseModel);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -353,10 +410,10 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(Ab_); ar & BOOST_SERIALIZATION_NVP(Ab_);
ar & BOOST_SERIALIZATION_NVP(model_); ar & BOOST_SERIALIZATION_NVP(model_);
} }
}; // JacobianFactor };
// JacobianFactor
} // gtsam }// gtsam
#include <gtsam/linear/JacobianFactor-inl.h> #include <gtsam/linear/JacobianFactor-inl.h>

448
gtsam/nonlinear/NonlinearFactor.h
View File

@ -17,7 +17,6 @@
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <boost/serialization/base_object.hpp> #include <boost/serialization/base_object.hpp>
@ -28,7 +27,6 @@
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Factor.h> #include <gtsam/inference/Factor.h>
/** /**
* Macro to add a standard clone function to a derived factor * Macro to add a standard clone function to a derived factor
* @deprecated: will go away shortly - just add the clone function directly * @deprecated: will go away shortly - just add the clone function directly
@ -59,6 +57,8 @@ protected:
typedef Factor Base; typedef Factor Base;
typedef NonlinearFactor This; typedef NonlinearFactor This;
boost::optional<Key> dualKey_; //!< Key for the dual variable associated with this factor if it is a constraint
public: public:
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
@ -67,14 +67,23 @@ public:
/// @{ /// @{
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NonlinearFactor() {} NonlinearFactor() :
dualKey_(boost::none) {
}
/** Construct with a dual key */
NonlinearFactor(const boost::optional<Key>& dualKey) :
dualKey_(dualKey) {
}
/** /**
* Constructor from a collection of the keys involved in this factor * Constructor from a collection of the keys involved in this factor
*/ */
template<typename CONTAINER> template<typename CONTAINER>
NonlinearFactor(const CONTAINER& keys) : NonlinearFactor(const CONTAINER& keys, const boost::optional<Key>& dualKey =
Base(keys) {} boost::none) :
Base(keys), dualKey_(dualKey) {
}
/// @} /// @}
/// @name Testable /// @name Testable
@ -82,10 +91,9 @@ public:
/** print */ /** print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
{
std::cout << s << " keys = { "; 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; std::cout << "}" << std::endl;
} }
@ -99,8 +107,8 @@ public:
/// @{ /// @{
/** Destructor */ /** Destructor */
virtual ~NonlinearFactor() {} virtual ~NonlinearFactor() {
}
/** /**
* Calculate the error of the factor * Calculate the error of the factor
@ -122,7 +130,9 @@ public:
* when the constraint is *NOT* fulfilled. * when the constraint is *NOT* fulfilled.
* @return true if the constraint is active * @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 */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> virtual boost::shared_ptr<GaussianFactor>
@ -138,7 +148,6 @@ public:
// indices[j] = ordering[this->keys()[j]]; // indices[j] = ordering[this->keys()[j]];
// return IndexFactor::shared_ptr(new IndexFactor(indices)); // return IndexFactor::shared_ptr(new IndexFactor(indices));
//} //}
/** /**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow * Creates a shared_ptr clone of the factor - needs to be specialized to allow
* for subclasses * for subclasses
@ -147,7 +156,8 @@ public:
*/ */
virtual shared_ptr clone() const { virtual shared_ptr clone() const {
// TODO: choose better exception to throw here // 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(); return shared_ptr();
} }
@ -155,11 +165,11 @@ public:
* Creates a shared_ptr clone of the factor with different keys using * Creates a shared_ptr clone of the factor with different keys using
* a map from old->new keys * a map from old->new keys
*/ */
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const { shared_ptr rekey(const std::map<Key, Key>& rekey_mapping) const {
shared_ptr new_factor = clone(); shared_ptr new_factor = clone();
for (size_t i=0; i<new_factor->size(); ++i) { for (size_t i = 0; i < new_factor->size(); ++i) {
Key& cur_key = new_factor->keys()[i]; Key& cur_key = new_factor->keys()[i];
std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key); std::map<Key, Key>::const_iterator mapping = rekey_mapping.find(cur_key);
if (mapping != rekey_mapping.end()) if (mapping != rekey_mapping.end())
cur_key = mapping->second; cur_key = mapping->second;
} }
@ -177,8 +187,27 @@ public:
return new_factor; 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<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NoiseModelFactor() {} NoiseModelFactor() :
Base() {
}
/** Destructor */ /** Destructor */
virtual ~NoiseModelFactor() {} virtual ~NoiseModelFactor() {
}
/** /**
* Constructor * Constructor
*/ */
template<typename CONTAINER> template<typename CONTAINER>
NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) : NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys,
Base(keys), noiseModel_(noiseModel) {} const boost::optional<Key>& dualKey = boost::none) :
Base(keys, dualKey), noiseModel_(noiseModel) {
}
protected: protected:
/** /**
* Constructor - only for subclasses, as this does not set keys. * Constructor - only for subclasses, as this does not set keys.
*/ */
NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {} NoiseModelFactor(const SharedNoiseModel& noiseModel,
const boost::optional<Key>& dualKey = boost::none) : Base(dualKey),
noiseModel_(noiseModel) {
}
public: public:
/** Print */ /** Print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
{
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
if (dualKey_) {
std::cout << "Dual Key: " << this->dualKey() << std::endl;
}
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f); const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) && return e && Base::equals(f, tol)
((!noiseModel_ && !e->noiseModel_) || && ((!noiseModel_ && !e->noiseModel_)
(noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol))); || (noiseModel_ && e->noiseModel_
&& noiseModel_->equals(*e->noiseModel_, tol)));
} }
/** get the dimension of the factor (number of rows on linearization) */ /** 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 * 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...). * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
*/ */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0; virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/** /**
* Vector of errors, whitened * Vector of errors, whitened
@ -267,8 +308,9 @@ public:
*/ */
Vector whitenedError(const Values& c) const { Vector whitenedError(const Values& c) const {
const Vector unwhitenedErrorVec = unwhitenedError(c); const Vector unwhitenedErrorVec = unwhitenedError(c);
if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
return noiseModel_->whiten(unwhitenedErrorVec); return noiseModel_->whiten(unwhitenedErrorVec);
} }
@ -281,8 +323,9 @@ public:
virtual double error(const Values& c) const { virtual double error(const Values& c) const {
if (this->active(c)) { if (this->active(c)) {
const Vector unwhitenedErrorVec = unwhitenedError(c); const Vector unwhitenedErrorVec = unwhitenedError(c);
if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim()) if ((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); throw std::invalid_argument(
"This factor was created with a NoiseModel of incorrect dimension.");
return 0.5 * noiseModel_->distance(unwhitenedErrorVec); return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
} else { } else {
return 0.0; return 0.0;
@ -304,29 +347,30 @@ public:
// Call evaluate error to get Jacobians and b vector // Call evaluate error to get Jacobians and b vector
std::vector<Matrix> A(this->size()); std::vector<Matrix> A(this->size());
b = -unwhitenedError(x, A); b = -unwhitenedError(x, A);
if(noiseModel_) if (noiseModel_) {
{ if ((size_t) b.size() != noiseModel_->dim())
if((size_t) b.size() != noiseModel_->dim()) throw std::invalid_argument(
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension."); "This factor was created with a NoiseModel of incorrect dimension.");
this->noiseModel_->WhitenSystem(A,b); this->noiseModel_->WhitenSystem(A, b);
} }
std::vector<std::pair<Key, Matrix> > terms(this->size()); std::vector<std::pair<Key, Matrix> > terms(this->size());
// Fill in terms // Fill in terms
for(size_t j=0; j<this->size(); ++j) { for (size_t j = 0; j < this->size(); ++j) {
terms[j].first = this->keys()[j]; terms[j].first = this->keys()[j];
terms[j].second.swap(A[j]); terms[j].second.swap(A[j]);
} }
// TODO pass unwhitened + noise model to Gaussian factor // TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!! // For now, only linearized constrained factors have noise model at linear level!!!
if(noiseModel_) if (noiseModel_) {
{
noiseModel::Constrained::shared_ptr constrained = noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); boost::dynamic_pointer_cast<noiseModel::Constrained>(
if(constrained) this->noiseModel_);
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit())); if (constrained)
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, constrained->unit(), dualKey_));
} }
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
@ -338,13 +382,14 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor", ar
& boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(noiseModel_); ar & BOOST_SERIALIZATION_NVP(noiseModel_);
} }
}; // \class NoiseModelFactor };
// \class NoiseModelFactor
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1 /** A convenient base class for creating your own NoiseModelFactor with 1
@ -365,27 +410,35 @@ protected:
public: public:
/** Default constructor for I/O only */ /** 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 * Constructor
* @param noiseModel shared pointer to noise model * @param noiseModel shared pointer to noise model
* @param key1 by which to look up X value in Values * @param key1 by which to look up X value in Values
*/ */
NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1,
Base(noiseModel, cref_list_of<1>(key1)) {} const boost::optional<Key>& dualKey = boost::none) :
Base(noiseModel, cref_list_of<1>(key1), dualKey) {
}
/** Calls the 1-key specific version of evaluateError, which is pure virtual /** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. * so must be implemented in the derived class.
*/ */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x,
if(this->active(x)) { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (this->active(x)) {
const X& x1 = x.at<X>(keys_[0]); const X& x1 = x.at<X>(keys_[0]);
if(H) { if (H) {
return evaluateError(x1, (*H)[0]); return evaluateError(x1, (*H)[0]);
} else { } else {
return evaluateError(x1); return evaluateError(x1);
@ -409,11 +462,12 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
};// \class NoiseModelFactor1 };
// \class NoiseModelFactor1
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2 /** A convenient base class for creating your own NoiseModelFactor with 2
@ -437,7 +491,9 @@ public:
/** /**
* Default Constructor for I/O * Default Constructor for I/O
*/ */
NoiseModelFactor2() {} NoiseModelFactor2() :
Base() {
}
/** /**
* Constructor * Constructor
@ -445,22 +501,30 @@ public:
* @param j1 key of the first variable * @param j1 key of the first variable
* @param j2 key of the second variable * @param j2 key of the second variable
*/ */
NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2,
Base(noiseModel, cref_list_of<2>(j1)(j2)) {} const boost::optional<Key>& dualKey = boost::none) :
Base(noiseModel, cref_list_of<2>(j1)(j2), dualKey) {
}
virtual ~NoiseModelFactor2() {} virtual ~NoiseModelFactor2() {
}
/** methods to retrieve both keys */ /** methods to retrieve both keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const {
inline Key key2() const { return keys_[1]; } return keys_[0];
}
inline Key key2() const {
return keys_[1];
}
/** Calls the 2-key specific version of evaluateError, which is pure virtual /** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x,
if(this->active(x)) { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (this->active(x)) {
const X1& x1 = x.at<X1>(keys_[0]); const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x.at<X2>(keys_[1]); const X2& x2 = x.at<X2>(keys_[1]);
if(H) { if (H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]); return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else { } else {
return evaluateError(x1, x2); return evaluateError(x1, x2);
@ -476,8 +540,8 @@ public:
* both the function evaluation and its derivative(s) in X1 (and/or X2). * both the function evaluation and its derivative(s) in X1 (and/or X2).
*/ */
virtual Vector virtual Vector
evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 = evaluateError(const X1&, const X2&, boost::optional<Matrix&> H1 = boost::none,
boost::none, boost::optional<Matrix&> H2 = boost::none) const = 0; boost::optional<Matrix&> H2 = boost::none) const = 0;
private: private:
@ -485,10 +549,12 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor2 };
// \class NoiseModelFactor2
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3 /** A convenient base class for creating your own NoiseModelFactor with 3
@ -513,7 +579,9 @@ public:
/** /**
* Default Constructor for I/O * Default Constructor for I/O
*/ */
NoiseModelFactor3() {} NoiseModelFactor3() :
Base() {
}
/** /**
* Constructor * Constructor
@ -522,24 +590,36 @@ public:
* @param j2 key of the second variable * @param j2 key of the second variable
* @param j3 key of the third variable * @param j3 key of the third variable
*/ */
NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3,
Base(noiseModel, cref_list_of<3>(j1)(j2)(j3)) {} const boost::optional<Key>& dualKey = boost::none) :
Base(noiseModel, cref_list_of<3>(j1)(j2)(j3), dualKey) {
}
virtual ~NoiseModelFactor3() {} virtual ~NoiseModelFactor3() {
}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const {
inline Key key2() const { return keys_[1]; } return keys_[0];
inline Key key3() const { return keys_[2]; } }
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 /** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x,
if(this->active(x)) { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H) if (this->active(x)) {
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); if (H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
else else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2])); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -551,9 +631,8 @@ public:
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3). * both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/ */
virtual Vector virtual Vector
evaluateError(const X1&, const X2&, const X3&, evaluateError(const X1&, const X2&, const X3&, boost::optional<Matrix&> H1 =
boost::optional<Matrix&> H1 = boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const = 0; boost::optional<Matrix&> H3 = boost::none) const = 0;
private: private:
@ -562,10 +641,12 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor3 };
// \class NoiseModelFactor3
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4 /** A convenient base class for creating your own NoiseModelFactor with 4
@ -591,7 +672,9 @@ public:
/** /**
* Default Constructor for I/O * Default Constructor for I/O
*/ */
NoiseModelFactor4() {} NoiseModelFactor4() :
Base() {
}
/** /**
* Constructor * Constructor
@ -601,25 +684,40 @@ public:
* @param j3 key of the third variable * @param j3 key of the third variable
* @param j4 key of the fourth variable * @param j4 key of the fourth variable
*/ */
NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3,
Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4)) {} Key j4, const boost::optional<Key>& dualKey = boost::none) :
Base(noiseModel, cref_list_of<4>(j1)(j2)(j3)(j4), dualKey) {
}
virtual ~NoiseModelFactor4() {} virtual ~NoiseModelFactor4() {
}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const {
inline Key key2() const { return keys_[1]; } return keys_[0];
inline Key key3() const { return keys_[2]; } }
inline Key key4() const { return keys_[3]; } 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 /** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x,
if(this->active(x)) { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H) if (this->active(x)) {
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); if (H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2],
(*H)[3]);
else else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3])); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -632,9 +730,8 @@ public:
*/ */
virtual Vector virtual Vector
evaluateError(const X1&, const X2&, const X3&, const X4&, evaluateError(const X1&, const X2&, const X3&, const X4&,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2 = boost::none, boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const = 0; boost::optional<Matrix&> H4 = boost::none) const = 0;
private: private:
@ -643,10 +740,12 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor4 };
// \class NoiseModelFactor4
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5 /** A convenient base class for creating your own NoiseModelFactor with 5
@ -673,7 +772,9 @@ public:
/** /**
* Default Constructor for I/O * Default Constructor for I/O
*/ */
NoiseModelFactor5() {} NoiseModelFactor5() :
Base() {
}
/** /**
* Constructor * Constructor
@ -684,26 +785,43 @@ public:
* @param j4 key of the fourth variable * @param j4 key of the fourth variable
* @param j5 key of the fifth variable * @param j5 key of the fifth variable
*/ */
NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3,
Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5)) {} Key j4, Key j5, const boost::optional<Key>& dualKey = boost::none) :
Base(noiseModel, cref_list_of<5>(j1)(j2)(j3)(j4)(j5), dualKey) {
}
virtual ~NoiseModelFactor5() {} virtual ~NoiseModelFactor5() {
}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const {
inline Key key2() const { return keys_[1]; } return keys_[0];
inline Key key3() const { return keys_[2]; } }
inline Key key4() const { return keys_[3]; } inline Key key2() const {
inline Key key5() const { return keys_[4]; } 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 /** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x,
if(this->active(x)) { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H) if (this->active(x)) {
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); if (H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0],
(*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4])); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -716,11 +834,10 @@ public:
*/ */
virtual Vector virtual Vector
evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2 = boost::none, boost::none, boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::optional<Matrix&> H4 = boost::none, boost::none) const = 0;
boost::optional<Matrix&> H5 = boost::none) const = 0;
private: private:
@ -728,15 +845,18 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor5 };
// \class NoiseModelFactor5
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6 /** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6> template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5,
class VALUE6>
class NoiseModelFactor6: public NoiseModelFactor { class NoiseModelFactor6: public NoiseModelFactor {
public: public:
@ -759,7 +879,9 @@ public:
/** /**
* Default Constructor for I/O * Default Constructor for I/O
*/ */
NoiseModelFactor6() {} NoiseModelFactor6() :
Base() {
}
/** /**
* Constructor * Constructor
@ -771,27 +893,48 @@ public:
* @param j5 key of the fifth variable * @param j5 key of the fifth variable
* @param j6 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) : NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3,
Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)) {} Key j4, Key j5, Key j6, const boost::optional<Key>& dualKey = boost::none) :
Base(noiseModel, cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6), dualKey) {
}
virtual ~NoiseModelFactor6() {} virtual ~NoiseModelFactor6() {
}
/** methods to retrieve keys */ /** methods to retrieve keys */
inline Key key1() const { return keys_[0]; } inline Key key1() const {
inline Key key2() const { return keys_[1]; } return keys_[0];
inline Key key3() const { return keys_[2]; } }
inline Key key4() const { return keys_[3]; } inline Key key2() const {
inline Key key5() const { return keys_[4]; } return keys_[1];
inline Key key6() const { return keys_[5]; } }
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 /** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const Values& x,
if(this->active(x)) { boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(H) if (this->active(x)) {
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); if (H)
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]),
x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4],
(*H)[5]);
else else
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5])); return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]),
x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]),
x.at<X6>(keys_[5]));
} else { } else {
return zero(this->dim()); return zero(this->dim());
} }
@ -803,13 +946,12 @@ public:
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3). * both the function evaluation and its derivative(s) in X1 (and/or X2, X3).
*/ */
virtual Vector virtual Vector
evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&, evaluateError(const X1&, const X2&, const X3&, const X4&, const X5&,
boost::optional<Matrix&> H1 = boost::none, const X6&, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::optional<Matrix&> H3 = boost::none, boost::none, boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 = boost::none, boost::optional<Matrix&> H6 =
boost::optional<Matrix&> H5 = boost::none, boost::none) const = 0;
boost::optional<Matrix&> H6 = boost::none) const = 0;
private: private:
@ -817,11 +959,13 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor", ar
& boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor6 };
// \class NoiseModelFactor6
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam }// \namespace gtsam