diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index eefb6302f..60187d129 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -12,11 +12,13 @@ /** * @file GaussianConditional.cpp * @brief Conditional Gaussian Base class - * @author Christian Potthast + * @author Christian Potthast, Frank Dellaert */ -#include -#include +#include +#include +#include + #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -28,9 +30,9 @@ #pragma GCC diagnostic pop #endif -#include -#include -#include +#include +#include +#include using namespace std; @@ -54,38 +56,36 @@ namespace gtsam { BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} /* ************************************************************************* */ - void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const - { + void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " Conditional density "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; } cout << endl; cout << formatMatrixIndented(" R = ", get_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)) << endl; } cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; - if(model_) + if (model_) model_->print(" Noise model: "); else cout << " No noise model" << endl; } /* ************************************************************************* */ - bool GaussianConditional::equals(const GaussianFactor& f, double tol) const - { - if (const GaussianConditional* c = dynamic_cast(&f)) - { + bool GaussianConditional::equals(const GaussianFactor& f, double tol) const { + if (const GaussianConditional* c = dynamic_cast(&f)) { // check if the size of the parents_ map is the same if (parents().size() != c->parents().size()) return false; // check if R_ and d_ are linear independent for (DenseIndex i = 0; i < Ab_.rows(); i++) { - list rows1; rows1.push_back(Vector(get_R().row(i))); - list rows2; rows2.push_back(Vector(c->get_R().row(i))); + list rows1, rows2; + rows1.push_back(Vector(get_R().row(i))); + rows2.push_back(Vector(c->get_R().row(i))); // check if the matrices are the same // iterate over the parents_ map @@ -109,16 +109,13 @@ namespace gtsam { return false; return true; - } - else - { + } else { return false; } } /* ************************************************************************* */ - VectorValues GaussianConditional::solve(const VectorValues& x) const - { + VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables const Vector xS = x.vector(FastVector(beginParents(), endParents())); @@ -146,8 +143,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solveOtherRHS( - const VectorValues& parents, const VectorValues& rhs) const - { + const VectorValues& parents, const VectorValues& rhs) const { // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); @@ -159,13 +155,13 @@ namespace gtsam { Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas - if(model_) + if (model_) soln.array() *= model_->sigmas().array(); // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -174,8 +170,7 @@ namespace gtsam { } /* ************************************************************************* */ - void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const - { + void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(FastVector(beginFrontals(), endFrontals())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); @@ -186,25 +181,24 @@ namespace gtsam { gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas - if(model_) + if (model_) frontalVec.array() *= model_->sigmas().array(); // Write frontal solution into a VectorValues DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal)); vectorPosition += getDim(frontal); } } /* ************************************************************************* */ - void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const - { + void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); vectorPosition += getDim(frontal); } } -} +} // namespace gtsam