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 // combine all factors
JacobianFactor combined(*this); JacobianFactor combined(*this, optionalOrdering);
return combined.augmentedJacobian(); return combined.augmentedJacobian();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const { std::pair<Matrix,Vector> GaussianFactorGraph::jacobian(boost::optional<const Ordering&> optionalOrdering) const {
Matrix augmented = augmentedJacobian(); Matrix augmented = augmentedJacobian(optionalOrdering);
return make_pair( return make_pair(
augmented.leftCols(augmented.cols()-1), augmented.leftCols(augmented.cols()-1),
augmented.col(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 // combine all factors and get upper-triangular part of Hessian
HessianFactor combined(*this); HessianFactor combined(*this, Scatter(*this, optionalOrdering));
Matrix result = combined.info(); Matrix result = combined.info();
// Fill in lower-triangular part of Hessian // Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose(); result.triangularView<Eigen::StrictlyLower>() = result.transpose();
@ -159,8 +159,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const { std::pair<Matrix,Vector> GaussianFactorGraph::hessian(boost::optional<const Ordering&> optionalOrdering) const {
Matrix augmented = augmentedHessian(); Matrix augmented = augmentedHessian(optionalOrdering);
return make_pair( return make_pair(
augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1), augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
augmented.col(augmented.rows()-1).head(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 * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$. See also
* GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian. * 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$, * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
@ -181,7 +181,7 @@ namespace gtsam {
* GaussianFactorGraph::augmentedJacobian and * GaussianFactorGraph::augmentedJacobian and
* GaussianFactorGraph::sparseJacobian. * 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 * 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 and the negative log-likelihood is
\f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$. \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 * 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 * is \frac{1}{2} x^T \Lambda x + \eta^T x + c. See also
* GaussianFactorGraph::augmentedHessian. * 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 /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using
* the dense elimination function specified in \c function (default EliminatePreferCholesky), * the dense elimination function specified in \c function (default EliminatePreferCholesky),