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

Fixed lint errors

release/4.3a0
Frank Dellaert 2018-10-02 21:53:49 -04:00
parent efa35e6a82
commit 3116fd30b9
1 changed files with 28 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/linear/GaussianConditional.cpp
View File

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