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
{
cout << s << ": density on ";
cout << s << " Conditional density ";
for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
}
cout << endl;
gtsam::print(Matrix(get_R()),"R");
cout << formatMatrixIndented(" R = ", get_R()) << endl;
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");
model_->print("sigmas");
cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
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 {
cout << s << "\n";
if (empty()) {
cout << " empty, keys: ";
BOOST_FOREACH(const Key& key, keys()) { cout << formatter(key) << " "; }
cout << endl;
} else {
for(const_iterator key=begin(); key!=end(); ++key)
cout << boost::format("A[%1%]=\n")%formatter(*key) << getA(key) << endl;
cout << "b=" << getb() << endl;
model_->print("model");
void JacobianFactorUnordered::print(const string& s, const KeyFormatter& formatter) const
{
if(!s.empty())
cout << s << "\n";
for(const_iterator key = begin(); key != end(); ++key) {
cout <<
formatMatrixIndented((boost::format(" A[%1%] = ") % formatter(*key)).str(), getA(key))
<< endl;
}
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
bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const {
bool JacobianFactorUnordered::equals(const GaussianFactorUnordered& f_, double tol) const
{
if(!dynamic_cast<const JacobianFactorUnordered*>(&f_))
return false;
else {
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;
// 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()))
return false;
// Check matrix contents
constABlock Ab1(Ab_.range(0, Ab_.nBlocks()));
constABlock Ab2(f.Ab_.range(0, f.Ab_.nBlocks()));
for(size_t row=0; row< (size_t) Ab1.rows(); ++row)