get_* naming convention in GaussianConditional
parent
8801de4d63
commit
ff6fe5e5e3
|
@ -140,7 +140,7 @@ int main() {
|
||||||
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
|
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
|
||||||
assert(cg0->nrFrontals() == 1);
|
assert(cg0->nrFrontals() == 1);
|
||||||
assert(cg0->nrParents() == 0);
|
assert(cg0->nrParents() == 0);
|
||||||
linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
|
linearFactorGraph->add(0, cg0->R(), cg0->d() - cg0->R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
|
||||||
|
|
||||||
// Create the desired ordering
|
// Create the desired ordering
|
||||||
ordering = Ordering::shared_ptr(new Ordering);
|
ordering = Ordering::shared_ptr(new Ordering);
|
||||||
|
|
|
@ -192,11 +192,11 @@ namespace gtsam {
|
||||||
double logDet = 0.0;
|
double logDet = 0.0;
|
||||||
for(const sharedConditional& cg: *this) {
|
for(const sharedConditional& cg: *this) {
|
||||||
if(cg->get_model()) {
|
if(cg->get_model()) {
|
||||||
Vector diag = cg->get_R().diagonal();
|
Vector diag = cg->R().diagonal();
|
||||||
cg->get_model()->whitenInPlace(diag);
|
cg->get_model()->whitenInPlace(diag);
|
||||||
logDet += diag.unaryExpr(ptr_fun<double,double>(log)).sum();
|
logDet += diag.unaryExpr(ptr_fun<double,double>(log)).sum();
|
||||||
} else {
|
} else {
|
||||||
logDet += cg->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
|
logDet += cg->R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return logDet;
|
return logDet;
|
||||||
|
|
|
@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
|
||||||
double result = 0.0;
|
double result = 0.0;
|
||||||
|
|
||||||
// this clique
|
// this clique
|
||||||
result += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
|
result += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
|
||||||
|
|
||||||
// sum of children
|
// sum of children
|
||||||
for(const typename BAYESTREE::sharedClique& child: clique->children_)
|
for(const typename BAYESTREE::sharedClique& child: clique->children_)
|
||||||
|
|
|
@ -37,7 +37,7 @@ namespace gtsam {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum)
|
double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum)
|
||||||
{
|
{
|
||||||
parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
|
parentSum += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
|
||||||
assert(false);
|
assert(false);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -62,7 +62,7 @@ namespace gtsam {
|
||||||
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
|
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
|
||||||
}
|
}
|
||||||
cout << endl;
|
cout << endl;
|
||||||
cout << formatMatrixIndented(" R = ", get_R()) << endl;
|
cout << formatMatrixIndented(" R = ", R()) << endl;
|
||||||
for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
|
for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
|
||||||
cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it))
|
cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it))
|
||||||
<< endl;
|
<< endl;
|
||||||
|
@ -84,8 +84,8 @@ namespace gtsam {
|
||||||
// check if R_ and d_ are linear independent
|
// check if R_ and d_ are linear independent
|
||||||
for (DenseIndex i = 0; i < Ab_.rows(); i++) {
|
for (DenseIndex i = 0; i < Ab_.rows(); i++) {
|
||||||
list<Vector> rows1, rows2;
|
list<Vector> rows1, rows2;
|
||||||
rows1.push_back(Vector(get_R().row(i)));
|
rows1.push_back(Vector(R().row(i)));
|
||||||
rows2.push_back(Vector(c->get_R().row(i)));
|
rows2.push_back(Vector(c->R().row(i)));
|
||||||
|
|
||||||
// check if the matrices are the same
|
// check if the matrices are the same
|
||||||
// iterate over the parents_ map
|
// iterate over the parents_ map
|
||||||
|
@ -120,10 +120,10 @@ namespace gtsam {
|
||||||
const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
|
const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
|
||||||
|
|
||||||
// Update right-hand-side
|
// Update right-hand-side
|
||||||
const Vector rhs = get_d() - get_S() * xS;
|
const Vector rhs = d() - S() * xS;
|
||||||
|
|
||||||
// Solve matrix
|
// Solve matrix
|
||||||
const Vector solution = get_R().triangularView<Eigen::Upper>().solve(rhs);
|
const Vector solution = R().triangularView<Eigen::Upper>().solve(rhs);
|
||||||
|
|
||||||
// Check for indeterminant solution
|
// Check for indeterminant solution
|
||||||
if (solution.hasNaN()) {
|
if (solution.hasNaN()) {
|
||||||
|
@ -149,10 +149,10 @@ namespace gtsam {
|
||||||
|
|
||||||
// Instead of updating getb(), update the right-hand-side from the given rhs
|
// Instead of updating getb(), update the right-hand-side from the given rhs
|
||||||
const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
|
const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
|
||||||
xS = rhsR - get_S() * xS;
|
xS = rhsR - S() * xS;
|
||||||
|
|
||||||
// Solve Matrix
|
// Solve Matrix
|
||||||
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
|
Vector soln = R().triangularView<Eigen::Upper>().solve(xS);
|
||||||
|
|
||||||
// Scale by sigmas
|
// Scale by sigmas
|
||||||
if (model_)
|
if (model_)
|
||||||
|
@ -172,7 +172,7 @@ namespace gtsam {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
|
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
|
||||||
Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
|
Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
|
||||||
frontalVec = get_R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);
|
frontalVec = R().transpose().triangularView<Eigen::Lower>().solve(frontalVec);
|
||||||
|
|
||||||
// Check for indeterminant solution
|
// Check for indeterminant solution
|
||||||
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
|
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
|
||||||
|
|
|
@ -94,16 +94,16 @@ namespace gtsam {
|
||||||
bool equals(const GaussianFactor&cg, double tol = 1e-9) const;
|
bool equals(const GaussianFactor&cg, double tol = 1e-9) const;
|
||||||
|
|
||||||
/** Return a view of the upper-triangular R block of the conditional */
|
/** Return a view of the upper-triangular R block of the conditional */
|
||||||
constABlock get_R() const { return Ab_.range(0, nrFrontals()); }
|
constABlock R() const { return Ab_.range(0, nrFrontals()); }
|
||||||
|
|
||||||
/** Get a view of the parent blocks. */
|
/** Get a view of the parent blocks. */
|
||||||
constABlock get_S() const { return Ab_.range(nrFrontals(), size()); }
|
constABlock S() const { return Ab_.range(nrFrontals(), size()); }
|
||||||
|
|
||||||
/** Get a view of the S matrix for the variable pointed to by the given key iterator */
|
/** Get a view of the S matrix for the variable pointed to by the given key iterator */
|
||||||
constABlock get_S(const_iterator variable) const { return BaseFactor::getA(variable); }
|
constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
|
||||||
|
|
||||||
/** Get a view of the r.h.s. vector d */
|
/** Get a view of the r.h.s. vector d */
|
||||||
const constBVector get_d() const { return BaseFactor::getb(); }
|
const constBVector d() const { return BaseFactor::getb(); }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Solves a conditional Gaussian and writes the solution into the entries of
|
* Solves a conditional Gaussian and writes the solution into the entries of
|
||||||
|
@ -130,8 +130,17 @@ namespace gtsam {
|
||||||
void scaleFrontalsBySigma(VectorValues& gy) const;
|
void scaleFrontalsBySigma(VectorValues& gy) const;
|
||||||
// __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; // FIXME: depreciated flag doesn't appear to exist?
|
// __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; // FIXME: depreciated flag doesn't appear to exist?
|
||||||
|
|
||||||
private:
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
|
/// @name Deprecated
|
||||||
|
/// @{
|
||||||
|
constABlock get_R() const { return R(); }
|
||||||
|
constABlock get_S() const { return S(); }
|
||||||
|
constABlock get_S(const_iterator it) const { return S(it); }
|
||||||
|
const constBVector get_d() const { return d(); }
|
||||||
|
/// @}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class Archive>
|
template<class Archive>
|
||||||
|
|
|
@ -35,8 +35,8 @@ namespace gtsam {
|
||||||
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
|
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
|
||||||
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
|
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
|
||||||
cout << endl;
|
cout << endl;
|
||||||
gtsam::print(Matrix(get_R()), "R: ");
|
gtsam::print(Matrix(R()), "R: ");
|
||||||
gtsam::print(Vector(get_d()), "d: ");
|
gtsam::print(Vector(d()), "d: ");
|
||||||
if(model_)
|
if(model_)
|
||||||
model_->print("Noise model: ");
|
model_->print("Noise model: ");
|
||||||
}
|
}
|
||||||
|
|
|
@ -213,8 +213,8 @@ void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
|
||||||
const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);
|
const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);
|
||||||
|
|
||||||
/* compute the solution for the current pivot */
|
/* compute the solution for the current pivot */
|
||||||
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(
|
const Vector solFrontal = cg->R().triangularView<Eigen::Upper>().solve(
|
||||||
rhsFrontal - cg->get_S() * xParent);
|
rhsFrontal - cg->S() * xParent);
|
||||||
|
|
||||||
/* assign subvector of sol to the frontal variables */
|
/* assign subvector of sol to the frontal variables */
|
||||||
setSubvector(solFrontal, keyInfo_, frontalKeys, x);
|
setSubvector(solFrontal, keyInfo_, frontalKeys, x);
|
||||||
|
@ -232,7 +232,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const {
|
||||||
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
|
const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals());
|
||||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
|
const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
|
||||||
const Vector solFrontal =
|
const Vector solFrontal =
|
||||||
cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
|
cg->R().transpose().triangularView<Eigen::Lower>().solve(
|
||||||
rhsFrontal);
|
rhsFrontal);
|
||||||
|
|
||||||
// Check for indeterminant solution
|
// Check for indeterminant solution
|
||||||
|
|
|
@ -87,10 +87,10 @@ namespace gtsam
|
||||||
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
|
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
|
||||||
// a temporary, and the operation trashes valus in xS.
|
// a temporary, and the operation trashes valus in xS.
|
||||||
// See: http://eigen.tuxfamily.org/index.php?title=3.3
|
// See: http://eigen.tuxfamily.org/index.php?title=3.3
|
||||||
const Vector rhs = c.getb() - c.get_S() * xS;
|
const Vector rhs = c.getb() - c.S() * xS;
|
||||||
|
|
||||||
// TODO(gareth): Inline instantiation of Eigen::Solve and check flag
|
// TODO(gareth): Inline instantiation of Eigen::Solve and check flag
|
||||||
const Vector solution = c.get_R().triangularView<Eigen::Upper>().solve(rhs);
|
const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
|
||||||
|
|
||||||
// Check for indeterminant solution
|
// Check for indeterminant solution
|
||||||
if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
|
if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());
|
||||||
|
|
|
@ -71,33 +71,33 @@ TEST(GaussianConditional, constructor)
|
||||||
|
|
||||||
GaussianConditional::const_iterator it = actual.beginFrontals();
|
GaussianConditional::const_iterator it = actual.beginFrontals();
|
||||||
EXPECT(assert_equal(Key(1), *it));
|
EXPECT(assert_equal(Key(1), *it));
|
||||||
EXPECT(assert_equal(R, actual.get_R()));
|
EXPECT(assert_equal(R, actual.R()));
|
||||||
++ it;
|
++ it;
|
||||||
EXPECT(it == actual.endFrontals());
|
EXPECT(it == actual.endFrontals());
|
||||||
|
|
||||||
it = actual.beginParents();
|
it = actual.beginParents();
|
||||||
EXPECT(assert_equal(Key(3), *it));
|
EXPECT(assert_equal(Key(3), *it));
|
||||||
EXPECT(assert_equal(S1, actual.get_S(it)));
|
EXPECT(assert_equal(S1, actual.S(it)));
|
||||||
|
|
||||||
++ it;
|
++ it;
|
||||||
EXPECT(assert_equal(Key(5), *it));
|
EXPECT(assert_equal(Key(5), *it));
|
||||||
EXPECT(assert_equal(S2, actual.get_S(it)));
|
EXPECT(assert_equal(S2, actual.S(it)));
|
||||||
|
|
||||||
++ it;
|
++ it;
|
||||||
EXPECT(assert_equal(Key(7), *it));
|
EXPECT(assert_equal(Key(7), *it));
|
||||||
EXPECT(assert_equal(S3, actual.get_S(it)));
|
EXPECT(assert_equal(S3, actual.S(it)));
|
||||||
|
|
||||||
++it;
|
++it;
|
||||||
EXPECT(it == actual.endParents());
|
EXPECT(it == actual.endParents());
|
||||||
|
|
||||||
EXPECT(assert_equal(d, actual.get_d()));
|
EXPECT(assert_equal(d, actual.d()));
|
||||||
EXPECT(assert_equal(*s, *actual.get_model()));
|
EXPECT(assert_equal(*s, *actual.get_model()));
|
||||||
|
|
||||||
// test copy constructor
|
// test copy constructor
|
||||||
GaussianConditional copied(actual);
|
GaussianConditional copied(actual);
|
||||||
EXPECT(assert_equal(d, copied.get_d()));
|
EXPECT(assert_equal(d, copied.d()));
|
||||||
EXPECT(assert_equal(*s, *copied.get_model()));
|
EXPECT(assert_equal(*s, *copied.get_model()));
|
||||||
EXPECT(assert_equal(R, copied.get_R()));
|
EXPECT(assert_equal(R, copied.R()));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -212,7 +212,7 @@ TEST( GaussianConditional, solve_multifrontal )
|
||||||
// 3 variables, all dim=2
|
// 3 variables, all dim=2
|
||||||
GaussianConditional cg(list_of(1)(2)(10), 2, blockMatrix);
|
GaussianConditional cg(list_of(1)(2)(10), 2, blockMatrix);
|
||||||
|
|
||||||
EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d()));
|
EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.d()));
|
||||||
|
|
||||||
// partial solution
|
// partial solution
|
||||||
Vector sl1 = Vector2(9.0, 10.0);
|
Vector sl1 = Vector2(9.0, 10.0);
|
||||||
|
|
|
@ -33,7 +33,7 @@ TEST(GaussianDensity, constructor)
|
||||||
GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
|
GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
|
||||||
|
|
||||||
GaussianDensity copied(conditional);
|
GaussianDensity copied(conditional);
|
||||||
EXPECT(assert_equal(d, copied.get_d()));
|
EXPECT(assert_equal(d, copied.d()));
|
||||||
EXPECT(assert_equal(s, copied.get_model()->sigmas()));
|
EXPECT(assert_equal(s, copied.get_model()->sigmas()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -246,8 +246,8 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
|
||||||
clique->conditional()->endParents()));
|
clique->conditional()->endParents()));
|
||||||
|
|
||||||
// Compute R*g and S*g for this clique
|
// Compute R*g and S*g for this clique
|
||||||
Vector RSgProd = clique->conditional()->get_R() * gR +
|
Vector RSgProd = clique->conditional()->R() * gR +
|
||||||
clique->conditional()->get_S() * gS;
|
clique->conditional()->S() * gS;
|
||||||
|
|
||||||
// Write into RgProd vector
|
// Write into RgProd vector
|
||||||
DenseIndex vectorPosition = 0;
|
DenseIndex vectorPosition = 0;
|
||||||
|
|
|
@ -37,9 +37,9 @@ void ISAM2Clique::setEliminationResult(
|
||||||
gradientContribution_.resize(conditional_->cols() - 1);
|
gradientContribution_.resize(conditional_->cols() - 1);
|
||||||
// Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed
|
// Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed
|
||||||
// reasons
|
// reasons
|
||||||
gradientContribution_ << -conditional_->get_R().transpose() *
|
gradientContribution_ << -conditional_->R().transpose() *
|
||||||
conditional_->get_d(),
|
conditional_->d(),
|
||||||
-conditional_->get_S().transpose() * conditional_->get_d();
|
-conditional_->S().transpose() * conditional_->d();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -141,8 +141,8 @@ void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const {
|
||||||
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
|
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
|
||||||
// a temporary, and the operation trashes valus in xS.
|
// a temporary, and the operation trashes valus in xS.
|
||||||
// See: http://eigen.tuxfamily.org/index.php?title=3.3
|
// See: http://eigen.tuxfamily.org/index.php?title=3.3
|
||||||
const Vector rhs = c.getb() - c.get_S() * xS;
|
const Vector rhs = c.getb() - c.S() * xS;
|
||||||
const Vector solution = c.get_R().triangularView<Eigen::Upper>().solve(rhs);
|
const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
|
||||||
|
|
||||||
// Check for indeterminant solution
|
// Check for indeterminant solution
|
||||||
if (solution.hasNaN())
|
if (solution.hasNaN())
|
||||||
|
@ -284,7 +284,7 @@ size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void ISAM2Clique::nnz_internal(size_t* result) const {
|
void ISAM2Clique::nnz_internal(size_t* result) const {
|
||||||
size_t dimR = conditional_->rows();
|
size_t dimR = conditional_->rows();
|
||||||
size_t dimSep = conditional_->get_S().cols();
|
size_t dimSep = conditional_->S().cols();
|
||||||
*result += ((dimR + 1) * dimR) / 2 + dimSep * dimR;
|
*result += ((dimR + 1) * dimR) / 2 + dimSep * dimR;
|
||||||
// traverse the children
|
// traverse the children
|
||||||
for (const auto& child : children) {
|
for (const auto& child : children) {
|
||||||
|
|
Loading…
Reference in New Issue