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

Pretty-printing in JacobianFactorUnordered and GaussianConditionalUnordered

release/4.3a0
Richard Roberts 2013-07-12 22:27:41 +00:00
parent f8ef1d9604
commit 073679f831
2 changed files with 35 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -55,17 +55,21 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditionalUnordered::print(const string &s, const IndexFormatter& formatter) const void GaussianConditionalUnordered::print(const string &s, const IndexFormatter& formatter) const
{ {
cout << s << ": density on "; 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;
gtsam::print(Matrix(get_R()),"R"); cout << formatMatrixIndented(" R = ", get_R()) << endl;
for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { for(const_iterator it = beginParents() ; it != endParents() ; ++it ) {
gtsam::print(Matrix(getA(it)), (boost::format("S[%1%]")%(formatter(*it))).str()); cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it))
<< endl;
} }
gtsam::print(Vector(getb()),"d"); cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
model_->print("sigmas"); if(model_)
model_->print(" Noise model: ");
else
cout << " No noise model" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */

40
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -315,34 +315,46 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactorUnordered::print(const string& s, const KeyFormatter& formatter) const { void JacobianFactorUnordered::print(const string& s, const KeyFormatter& formatter) const
cout << s << "\n"; {
if (empty()) { if(!s.empty())
cout << " empty, keys: "; cout << s << "\n";
BOOST_FOREACH(const Key& key, keys()) { cout << formatter(key) << " "; } for(const_iterator key = begin(); key != end(); ++key) {
cout << endl; cout <<
} else { formatMatrixIndented((boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key))
for(const_iterator key=begin(); key!=end(); ++key) << endl;
cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl;
cout << "b=" << getb() << endl;
model_->print("model");
} }
cout << formatMatrixIndented(" b = ", getb(), true) << "\n";
if(model_)
model_->print(" Noise model: ");
else
cout << " No noise model" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Check if two linear factors are equal // Check if two linear factors are equal
bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const { bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const
{
if(!dynamic_cast<const JacobianFactorUnordered*>(&f_)) if(!dynamic_cast<const JacobianFactorUnordered*>(&f_))
return false; return false;
else { else {
const JacobianFactorUnordered& f(static_cast<const JacobianFactorUnordered&>(f_)); const JacobianFactorUnordered& f(static_cast<const JacobianFactorUnordered&>(f_));
if (empty()) return (f.empty());
if(keys()!=f.keys() /*|| !model_->equals(lf->model_, tol)*/) // Check keys
if(keys() != f.keys())
return false; return false;
// Check noise model
if(model_ && !f.model_ || !model_ && f.model_)
return false;
if(model_ && f.model_ && !model_->equals(*f.model_, tol))
return false;
// Check matrix sizes
if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols())) if (!(Ab_.rows() == f.Ab_.rows() && Ab_.cols() == f.Ab_.cols()))
return false; return false;
// Check matrix contents
constABlock Ab1(Ab_.range(0, Ab_.nBlocks())); constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks())); constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
for(size_t row=0; row< (size_t) Ab1.rows(); ++row) for(size_t row=0; row< (size_t) Ab1.rows(); ++row)