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

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

@ -26,90 +26,93 @@
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) :
fillTerms(terms, b, model); dualKey_(dualKey) {
fillTerms(terms, b, model);
}
/* ************************************************************************* */
template<typename KEYS>
JacobianFactor::JacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model,
const boost::optional<Key>& 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<Key, Matrix>& p) {
return p.second.cols();
}
}
/* ************************************************************************* */
template<typename TERMS>
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<Key, Matrix>& 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
template<typename KEYS> getb() = b;
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());
// Check number of variables // Assign noise model
if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) model_ = noiseModel;
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<Key,Matrix>& p) {
return p.second.cols();
}
}
/* ************************************************************************* */
template<typename TERMS>
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<Key, Matrix>& 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;
}
} // gtsam } // gtsam

147
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,15 +242,15 @@ 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))
jacobians.push_back(jf); jacobians.push_back(jf);
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 {
// 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);
} }
} 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); gttoc(Order_slots);
// Count dimensions // Count dimensions
@ -321,28 +331,28 @@ 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;
for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) { for (size_t factorI = 0; factorI < jacobians.size(); ++factorI) {
// Slot in source factor // Slot in source factor
const size_t sourceSlot = varslot->second[factorI]; const size_t sourceSlot = varslot->second[factorI];
const DenseIndex sourceRows = jacobians[factorI]->rows(); const DenseIndex sourceRows = jacobians[factorI]->rows();
if (sourceRows > 0) { if (sourceRows > 0) {
JacobianFactor::ABlock::RowsBlockXpr destBlock( JacobianFactor::ABlock::RowsBlockXpr destBlock(
destSlot.middleRows(nextRow, sourceRows)); destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero // Copy if exists in source factor, otherwise set zero
if (sourceSlot != VariableSlots::Empty) if (sourceSlot != VariableSlots::Empty)
destBlock = jacobians[factorI]->getA( destBlock = jacobians[factorI]->getA(
jacobians[factorI]->begin() + sourceSlot); jacobians[factorI]->begin() + sourceSlot);
else else
destBlock.setZero(); destBlock.setZero();
nextRow += sourceRows; nextRow += sourceRows;
}
} }
++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);

625
gtsam/linear/JacobianFactor.h
View File

@ -27,336 +27,393 @@
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.
*
* 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<This> 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<Key> 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<Key>& 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<Key>& 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<Key>& 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<Key>& 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<Key>& 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<Key>& dualKey = boost::none);
/** Construct an n-ary factor
* @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
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
* 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<typename KEYS>
JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix,
const SharedDiagonal& sigmas = SharedDiagonal(),
const boost::optional<Key>& dualKey = boost::none);
/** /**
* A Gaussian factor in the squared-error form. * 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
* JacobianFactor implements a * computation performed. */
* Gaussian, which has quadratic negative log-likelihood explicit JacobianFactor(const GaussianFactorGraph& graph,
* \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<This> 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<Key, Matrix>, specifying the
* collection of keys and matrices making up the factor. */
template<typename TERMS>
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<typename KEYS>
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,
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 {
return boost::static_pointer_cast<GaussianFactor>( return boost::static_pointer_cast<GaussianFactor>(
boost::make_shared<JacobianFactor>(*this)); boost::make_shared<JacobianFactor>(*this));
} }
// 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) */
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ 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) */ 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. /** Return the augmented information matrix represented by this GaussianFactor.
* The augmented information matrix contains the information matrix with an * The augmented information matrix contains the information matrix with an
* additional column holding the information vector, and an additional row * additional column holding the information vector, and an additional row
* holding the transpose of the information vector. The lower-right entry * holding the transpose of the information vector. The lower-right entry
* contains the constant error term (when \f$ \delta x = 0 \f$). The * contains the constant error term (when \f$ \delta x = 0 \f$). The
* augmented information matrix is described in more detail in HessianFactor, * augmented information matrix is described in more detail in HessianFactor,
* which in fact stores an augmented information matrix. * which in fact stores an augmented information matrix.
*/ */
virtual Matrix augmentedInformation() const; 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 non-augmented information matrix represented by this
virtual void hessianDiagonal(double* d) const; * GaussianFactor.
*/
virtual Matrix information() const;
/// Return the block diagonal of the Hessian for this factor /// Return the diagonal of the Hessian for this factor
virtual std::map<Key,Matrix> hessianBlockDiagonal() const; virtual VectorValues hessianDiagonal() const;
/** /* ************************************************************************* */
* @brief Returns (dense) A,b pair associated with factor, bakes in the weights virtual void hessianDiagonal(double* d) const;
*/
virtual std::pair<Matrix, Vector> jacobian() const;
/**
* @brief Returns (dense) A,b pair associated with factor, does not bake in weights
*/
std::pair<Matrix, Vector> jacobianUnweighted() const;
/** Return (dense) matrix associated with factor. The returned system is an augmented matrix: /// Return the block diagonal of the Hessian for this factor
* [A b] virtual std::map<Key, Matrix> hessianBlockDiagonal() const;
* weights are baked in */
virtual Matrix augmentedJacobian() const;
/** Return (dense) matrix associated with factor. The returned system is an augmented matrix: /**
* [A b] * @brief Returns (dense) A,b pair associated with factor, bakes in the weights
* weights are not baked in */ */
Matrix augmentedJacobianUnweighted() const; virtual std::pair<Matrix, Vector> 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<Matrix, Vector> jacobianUnweighted() const;
/** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ /** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
VerticalBlockMatrix& matrixObject() { return Ab_; } * [A b]
* weights are baked in */
virtual Matrix augmentedJacobian() const;
/** /** Return (dense) matrix associated with factor. The returned system is an augmented matrix:
* Construct the corresponding anti-factor to negate information * [A b]
* stored stored in this factor. * weights are not baked in */
* @return a HessianFactor with negated Hessian matrices Matrix augmentedJacobianUnweighted() const;
*/
virtual GaussianFactor::shared_ptr negate() const;
/** Check if the factor is empty. TODO: How should this be defined? */ /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } const VerticalBlockMatrix& matrixObject() const {
return Ab_;
}
/** is noise model constrained ? */ /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
bool isConstrained() const { return model_ && model_->isConstrained(); } 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? * Construct the corresponding anti-factor to negate information
*/ * stored stored in this factor.
virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } * @return a HessianFactor with negated Hessian matrices
*/
virtual GaussianFactor::shared_ptr negate() const;
/** /** Check if the factor is empty. TODO: How should this be defined? */
* return the number of rows in the corresponding linear system virtual bool empty() const {
*/ return size() == 0 /*|| rows() == 0*/;
size_t rows() const { return Ab_.rows(); } }
/** /** is noise model constrained ? */
* return the number of columns in the corresponding linear system bool isConstrained() const {
*/ return model_ && model_->isConstrained();
size_t cols() const { return Ab_.cols(); } }
/** get a copy of model */ /** Return the dimension of the variable pointed to by the given key iterator
const SharedDiagonal& get_model() const { return model_; } * 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 */ /** get a copy of model */
constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } const SharedDiagonal& get_model() const {
return model_;
}
/** Get a view of the A matrix, not weighted by noise */ /** get a copy of model (non-const version) */
constABlock getA() const { return Ab_.range(0, size()); } SharedDiagonal& get_model() {
return model_;
}
/** Get a view of the r.h.s. vector b (non-const version) */ /** Get a view of the r.h.s. vector b, not weighted by noise */
BVector getb() { 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 (non-const version) */ /** Get a view of the A matrix for the variable pointed to by the given key iterator */
ABlock getA(iterator variable) { return Ab_(variable - begin()); } constABlock getA(const_iterator variable) const {
return Ab_(variable - begin());
}
/** Get a view of the A matrix */ /** Get a view of the A matrix, not weighted by noise */
ABlock getA() { return Ab_.range(0, size()); } constABlock getA() const {
return Ab_.range(0, size());
}
/** Return A*x */ /** Get a view of the r.h.s. vector b (non-const version) */
Vector operator*(const VectorValues& x) const; 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 /** Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version) */
* zero vectors. */ ABlock getA(iterator variable) {
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; return Ab_(variable - begin());
}
/** y += alpha * A'*A*x */ /** Get a view of the A matrix */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; ABlock getA() {
return Ab_.range(0, size());
}
void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> 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 /** y += alpha * A'*A*x */
VectorValues gradientAtZero(const boost::optional<Vector&> dual = boost::none) const; void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const;
/* ************************************************************************* */ void multiplyHessianAdd(double alpha, const double* x, double* y,
virtual void gradientAtZero(double* d) const; std::vector<size_t> keys) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ void multiplyHessianAdd(double alpha, const double* x, double* y) const {
JacobianFactor whiten() const; }
;
/** Eliminate the requested variables. */ /// A'*b for Jacobian
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> > VectorValues gradientAtZero(
eliminate(const Ordering& keys); const boost::optional<Vector&> dual = boost::none) const;
/** set noiseModel correctly */ /* ************************************************************************* */
void setModel(bool anyConstrained, const Vector& sigmas); virtual void gradientAtZero(double* d) const;
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<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
/** /// Compute the gradient wrt a key at any values
* splits a pre-factorized factor into a conditional, and changes the current Vector gradient(Key key, const VectorValues& x) const;
* factor to be the remaining component. Performs same operation as eliminate(),
* but without running QR.
*/
boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
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 /** Eliminate the requested variables. */
template<typename TERMS> std::pair<boost::shared_ptr<GaussianConditional>,
void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); boost::shared_ptr<JacobianFactor> >
eliminate(const Ordering& keys);
private:
/** Serialization function */ /** set noiseModel correctly */
friend class boost::serialization::access; void setModel(bool anyConstrained, const Vector& sigmas);
template<class ARCHIVE> void setModel(const noiseModel::Diagonal::shared_ptr& model);
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 /**
* 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<GaussianConditional>,
boost::shared_ptr<JacobianFactor> >
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<GaussianConditional> 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<typename TERMS>
void fillTerms(const TERMS& terms, const Vector& b,
const SharedDiagonal& noiseModel);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
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 <gtsam/linear/JacobianFactor-inl.h> #include <gtsam/linear/JacobianFactor-inl.h>

462
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NonlinearFactor",
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor",
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor",
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor",
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor",
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor",
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::base_object<Base>(*this)); & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
} }
}; // \class NoiseModelFactor6 };
// \class NoiseModelFactor6
/* ************************************************************************* */ /* ************************************************************************* */
} // \namespace gtsam }// \namespace gtsam