Some low hanging fruit changing `func(..., boost::optional<const Class &>)` to overloaded
`func(...)` and `func(..., const Class &)` not all done Also, although it currently passes all tests, I think some more tests may be needed...release/4.3a0
parent
6ef7b48d68
commit
5a358489dc
|
@ -71,12 +71,8 @@ bool assert_equal(const V& expected, const boost::optional<V>& actual, double to
|
|||
}
|
||||
|
||||
template<class V>
|
||||
bool assert_equal(const V& expected, const boost::optional<const V&>& actual, double tol = 1e-9) {
|
||||
if (!actual) {
|
||||
std::cout << "actual is boost::none" << std::endl;
|
||||
bool assert_equal(const V& expected, double tol = 1e-9) {
|
||||
return false;
|
||||
}
|
||||
return assert_equal(expected, *actual, tol);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -89,10 +89,10 @@ namespace gtsam {
|
|||
typedef boost::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
|
||||
|
||||
/// Typedef for an optional ordering as an argument to elimination functions
|
||||
typedef boost::optional<const Ordering&> OptionalOrdering;
|
||||
typedef const boost::optional<Ordering>& OptionalOrdering;
|
||||
|
||||
/// Typedef for an optional variable index as an argument to elimination functions
|
||||
typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
|
||||
typedef const boost::optional<VariableIndex>& OptionalVariableIndex;
|
||||
|
||||
/// Typedef for an optional ordering type
|
||||
typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
|
||||
|
|
|
@ -156,17 +156,18 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> GaussianBayesNet::matrix(boost::optional<const Ordering&> ordering) const {
|
||||
if (ordering) {
|
||||
pair<Matrix, Vector> GaussianBayesNet::matrix(const Ordering& ordering) const {
|
||||
// Convert to a GaussianFactorGraph and use its machinery
|
||||
GaussianFactorGraph factorGraph(*this);
|
||||
return factorGraph.jacobian(ordering);
|
||||
} else {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> GaussianBayesNet::matrix() const {
|
||||
// recursively call with default ordering
|
||||
const auto defaultOrdering = this->ordering();
|
||||
return matrix(defaultOrdering);
|
||||
}
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//VectorValues GaussianBayesNet::gradient(const VectorValues& x0) const
|
||||
|
|
|
@ -92,7 +92,14 @@ namespace gtsam {
|
|||
* Will return upper-triangular matrix only when using 'ordering' above.
|
||||
* In case Bayes net is incomplete zero columns are added to the end.
|
||||
*/
|
||||
std::pair<Matrix, Vector> matrix(boost::optional<const Ordering&> ordering = boost::none) const;
|
||||
std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Return (dense) upper-triangular matrix representation
|
||||
* Will return upper-triangular matrix only when using 'ordering' above.
|
||||
* In case Bayes net is incomplete zero columns are added to the end.
|
||||
*/
|
||||
std::pair<Matrix, Vector> matrix() const;
|
||||
|
||||
/**
|
||||
* Optimize along the gradient direction, with a closed-form computation to perform the line
|
||||
|
|
|
@ -190,33 +190,62 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraph::augmentedJacobian(
|
||||
boost::optional<const Ordering&> optionalOrdering) const {
|
||||
const Ordering& ordering) const {
|
||||
// combine all factors
|
||||
JacobianFactor combined(*this, optionalOrdering);
|
||||
JacobianFactor combined(*this, ordering);
|
||||
return combined.augmentedJacobian();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraph::augmentedJacobian() const {
|
||||
// combine all factors
|
||||
JacobianFactor combined(*this);
|
||||
return combined.augmentedJacobian();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> GaussianFactorGraph::jacobian(
|
||||
boost::optional<const Ordering&> optionalOrdering) const {
|
||||
Matrix augmented = augmentedJacobian(optionalOrdering);
|
||||
const Ordering& ordering) const {
|
||||
Matrix augmented = augmentedJacobian(ordering);
|
||||
return make_pair(augmented.leftCols(augmented.cols() - 1),
|
||||
augmented.col(augmented.cols() - 1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> GaussianFactorGraph::jacobian() const {
|
||||
Matrix augmented = augmentedJacobian();
|
||||
return make_pair(augmented.leftCols(augmented.cols() - 1),
|
||||
augmented.col(augmented.cols() - 1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraph::augmentedHessian(
|
||||
boost::optional<const Ordering&> optionalOrdering) const {
|
||||
const Ordering& ordering) const {
|
||||
// combine all factors and get upper-triangular part of Hessian
|
||||
Scatter scatter(*this, optionalOrdering);
|
||||
Scatter scatter(*this, ordering);
|
||||
HessianFactor combined(*this, scatter);
|
||||
return combined.info().selfadjointView();;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianFactorGraph::augmentedHessian() const {
|
||||
// combine all factors and get upper-triangular part of Hessian
|
||||
Scatter scatter(*this);
|
||||
HessianFactor combined(*this, scatter);
|
||||
return combined.info().selfadjointView();;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> GaussianFactorGraph::hessian(
|
||||
boost::optional<const Ordering&> optionalOrdering) const {
|
||||
Matrix augmented = augmentedHessian(optionalOrdering);
|
||||
const Ordering& ordering) const {
|
||||
Matrix augmented = augmentedHessian(ordering);
|
||||
size_t n = augmented.rows() - 1;
|
||||
return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> GaussianFactorGraph::hessian() const {
|
||||
Matrix augmented = augmentedHessian();
|
||||
size_t n = augmented.rows() - 1;
|
||||
return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
|
||||
}
|
||||
|
|
|
@ -201,7 +201,16 @@ namespace gtsam {
|
|||
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
|
||||
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
|
||||
*/
|
||||
Matrix augmentedJacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
|
||||
Matrix augmentedJacobian(const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
|
||||
* Jacobian matrix, augmented with b with the noise models baked
|
||||
* into A and b. The negative log-likelihood is
|
||||
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
|
||||
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
|
||||
*/
|
||||
Matrix augmentedJacobian() const;
|
||||
|
||||
/**
|
||||
* Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
|
||||
|
@ -210,7 +219,16 @@ namespace gtsam {
|
|||
* GaussianFactorGraph::augmentedJacobian and
|
||||
* GaussianFactorGraph::sparseJacobian.
|
||||
*/
|
||||
std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
|
||||
std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
|
||||
* with the noise models baked into A and b. The negative log-likelihood
|
||||
* is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
|
||||
* GaussianFactorGraph::augmentedJacobian and
|
||||
* GaussianFactorGraph::sparseJacobian.
|
||||
*/
|
||||
std::pair<Matrix,Vector> jacobian() const;
|
||||
|
||||
/**
|
||||
* Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
|
||||
|
@ -223,7 +241,20 @@ namespace gtsam {
|
|||
and the negative log-likelihood is
|
||||
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
|
||||
*/
|
||||
Matrix augmentedHessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
|
||||
Matrix augmentedHessian(const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
|
||||
* matrix, augmented with the information vector \f$ \eta \f$. The
|
||||
* augmented Hessian is
|
||||
\f[ \left[ \begin{array}{ccc}
|
||||
\Lambda & \eta \\
|
||||
\eta^T & c
|
||||
\end{array} \right] \f]
|
||||
and the negative log-likelihood is
|
||||
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
|
||||
*/
|
||||
Matrix augmentedHessian() const;
|
||||
|
||||
/**
|
||||
* Return the dense Hessian \f$ \Lambda \f$ and information vector
|
||||
|
@ -231,7 +262,15 @@ namespace gtsam {
|
|||
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
|
||||
* GaussianFactorGraph::augmentedHessian.
|
||||
*/
|
||||
std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
|
||||
std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
|
||||
|
||||
/**
|
||||
* Return the dense Hessian \f$ \Lambda \f$ and information vector
|
||||
* \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
|
||||
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
|
||||
* GaussianFactorGraph::augmentedHessian.
|
||||
*/
|
||||
std::pair<Matrix,Vector> hessian() const;
|
||||
|
||||
/** Return only the diagonal of the Hessian A'*A, as a VectorValues */
|
||||
virtual VectorValues hessianDiagonal() const;
|
||||
|
|
|
@ -250,10 +250,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) :
|
|||
|
||||
/* ************************************************************************* */
|
||||
HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
|
||||
boost::optional<const Scatter&> scatter) {
|
||||
const Scatter& scatter) {
|
||||
gttic(HessianFactor_MergeConstructor);
|
||||
|
||||
Allocate(scatter ? *scatter : Scatter(factors));
|
||||
Allocate(scatter);
|
||||
|
||||
// Form A' * A
|
||||
gttic(update);
|
||||
|
|
|
@ -176,7 +176,11 @@ namespace gtsam {
|
|||
|
||||
/** Combine a set of factors into a single dense HessianFactor */
|
||||
explicit HessianFactor(const GaussianFactorGraph& factors,
|
||||
boost::optional<const Scatter&> scatter = boost::none);
|
||||
const Scatter& scatter);
|
||||
|
||||
/** Combine a set of factors into a single dense HessianFactor */
|
||||
explicit HessianFactor(const GaussianFactorGraph& factors)
|
||||
: HessianFactor(factors, Scatter(factors)) {}
|
||||
|
||||
/** Destructor */
|
||||
virtual ~HessianFactor() {}
|
||||
|
|
|
@ -84,16 +84,25 @@ string IterativeOptimizationParameters::verbosityTranslator(
|
|||
|
||||
/*****************************************************************************/
|
||||
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
|
||||
boost::optional<const KeyInfo&> keyInfo,
|
||||
boost::optional<const std::map<Key, Vector>&> lambda) {
|
||||
return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
|
||||
lambda ? *lambda : std::map<Key, Vector>());
|
||||
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
|
||||
return optimize(gfg, keyInfo, lambda, keyInfo.x0());
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
|
||||
return optimize(gfg, keyInfo, lambda, keyInfo.x0());
|
||||
const KeyInfo& keyInfo) {
|
||||
return optimize(gfg, keyInfo, std::map<Key, Vector>());
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
|
||||
const std::map<Key, Vector>& lambda) {
|
||||
return optimize(gfg, KeyInfo(gfg), lambda);
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg) {
|
||||
return optimize(gfg, KeyInfo(gfg), std::map<Key, Vector>());
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
|
|
|
@ -91,15 +91,21 @@ public:
|
|||
virtual ~IterativeSolver() {
|
||||
}
|
||||
|
||||
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
|
||||
boost::optional<const KeyInfo&> = boost::none,
|
||||
boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
|
||||
|
||||
/* interface to the nonlinear optimizer, without initial estimate */
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
|
||||
const std::map<Key, Vector> &lambda);
|
||||
|
||||
/* interface to the nonlinear optimizer, without damping and initial estimate */
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
|
||||
const KeyInfo& keyInfo);
|
||||
|
||||
/* interface to the nonlinear optimizer, without metadata and initial estimate */
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
|
||||
const std::map<Key, Vector>& lambda);
|
||||
|
||||
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
|
||||
GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg);
|
||||
|
||||
/* interface to the nonlinear optimizer that the subclasses have to implement */
|
||||
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
|
||||
|
|
|
@ -220,60 +220,13 @@ FastVector<JacobianFactor::shared_ptr> _convertOrCastToJacobians(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
|
||||
boost::optional<const Ordering&> ordering,
|
||||
boost::optional<const VariableSlots&> p_variableSlots) {
|
||||
gttic(JacobianFactor_combine_constructor);
|
||||
|
||||
// Compute VariableSlots if one was not provided
|
||||
// Binds reference, does not copy VariableSlots
|
||||
const VariableSlots & variableSlots =
|
||||
p_variableSlots ? p_variableSlots.get() : VariableSlots(graph);
|
||||
void JacobianFactor::JacobianFactorHelper(const GaussianFactorGraph& graph,
|
||||
const FastVector<VariableSlots::const_iterator>& orderedSlots) {
|
||||
|
||||
// Cast or convert to Jacobians
|
||||
FastVector<JacobianFactor::shared_ptr> jacobians = _convertOrCastToJacobians(
|
||||
graph);
|
||||
|
||||
gttic(Order_slots);
|
||||
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
|
||||
// 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
|
||||
// be added after all of the ordered variables.
|
||||
FastVector<VariableSlots::const_iterator> orderedSlots;
|
||||
orderedSlots.reserve(variableSlots.size());
|
||||
if (ordering) {
|
||||
// If an ordering is provided, arrange the slots first that ordering
|
||||
FastList<VariableSlots::const_iterator> unorderedSlots;
|
||||
size_t nOrderingSlotsUsed = 0;
|
||||
orderedSlots.resize(ordering->size());
|
||||
FastMap<Key, size_t> inverseOrdering = ordering->invert();
|
||||
for (VariableSlots::const_iterator item = variableSlots.begin();
|
||||
item != variableSlots.end(); ++item) {
|
||||
FastMap<Key, size_t>::const_iterator orderingPosition =
|
||||
inverseOrdering.find(item->first);
|
||||
if (orderingPosition == inverseOrdering.end()) {
|
||||
unorderedSlots.push_back(item);
|
||||
} else {
|
||||
orderedSlots[orderingPosition->second] = item;
|
||||
++nOrderingSlotsUsed;
|
||||
}
|
||||
}
|
||||
if (nOrderingSlotsUsed != ordering->size())
|
||||
throw std::invalid_argument(
|
||||
"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
|
||||
for(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
|
||||
FastVector<DenseIndex> varDims;
|
||||
DenseIndex m, n;
|
||||
|
@ -344,6 +297,127 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
|
|||
this->setModel(anyConstrained, *sigmas);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
|
||||
// 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
|
||||
// be added after all of the ordered variables.
|
||||
FastVector<VariableSlots::const_iterator> orderedSlotsHelper(
|
||||
const Ordering& ordering,
|
||||
const VariableSlots& variableSlots) {
|
||||
gttic(Order_slots);
|
||||
|
||||
FastVector<VariableSlots::const_iterator> orderedSlots;
|
||||
orderedSlots.reserve(variableSlots.size());
|
||||
|
||||
// If an ordering is provided, arrange the slots first that ordering
|
||||
FastList<VariableSlots::const_iterator> unorderedSlots;
|
||||
size_t nOrderingSlotsUsed = 0;
|
||||
orderedSlots.resize(ordering.size());
|
||||
FastMap<Key, size_t> inverseOrdering = ordering.invert();
|
||||
for (VariableSlots::const_iterator item = variableSlots.begin();
|
||||
item != variableSlots.end(); ++item) {
|
||||
FastMap<Key, size_t>::const_iterator orderingPosition =
|
||||
inverseOrdering.find(item->first);
|
||||
if (orderingPosition == inverseOrdering.end()) {
|
||||
unorderedSlots.push_back(item);
|
||||
} else {
|
||||
orderedSlots[orderingPosition->second] = item;
|
||||
++nOrderingSlotsUsed;
|
||||
}
|
||||
}
|
||||
if (nOrderingSlotsUsed != ordering.size())
|
||||
throw std::invalid_argument(
|
||||
"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
|
||||
for(VariableSlots::const_iterator item: unorderedSlots) {
|
||||
orderedSlots.push_back(item);
|
||||
}
|
||||
|
||||
gttoc(Order_slots);
|
||||
|
||||
return orderedSlots;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph) {
|
||||
gttic(JacobianFactor_combine_constructor);
|
||||
|
||||
// Compute VariableSlots if one was not provided
|
||||
// Binds reference, does not copy VariableSlots
|
||||
const VariableSlots & variableSlots = VariableSlots(graph);
|
||||
|
||||
gttic(Order_slots);
|
||||
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
|
||||
// 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
|
||||
// be added after all of the ordered variables.
|
||||
FastVector<VariableSlots::const_iterator> orderedSlots;
|
||||
orderedSlots.reserve(variableSlots.size());
|
||||
|
||||
// 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);
|
||||
|
||||
JacobianFactorHelper(graph, orderedSlots);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
|
||||
const VariableSlots& p_variableSlots) {
|
||||
gttic(JacobianFactor_combine_constructor);
|
||||
|
||||
// Binds reference, does not copy VariableSlots
|
||||
const VariableSlots & variableSlots = p_variableSlots;
|
||||
|
||||
gttic(Order_slots);
|
||||
// Order variable slots - we maintain the vector of ordered slots, as well as keep a list
|
||||
// 'unorderedSlots' of any variables discovered that are not in the ordering. Those will then
|
||||
// be added after all of the ordered variables.
|
||||
FastVector<VariableSlots::const_iterator> orderedSlots;
|
||||
orderedSlots.reserve(variableSlots.size());
|
||||
|
||||
// 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);
|
||||
|
||||
JacobianFactorHelper(graph, orderedSlots);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
|
||||
const Ordering& ordering) {
|
||||
gttic(JacobianFactor_combine_constructor);
|
||||
|
||||
// Compute VariableSlots if one was not provided
|
||||
// Binds reference, does not copy VariableSlots
|
||||
const VariableSlots & variableSlots = VariableSlots(graph);
|
||||
|
||||
// Order variable slots
|
||||
FastVector<VariableSlots::const_iterator> orderedSlots =
|
||||
orderedSlotsHelper(ordering, variableSlots);
|
||||
|
||||
JacobianFactorHelper(graph, orderedSlots);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
|
||||
const Ordering& ordering,
|
||||
const VariableSlots& p_variableSlots) {
|
||||
gttic(JacobianFactor_combine_constructor);
|
||||
|
||||
// Order variable slots
|
||||
FastVector<VariableSlots::const_iterator> orderedSlots =
|
||||
orderedSlotsHelper(ordering, p_variableSlots);
|
||||
|
||||
JacobianFactorHelper(graph, orderedSlots);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactor::print(const string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/base/VerticalBlockMatrix.h>
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/inference/VariableSlots.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
|
@ -147,14 +148,37 @@ namespace gtsam {
|
|||
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&> p_variableSlots = boost::none);
|
||||
const VariableSlots& p_variableSlots);
|
||||
|
||||
/**
|
||||
* Build a dense joint factor from all the factors in a factor graph. If a VariableSlots
|
||||
* structure computed for \c graph is already available, providing it will reduce the amount of
|
||||
* computation performed. */
|
||||
explicit JacobianFactor(
|
||||
const GaussianFactorGraph& graph,
|
||||
const Ordering& ordering);
|
||||
|
||||
/**
|
||||
* 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,
|
||||
const Ordering& ordering,
|
||||
const VariableSlots& p_variableSlots);
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~JacobianFactor() {}
|
||||
|
@ -356,6 +380,14 @@ namespace gtsam {
|
|||
|
||||
private:
|
||||
|
||||
/**
|
||||
* Helper function for public constructors:
|
||||
* Build a dense joint factor from all the factors in a factor graph. Takes in
|
||||
* ordered variable slots */
|
||||
void JacobianFactorHelper(
|
||||
const GaussianFactorGraph& graph,
|
||||
const FastVector<VariableSlots::const_iterator>& orderedSlots);
|
||||
|
||||
/** Unsafe Constructor that creates an uninitialized Jacobian of right size
|
||||
* @param keys in some order
|
||||
* @param diemnsions of the variables in same order
|
||||
|
|
|
@ -77,11 +77,17 @@ public:
|
|||
|
||||
/// Construct from a GaussianFactorGraph
|
||||
RegularHessianFactor(const GaussianFactorGraph& factors,
|
||||
boost::optional<const Scatter&> scatter = boost::none)
|
||||
const Scatter& scatter)
|
||||
: HessianFactor(factors, scatter) {
|
||||
checkInvariants();
|
||||
}
|
||||
|
||||
/// Construct from a GaussianFactorGraph
|
||||
RegularHessianFactor(const GaussianFactorGraph& factors)
|
||||
: HessianFactor(factors) {
|
||||
checkInvariants();
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Check invariants after construction
|
||||
|
|
|
@ -128,17 +128,17 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
|
|||
const NonlinearOptimizerParams& params) const {
|
||||
// solution of linear solver is an update to the linearization point
|
||||
VectorValues delta;
|
||||
boost::optional<const Ordering&> optionalOrdering;
|
||||
Ordering ordering;
|
||||
if (params.ordering)
|
||||
optionalOrdering.reset(*params.ordering);
|
||||
ordering = *params.ordering;
|
||||
|
||||
// Check which solver we are using
|
||||
if (params.isMultifrontal()) {
|
||||
// Multifrontal QR or Cholesky (decided by params.getEliminationFunction())
|
||||
delta = gfg.optimize(optionalOrdering, params.getEliminationFunction());
|
||||
delta = gfg.optimize(ordering, params.getEliminationFunction());
|
||||
} else if (params.isSequential()) {
|
||||
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
|
||||
delta = gfg.eliminateSequential(optionalOrdering, params.getEliminationFunction(), boost::none,
|
||||
delta = gfg.eliminateSequential(ordering, params.getEliminationFunction(), boost::none,
|
||||
params.orderingType)->optimize();
|
||||
} else if (params.isIterative()) {
|
||||
// Conjugate Gradient -> needs params.iterativeParams
|
||||
|
|
Loading…
Reference in New Issue