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

46
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,8 +56,7 @@ 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() << " ";
@ -74,18 +75,17 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
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()));
@ -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()));
@ -198,8 +193,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
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();
@ -207,4 +201,4 @@ namespace gtsam {
} }
} }
} } // namespace gtsam