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

get_* naming convention in GaussianConditional

release/4.3a0
Frank Dellaert 2019-05-16 18:59:24 -04:00
parent 8801de4d63
commit ff6fe5e5e3
13 changed files with 51 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -140,7 +140,7 @@ int main() {
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
assert(cg0->nrFrontals() == 1);
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
ordering = Ordering::shared_ptr(new Ordering);

4
gtsam/linear/GaussianBayesNet.cpp
View File

@ -192,11 +192,11 @@ namespace gtsam {
double logDet = 0.0;
for(const sharedConditional& cg: *this) {
if(cg->get_model()) {
Vector diag = cg->get_R().diagonal();
Vector diag = cg->R().diagonal();
cg->get_model()->whitenInPlace(diag);
logDet += diag.unaryExpr(ptr_fun<double,double>(log)).sum();
} 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;

2
gtsam/linear/GaussianBayesTree-inl.h
View File

@ -43,7 +43,7 @@ double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
double result = 0.0;
// 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
for(const typename BAYESTREE::sharedClique& child: clique->children_)

2
gtsam/linear/GaussianBayesTree.cpp
View File

@ -37,7 +37,7 @@ namespace gtsam {
/* ************************************************************************* */
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);
return 0;
}

16
gtsam/linear/GaussianConditional.cpp
View File

@ -62,7 +62,7 @@ namespace gtsam {
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
}
cout << endl;
cout << formatMatrixIndented(" R = ", get_R()) << endl;
cout << formatMatrixIndented(" R = ", R()) << endl;
for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it))
<< endl;
@ -84,8 +84,8 @@ namespace gtsam {
// check if R_ and d_ are linear independent
for (DenseIndex i = 0; i < Ab_.rows(); i++) {
list<Vector> rows1, rows2;
rows1.push_back(Vector(get_R().row(i)));
rows2.push_back(Vector(c->get_R().row(i)));
rows1.push_back(Vector(R().row(i)));
rows2.push_back(Vector(c->R().row(i)));
// check if the matrices are the same
// iterate over the parents_ map
@ -120,10 +120,10 @@ namespace gtsam {
const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
// Update right-hand-side
const Vector rhs = get_d() - get_S() * xS;
const Vector rhs = d() - S() * xS;
// 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
if (solution.hasNaN()) {
@ -149,10 +149,10 @@ namespace gtsam {
// Instead of updating getb(), update the right-hand-side from the given rhs
const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
xS = rhsR - get_S() * xS;
xS = rhsR - S() * xS;
// Solve Matrix
Vector soln = get_R().triangularView<Eigen::Upper>().solve(xS);
Vector soln = R().triangularView<Eigen::Upper>().solve(xS);
// Scale by sigmas
if (model_)
@ -172,7 +172,7 @@ namespace gtsam {
/* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
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
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());

19
gtsam/linear/GaussianConditional.h
View File

@ -94,16 +94,16 @@ namespace gtsam {
bool equals(const GaussianFactor&cg, double tol = 1e-9) const;
/** 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. */
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 */
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 */
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
@ -130,8 +130,17 @@ namespace gtsam {
void scaleFrontalsBySigma(VectorValues& gy) const;
// __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 */
friend class boost::serialization::access;
template<class Archive>

4
gtsam/linear/GaussianDensity.cpp
View File

@ -35,8 +35,8 @@ namespace gtsam {
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it)
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
cout << endl;
gtsam::print(Matrix(get_R()), "R: ");
gtsam::print(Vector(get_d()), "d: ");
gtsam::print(Matrix(R()), "R: ");
gtsam::print(Vector(d()), "d: ");
if(model_)
model_->print("Noise model: ");
}

6
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -213,8 +213,8 @@ void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const {
const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys);
/* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(
rhsFrontal - cg->get_S() * xParent);
const Vector solFrontal = cg->R().triangularView<Eigen::Upper>().solve(
rhsFrontal - cg->S() * xParent);
/* assign subvector of sol to the frontal variables */
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 Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys);
const Vector solFrontal =
cg->get_R().transpose().triangularView<Eigen::Lower>().solve(
cg->R().transpose().triangularView<Eigen::Lower>().solve(
rhsFrontal);
// Check for indeterminant solution

4
gtsam/linear/linearAlgorithms-inst.h
View File

@ -87,10 +87,10 @@ namespace gtsam
// This is because Eigen (as of 3.3) no longer evaluates S * xS into
// a temporary, and the operation trashes valus in xS.
// 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
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
if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front());

16
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -71,33 +71,33 @@ TEST(GaussianConditional, constructor)
GaussianConditional::const_iterator it = actual.beginFrontals();
EXPECT(assert_equal(Key(1), *it));
EXPECT(assert_equal(R, actual.get_R()));
EXPECT(assert_equal(R, actual.R()));
++ it;
EXPECT(it == actual.endFrontals());
it = actual.beginParents();
EXPECT(assert_equal(Key(3), *it));
EXPECT(assert_equal(S1, actual.get_S(it)));
EXPECT(assert_equal(S1, actual.S(it)));
++ it;
EXPECT(assert_equal(Key(5), *it));
EXPECT(assert_equal(S2, actual.get_S(it)));
EXPECT(assert_equal(S2, actual.S(it)));
++ it;
EXPECT(assert_equal(Key(7), *it));
EXPECT(assert_equal(S3, actual.get_S(it)));
EXPECT(assert_equal(S3, actual.S(it)));
++it;
EXPECT(it == actual.endParents());
EXPECT(assert_equal(d, actual.get_d()));
EXPECT(assert_equal(d, actual.d()));
EXPECT(assert_equal(*s, *actual.get_model()));
// test copy constructor
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(R, copied.get_R()));
EXPECT(assert_equal(R, copied.R()));
}
/* ************************************************************************* */
@ -212,7 +212,7 @@ TEST( GaussianConditional, solve_multifrontal )
// 3 variables, all dim=2
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
Vector sl1 = Vector2(9.0, 10.0);

2
gtsam/linear/tests/testGaussianDensity.cpp
View File

@ -33,7 +33,7 @@ TEST(GaussianDensity, constructor)
GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s));
GaussianDensity copied(conditional);
EXPECT(assert_equal(d, copied.get_d()));
EXPECT(assert_equal(d, copied.d()));
EXPECT(assert_equal(s, copied.get_model()->sigmas()));
}

