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

Added optional ordering for creating dense jacobian and hessian matrices from GaussianFactorGraph

release/4.3a0
Richard Roberts 2013-09-20 15:25:16 +00:00
parent 5f2a4e0dc6
commit e1ef219916
2 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -134,24 +134,24 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedJacobian() const {
Matrix GaussianFactorGraph::augmentedJacobian(boost::optional<const Ordering&> optionalOrdering) const {
// combine all factors
JacobianFactor combined(*this);
JacobianFactor combined(*this, optionalOrdering);
return combined.augmentedJacobian();
}
/* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const {
Matrix augmented = augmentedJacobian();
std::pair<Matrix,Vector> GaussianFactorGraph::jacobian(boost::optional<const Ordering&> optionalOrdering) const {
Matrix augmented = augmentedJacobian(optionalOrdering);
return make_pair(
augmented.leftCols(augmented.cols()-1),
augmented.col(augmented.cols()-1));
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedHessian() const {
Matrix GaussianFactorGraph::augmentedHessian(boost::optional<const Ordering&> optionalOrdering) const {
// combine all factors and get upper-triangular part of Hessian
HessianFactor combined(*this);
HessianFactor combined(*this, Scatter(*this, optionalOrdering));
Matrix result = combined.info();
// Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose();
@ -159,8 +159,8 @@ namespace gtsam {
}
/* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const {
Matrix augmented = augmentedHessian();
std::pair<Matrix,Vector> GaussianFactorGraph::hessian(boost::optional<const Ordering&> optionalOrdering) const {
Matrix augmented = augmentedHessian(optionalOrdering);
return make_pair(
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
augmented.col(augmented.rows()-1).head(augmented.rows()-1));

8
gtsam/linear/GaussianFactorGraph.h
View File

@ -172,7 +172,7 @@ namespace gtsam {
* \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
*/
Matrix augmentedJacobian() const;
Matrix augmentedJacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
/**
* Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@ -181,7 +181,7 @@ namespace gtsam {
* GaussianFactorGraph::augmentedJacobian and
* GaussianFactorGraph::sparseJacobian.
*/
std::pair<Matrix,Vector> jacobian() const;
std::pair<Matrix,Vector> jacobian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
/**
* Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
@ -194,7 +194,7 @@ namespace gtsam {
and the negative log-likelihood is
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
*/
Matrix augmentedHessian() const;
Matrix augmentedHessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
/**
* Return the dense Hessian \f$ \Lambda \f$ and information vector
@ -202,7 +202,7 @@ namespace gtsam {
* is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
* GaussianFactorGraph::augmentedHessian.
*/
std::pair<Matrix,Vector> hessian() const;
std::pair<Matrix,Vector> hessian(boost::optional<const Ordering&> optionalOrdering = boost::none) const;
/** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
* the dense elimination function specified in \c function (default EliminatePreferCholesky),