diff --git a/linear/GaussianConditional.cpp b/linear/GaussianConditional.cpp index 2dc774678..cbff35bf2 100644 --- a/linear/GaussianConditional.cpp +++ b/linear/GaussianConditional.cpp @@ -139,36 +139,7 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const return true; } -///* ************************************************************************* */ -//void GaussianConditional::permuteWithInverse(const Permutation& inversePermutation) { -// Conditional::permuteWithInverse(inversePermutation); -// BOOST_FOREACH(Parents::value_type& parent, parents_) { -// parent.first = inversePermutation[parent.first]; -// } -//#ifndef NDEBUG -// const_iterator parent = parents_.begin(); -// Conditional::const_iterator baseParent = Conditional::parents_.begin(); -// while(parent != parents_.end()) -// assert((parent++)->first == *(baseParent++)); -// assert(baseParent == Conditional::parents_.end()); -//#endif -//} -// -///* ************************************************************************* */ -//bool GaussianConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { -// bool separatorChanged = Conditional::permuteSeparatorWithInverse(inversePermutation); -// BOOST_FOREACH(Parents::value_type& parent, parents_) { -// parent.first = inversePermutation[parent.first]; -// } -//#ifndef NDEBUG -// const_iterator parent = parents_.begin(); -// Conditional::const_iterator baseParent = Conditional::parents_.begin(); -// while(parent != parents_.end()) -// assert((parent++)->first == *(baseParent++)); -// assert(baseParent == Conditional::parents_.end()); -//#endif -// return separatorChanged; -//} + /* ************************************************************************* */ Vector GaussianConditional::solve(const VectorValues& x) const { @@ -177,7 +148,7 @@ Vector GaussianConditional::solve(const VectorValues& x) const { Vector rhs(get_d()); for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false); -// multiplyAdd(-1.0, get_S(parent), x[*parent], rhs); + } if(debug) gtsam::print(get_R(), "Calling backSubstituteUpper on "); if(debug) gtsam::print(rhs, "rhs: "); @@ -194,7 +165,7 @@ Vector GaussianConditional::solve(const Permuted& x) const { Vector rhs(get_d()); for (const_iterator parent = beginParents(); parent != endParents(); ++parent) { ublas::axpy_prod(-get_S(parent), x[*parent], rhs, false); -// multiplyAdd(-1.0, get_S(parent), x[*parent], rhs); + } return backSubstituteUpper(get_R(), rhs, false); } diff --git a/linear/GaussianConditional.h b/linear/GaussianConditional.h index 594bc5a83..a0daf2d0d 100644 --- a/linear/GaussianConditional.h +++ b/linear/GaussianConditional.h @@ -109,15 +109,6 @@ public: /** equals function */ bool equals(const GaussianConditional &cg, double tol = 1e-9) const; -// /** permute the variables in the conditional */ -// void permuteWithInverse(const Permutation& inversePermutation); -// -// /** Permute the variables when only separator variables need to be permuted. -// * Returns true if any reordered variables appeared in the separator and -// * false if not. -// */ -// bool permuteSeparatorWithInverse(const Permutation& inversePermutation); - /** dimension of multivariate variable */ size_t dim() const { return rsd_.size1(); } diff --git a/linear/GaussianFactor.cpp b/linear/GaussianFactor.cpp index f23b6b8a8..33b1dfddc 100644 --- a/linear/GaussianFactor.cpp +++ b/linear/GaussianFactor.cpp @@ -36,7 +36,7 @@ #include using namespace std; -//using namespace boost::assign; + namespace ublas = boost::numeric::ublas; using namespace boost::lambda; @@ -247,21 +247,6 @@ double GaussianFactor::error(const VectorValues& c) const { return 0.5 * inner_prod(weighted,weighted); } -///* ************************************************************************* */ -//Dimensions GaussianFactor::dimensions() const { -// Dimensions result; -// BOOST_FOREACH(const NamedMatrix& jA, As_) -// result.insert(std::pair(jA.first,jA.second.size2())); -// return result; -//} -// -///* ************************************************************************* */ -//void GaussianFactor::tally_separator(Index key, set& separator) const { -// if(involves(key)) { -// BOOST_FOREACH(const NamedMatrix& jA, As_) -// if(jA.first != key) separator.insert(jA.first); -// } -//} /* ************************************************************************* */ Vector GaussianFactor::operator*(const VectorValues& x) const { @@ -271,23 +256,10 @@ Vector GaussianFactor::operator*(const VectorValues& x) const { // Just iterate over all A matrices and multiply in correct config part for(size_t pos=0; poswhiten(Ax); } -///* ************************************************************************* */ -//VectorValues GaussianFactor::operator^(const Vector& e) const { -// Vector E = model_->whiten(e); -// VectorValues x; -// // Just iterate over all A matrices and insert Ai^e into VectorValues -// for(size_t pos=0; posdim(); ++i) matrix_(i,j) = 0.0; -// ublas::triangular_adaptor AbLower(Ab_); -// fill(AbLower.begin1(), AbLower.end1(), 0.0); + } if(debug) gtsam::print(matrix_, "QR result: "); @@ -509,8 +478,6 @@ GaussianBayesNet::shared_ptr GaussianFactor::eliminate(size_t nrFrontals) { for(size_t j=0; jdim(); ++i) matrix_(i,j) = 0.0; -// ublas::triangular_adaptor AbLower(Ab_); -// fill(AbLower.begin1(), AbLower.end1(), 0.0); } if(debug) gtsam::print(matrix_, "QR result: "); diff --git a/linear/GaussianFactor.h b/linear/GaussianFactor.h index be644e574..e1d8bfa22 100644 --- a/linear/GaussianFactor.h +++ b/linear/GaussianFactor.h @@ -22,7 +22,6 @@ #include #include -//#include #include #include #include @@ -110,8 +109,6 @@ public: /** Construct from Conditional Gaussian */ GaussianFactor(const GaussianConditional& cg); -// /** Constructor that combines a set of factors */ -// GaussianFactor(const std::vector & factors); // Implementing Testable interface void print(const std::string& s = "") const; @@ -191,8 +188,6 @@ public: /** Return A*x */ Vector operator*(const VectorValues& x) const; -// /** Return A'*e */ -// VectorValues operator^(const Vector& e) const; /** x += A'*e */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;