4
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -246,8 +246,8 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
clique->conditional()->endParents()));
// Compute R*g and S*g for this clique
Vector RSgProd = clique->conditional()->get_R() * gR +
clique->conditional()->get_S() * gS;
Vector RSgProd = clique->conditional()->R() * gR +
clique->conditional()->S() * gS;
// Write into RgProd vector
DenseIndex vectorPosition = 0;

12
gtsam/nonlinear/ISAM2Clique.cpp
View File

@ -37,9 +37,9 @@ void ISAM2Clique::setEliminationResult(
gradientContribution_.resize(conditional_->cols() - 1);
// Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed
// reasons
gradientContribution_ << -conditional_->get_R().transpose() *
conditional_->get_d(),
-conditional_->get_S().transpose() * conditional_->get_d();
gradientContribution_ << -conditional_->R().transpose() *
conditional_->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
// a temporary, and the operation trashes valus in xS.
// See: http://eigen.tuxfamily.org/index.php?title=3.3
const Vector rhs = c.getb() - c.get_S() * xS;
const Vector solution = c.get_R().triangularView<Eigen::Upper>().solve(rhs);
const Vector rhs = c.getb() - c.S() * xS;
const Vector solution = c.R().triangularView<Eigen::Upper>().solve(rhs);
// Check for indeterminant solution
if (solution.hasNaN())
@ -284,7 +284,7 @@ size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
/* ************************************************************************* */
void ISAM2Clique::nnz_internal(size_t* result) const {
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;
// traverse the children
for (const auto& child : children) {