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