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

BORG Formatting

release/4.3a0
dellaert 2015-06-14 11:16:54 -07:00
parent 2c99f68ed7
commit 850501ed52
1 changed files with 149 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
gtsam/linear/HessianFactor.cpp
View File

@ -49,23 +49,25 @@
using namespace std; using namespace std;
using namespace boost::assign; using namespace boost::assign;
namespace br { using namespace boost::range; using namespace boost::adaptors; } namespace br {
using namespace boost::range;
using namespace boost::adaptors;
}
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor() : HessianFactor::HessianFactor() :
info_(cref_list_of<1>(1)) info_(cref_list_of<1>(1)) {
{
linearTerm().setZero(); linearTerm().setZero();
constantTerm() = 0.0; constantTerm() = 0.0;
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) :
GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) {
{ if (G.rows() != G.cols() || G.rows() != g.size())
if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0, 0) = G; info_(0, 0) = G;
info_(0, 1) = g; info_(0, 1) = g;
@ -76,10 +78,9 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
GaussianFactor(cref_list_of<1>(j)), GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) {
info_(cref_list_of<2> (Sigma.cols()) (1) ) if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
{ throw invalid_argument(
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
info_(0, 0) = Sigma.inverse(); // G info_(0, 0) = Sigma.inverse(); // G
info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g
@ -87,12 +88,11 @@ HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) :
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
const Matrix& G11, const Matrix& G12, const Vector& g1, const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
const Matrix& G22, const Vector& g2, double f) : double f) :
GaussianFactor(cref_list_of<2>(j1)(j2)), GaussianFactor(cref_list_of<2>(j1)(j2)), info_(
info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) cref_list_of<3>(G11.cols())(G22.cols())(1)) {
{
info_(0, 0) = G11; info_(0, 0) = G11;
info_(0, 1) = G12; info_(0, 1) = G12;
info_(0, 2) = g1; info_(0, 2) = g1;
@ -102,16 +102,18 @@ HessianFactor::HessianFactor(Key j1, Key j2,
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(Key j1, Key j2, Key j3, HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22,
const Matrix& G22, const Matrix& G23, const Vector& g2, const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
const Matrix& G33, const Vector& g3, double f) : double f) :
GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_(
info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) {
{ if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || || G11.rows() != G13.rows() || G11.rows() != g1.size()
G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) || G22.cols() != G12.cols() || G33.cols() != G13.cols()
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); || G22.cols() != g2.size() || G33.cols() != g3.size())
throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
info_(0, 0) = G11; info_(0, 0) = G11;
info_(0, 1) = G12; info_(0, 1) = G12;
info_(0, 2) = G13; info_(0, 2) = G13;
@ -126,20 +128,23 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3,
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
DenseIndex _getSizeHF(const Vector& m) { return m.size(); } DenseIndex _getSizeHF(const Vector& m) {
return m.size();
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs, HessianFactor::HessianFactor(const std::vector<Key>& js,
const std::vector<Vector>& gs, double f) : const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
{
// Get the number of variables // Get the number of variables
size_t variable_count = js.size(); size_t variable_count = js.size();
// Verify the provided number of entries in the vectors are consistent // Verify the provided number of entries in the vectors are consistent
if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) if (gs.size() != variable_count
throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ || Gs.size() != (variable_count * (variable_count + 1)) / 2)
throw invalid_argument(
"Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");
// Verify the dimensions of each provided matrix are consistent // Verify the dimensions of each provided matrix are consistent
@ -150,14 +155,16 @@ HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matri
for (size_t j = 0; j < variable_count - i; ++j) { for (size_t j = 0; j < variable_count - i; ++j) {
size_t index = i * (2 * variable_count - i + 1) / 2 + j; size_t index = i * (2 * variable_count - i + 1) / 2 + j;
if (Gs[index].rows() != block_size) { if (Gs[index].rows() != block_size) {
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
} }
} }
// Check cols // Check cols
for (size_t j = 0; j <= i; ++j) { for (size_t j = 0; j <= i; ++j) {
size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j); size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j);
if (Gs[index].cols() != block_size) { if (Gs[index].cols() != block_size) {
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); throw invalid_argument(
"Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
} }
} }
} }
@ -175,54 +182,49 @@ HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matri
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) {
{
gttic(HessianFactor_fromJacobian); gttic(HessianFactor_fromJacobian);
const SharedDiagonal& jfModel = jf.get_model(); const SharedDiagonal& jfModel = jf.get_model();
if(jfModel) if (jfModel) {
{
if (jf.get_model()->isConstrained()) if (jf.get_model()->isConstrained())
throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); throw invalid_argument(
info.full().triangularView() = jf.matrixObject().full().transpose() * "Cannot construct HessianFactor from JacobianFactor with constrained noise model");
(jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * info.full().triangularView() =
jf.matrixObject().full(); jf.matrixObject().full().transpose()
* (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal()
* jf.matrixObject().full();
} else { } else {
info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); info.full().triangularView() = jf.matrixObject().full().transpose()
* jf.matrixObject().full();
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const JacobianFactor& jf) : HessianFactor::HessianFactor(const JacobianFactor& jf) :
GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) GaussianFactor(jf), info_(
{ SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) {
_FromJacobianHelper(jf, info_); _FromJacobianHelper(jf, info_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactor& gf) : HessianFactor::HessianFactor(const GaussianFactor& gf) :
GaussianFactor(gf) GaussianFactor(gf) {
{
// Copy the matrix data depending on what type of factor we're copying from // Copy the matrix data depending on what type of factor we're copying from
if(const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) if (const JacobianFactor* jf = dynamic_cast<const JacobianFactor*>(&gf)) {
{
info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject());
_FromJacobianHelper(*jf, info_); _FromJacobianHelper(*jf, info_);
} } else if (const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf)) {
else if(const HessianFactor* hf = dynamic_cast<const HessianFactor*>(&gf))
{
info_ = hf->info_; info_ = hf->info_;
} } else {
else throw std::invalid_argument(
{ "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor");
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianFactorGraph& factors, HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
boost::optional<const Scatter&> scatter) boost::optional<const Scatter&> scatter) {
{
gttic(HessianFactor_MergeConstructor); gttic(HessianFactor_MergeConstructor);
boost::optional<Scatter> computedScatter; boost::optional<Scatter> computedScatter;
if (!scatter) { if (!scatter) {
@ -253,13 +255,15 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { void HessianFactor::print(const std::string& s,
const KeyFormatter& formatter) const {
cout << s << "\n"; cout << s << "\n";
cout << " keys: "; cout << " keys: ";
for (const_iterator key = begin(); key != end(); ++key) for (const_iterator key = begin(); key != end(); ++key)
cout << formatter(*key) << "(" << getDim(key) << ") "; cout << formatter(*key) << "(" << getDim(key) << ") ";
cout << "\n"; cout << "\n";
gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); gtsam::print(Matrix(info_.full().selfadjointView()),
"Augmented information matrix: ");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -271,21 +275,20 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
return false; return false;
Matrix thisMatrix = info_.full().selfadjointView(); Matrix thisMatrix = info_.full().selfadjointView();
thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0; thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
Matrix rhsMatrix = static_cast<const HessianFactor&>(lf).info_.full().selfadjointView(); Matrix rhsMatrix =
static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0; rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix HessianFactor::augmentedInformation() const Matrix HessianFactor::augmentedInformation() const {
{
return info_.full().selfadjointView(); return info_.full().selfadjointView();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix HessianFactor::information() const Matrix HessianFactor::information() const {
{
return info_.range(0, size(), 0, size()).selfadjointView(); return info_.range(0, size(), 0, size()).selfadjointView();
} }
@ -321,14 +324,12 @@ map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix HessianFactor::augmentedJacobian() const Matrix HessianFactor::augmentedJacobian() const {
{
return JacobianFactor(*this).augmentedJacobian(); return JacobianFactor(*this).augmentedJacobian();
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Matrix, Vector> HessianFactor::jacobian() const std::pair<Matrix, Vector> HessianFactor::jacobian() const {
{
return JacobianFactor(*this).jacobian(); return JacobianFactor(*this).jacobian();
} }
@ -363,10 +364,10 @@ void HessianFactor::updateHessian(const FastVector<Key>& infoKeys,
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::shared_ptr HessianFactor::negate() const GaussianFactor::shared_ptr HessianFactor::negate() const {
{
shared_ptr result = boost::make_shared<This>(*this); shared_ptr result = boost::make_shared<This>(*this);
result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result result->info_.full().triangularView() =
-result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result
return result; return result;
} }
@ -436,8 +437,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
if (i > j) { if (i > j) {
Matrix Gji = info(j, i); Matrix Gji = info(j, i);
Gij = Gji.transpose(); Gij = Gji.transpose();
} } else {
else {
Gij = info(i, j); Gij = info(i, j);
} }
// Accumulate Gij*xj to gradf // Accumulate Gij*xj to gradf
@ -449,15 +449,16 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> > std::pair<boost::shared_ptr<GaussianConditional>,
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) boost::shared_ptr<HessianFactor> > EliminateCholesky(
{ const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminateCholesky); gttic(EliminateCholesky);
// Build joint factor // Build joint factor
HessianFactor::shared_ptr jointFactor; HessianFactor::shared_ptr jointFactor;
try { try {
jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys)); jointFactor = boost::make_shared<HessianFactor>(factors,
Scatter(factors, keys));
} catch (std::invalid_argument&) { } catch (std::invalid_argument&) {
throw InvalidDenseElimination( throw InvalidDenseElimination(
"EliminateCholesky was called with a request to eliminate variables that are not\n" "EliminateCholesky was called with a request to eliminate variables that are not\n"
@ -468,10 +469,13 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
GaussianConditional::shared_ptr conditional; GaussianConditional::shared_ptr conditional;
try { try {
size_t numberOfKeysToEliminate = keys.size(); size_t numberOfKeysToEliminate = keys.size();
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate); VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), numberOfKeysToEliminate, Ab); numberOfKeysToEliminate);
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(),
numberOfKeysToEliminate, Ab);
// Erase the eliminated keys in the remaining factor // Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); jointFactor->keys_.erase(jointFactor->begin(),
jointFactor->begin() + numberOfKeysToEliminate);
} catch (CholeskyFailed&) { } catch (CholeskyFailed&) {
throw IndeterminantLinearSystemException(keys.front()); throw IndeterminantLinearSystemException(keys.front());
} }
@ -481,9 +485,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<GaussianFactor> > std::pair<boost::shared_ptr<GaussianConditional>,
EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) boost::shared_ptr<GaussianFactor> > EliminatePreferCholesky(
{ const GaussianFactorGraph& factors, const Ordering& keys) {
gttic(EliminatePreferCholesky); gttic(EliminatePreferCholesky);
// If any JacobianFactors have constrained noise models, we have to convert // If any JacobianFactors have constrained noise models, we have to convert