remove format headers
parent
9347f35ae5
commit
7ed0083928
|
@ -23,9 +23,6 @@
|
||||||
#include <Eigen/SVD>
|
#include <Eigen/SVD>
|
||||||
#include <Eigen/LU>
|
#include <Eigen/LU>
|
||||||
|
|
||||||
#include <boost/tokenizer.hpp>
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <cstdarg>
|
#include <cstdarg>
|
||||||
#include <cstring>
|
#include <cstring>
|
||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
|
@ -128,8 +125,10 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
Vector operator^(const Matrix& A, const Vector & v) {
|
Vector operator^(const Matrix& A, const Vector & v) {
|
||||||
if (A.rows()!=v.size()) throw std::invalid_argument(
|
if (A.rows()!=v.size()) {
|
||||||
boost::str(boost::format("Matrix operator^ : A.m(%d)!=v.size(%d)") % A.rows() % v.size()));
|
throw std::invalid_argument("Matrix operator^ : A.m(" + std::to_string(A.rows()) + ")!=v.size(" +
|
||||||
|
std::to_string(v.size()) + ")");
|
||||||
|
}
|
||||||
// Vector vt = v.transpose();
|
// Vector vt = v.transpose();
|
||||||
// Vector vtA = vt * A;
|
// Vector vtA = vt * A;
|
||||||
// return vtA.transpose();
|
// return vtA.transpose();
|
||||||
|
@ -612,11 +611,12 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
|
||||||
else
|
else
|
||||||
matrixPrinted << matrix;
|
matrixPrinted << matrix;
|
||||||
const std::string matrixStr = matrixPrinted.str();
|
const std::string matrixStr = matrixPrinted.str();
|
||||||
boost::tokenizer<boost::char_separator<char> > tok(matrixStr, boost::char_separator<char>("\n"));
|
|
||||||
|
|
||||||
|
// Split the matrix string into lines and indent them
|
||||||
|
std::string line;
|
||||||
|
std::istringstream iss(matrixStr);
|
||||||
DenseIndex row = 0;
|
DenseIndex row = 0;
|
||||||
for(const std::string& line: tok)
|
while (std::getline(iss, line)) {
|
||||||
{
|
|
||||||
assert(row < effectiveRows);
|
assert(row < effectiveRows);
|
||||||
if(row > 0)
|
if(row > 0)
|
||||||
ss << padding;
|
ss << padding;
|
||||||
|
@ -625,6 +625,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
|
||||||
ss << "\n";
|
ss << "\n";
|
||||||
++ row;
|
++ row;
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")";
|
ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")";
|
||||||
}
|
}
|
||||||
|
|
|
@ -46,9 +46,11 @@ private:
|
||||||
protected:
|
protected:
|
||||||
typedef std::basic_string<char, std::char_traits<char>,
|
typedef std::basic_string<char, std::char_traits<char>,
|
||||||
tbb::tbb_allocator<char> > String;
|
tbb::tbb_allocator<char> > String;
|
||||||
|
typedef tbb::tbb_allocator<char> Allocator;
|
||||||
#else
|
#else
|
||||||
protected:
|
protected:
|
||||||
typedef std::string String;
|
typedef std::string String;
|
||||||
|
typedef std::allocator<char> Allocator;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
|
@ -20,7 +20,6 @@
|
||||||
#include <gtsam/base/cholesky.h>
|
#include <gtsam/base/cholesky.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
|
@ -19,9 +19,6 @@
|
||||||
#include <gtsam/base/debug.h>
|
#include <gtsam/base/debug.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <boost/algorithm/string/replace.hpp>
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <cstddef>
|
#include <cstddef>
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
@ -78,7 +75,7 @@ size_t TimingOutline::time() const {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void TimingOutline::print(const std::string& outline) const {
|
void TimingOutline::print(const std::string& outline) const {
|
||||||
std::string formattedLabel = label_;
|
std::string formattedLabel = label_;
|
||||||
boost::replace_all(formattedLabel, "_", " ");
|
std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' ');
|
||||||
std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU ("
|
std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU ("
|
||||||
<< n_ << " times, " << wall() << " wall, " << secs() << " children, min: "
|
<< n_ << " times, " << wall() << " wall, " << secs() << " children, min: "
|
||||||
<< min() << " max: " << max() << ")\n";
|
<< min() << " max: " << max() << ")\n";
|
||||||
|
@ -248,16 +245,14 @@ void toc(size_t id, const char *label) {
|
||||||
if (id != current->id_) {
|
if (id != current->id_) {
|
||||||
gTimingRoot->print();
|
gTimingRoot->print();
|
||||||
throw std::invalid_argument(
|
throw std::invalid_argument(
|
||||||
(boost::format(
|
"gtsam timing: Mismatched tic/toc: gttoc(\"" + std::string(label) +
|
||||||
"gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".")
|
"\") called when last tic was \"" + current->label_ + "\".");
|
||||||
% label % current->label_).str());
|
|
||||||
}
|
}
|
||||||
if (!current->parent_.lock()) {
|
if (!current->parent_.lock()) {
|
||||||
gTimingRoot->print();
|
gTimingRoot->print();
|
||||||
throw std::invalid_argument(
|
throw std::invalid_argument(
|
||||||
(boost::format(
|
"gtsam timing: Mismatched tic/toc: extra gttoc(\"" + std::string(label) +
|
||||||
"gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root")
|
"\"), already at the root");
|
||||||
% label).str());
|
|
||||||
}
|
}
|
||||||
current->toc();
|
current->toc();
|
||||||
gCurrentTimer = current->parent_;
|
gCurrentTimer = current->parent_;
|
||||||
|
|
|
@ -24,6 +24,7 @@
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <map>
|
#include <map>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <iomanip>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -162,7 +163,9 @@ namespace gtsam {
|
||||||
const typename Base::LabelFormatter& labelFormatter =
|
const typename Base::LabelFormatter& labelFormatter =
|
||||||
&DefaultFormatter) const {
|
&DefaultFormatter) const {
|
||||||
auto valueFormatter = [](const double& v) {
|
auto valueFormatter = [](const double& v) {
|
||||||
return (boost::format("%4.8g") % v).str();
|
std::stringstream ss;
|
||||||
|
ss << std::setw(4) << std::setprecision(8) << v;
|
||||||
|
return ss.str();
|
||||||
};
|
};
|
||||||
Base::print(s, labelFormatter, valueFormatter);
|
Base::print(s, labelFormatter, valueFormatter);
|
||||||
}
|
}
|
||||||
|
|
|
@ -22,7 +22,6 @@
|
||||||
#include <gtsam/discrete/DecisionTree.h>
|
#include <gtsam/discrete/DecisionTree.h>
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
@ -33,6 +32,7 @@
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <optional>
|
#include <optional>
|
||||||
|
#include <cassert>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -192,8 +192,8 @@ namespace gtsam {
|
||||||
|
|
||||||
~Choice() override {
|
~Choice() override {
|
||||||
#ifdef DT_DEBUG_MEMORY
|
#ifdef DT_DEBUG_MEMORY
|
||||||
std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id()
|
std::cout << Node::nrNodes << " destructing (Choice) " << this->id()
|
||||||
<< std::std::endl;
|
<< std::endl;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -282,9 +282,9 @@ namespace gtsam {
|
||||||
const ValueFormatter& valueFormatter) const override {
|
const ValueFormatter& valueFormatter) const override {
|
||||||
std::cout << s << " Choice(";
|
std::cout << s << " Choice(";
|
||||||
std::cout << labelFormatter(label_) << ") " << std::endl;
|
std::cout << labelFormatter(label_) << ") " << std::endl;
|
||||||
for (size_t i = 0; i < branches_.size(); i++)
|
for (size_t i = 0; i < branches_.size(); i++) {
|
||||||
branches_[i]->print((boost::format("%s %d") % s % i).str(),
|
branches_[i]->print(s + " " + std::to_string(i), labelFormatter, valueFormatter);
|
||||||
labelFormatter, valueFormatter);
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** output to graphviz (as a a graph) */
|
/** output to graphviz (as a a graph) */
|
||||||
|
@ -643,11 +643,8 @@ namespace gtsam {
|
||||||
// Create a simple choice node with values as leaves.
|
// Create a simple choice node with values as leaves.
|
||||||
if (size != nrChoices) {
|
if (size != nrChoices) {
|
||||||
std::cout << "Trying to create DD on " << begin->first << std::endl;
|
std::cout << "Trying to create DD on " << begin->first << std::endl;
|
||||||
std::cout << boost::format(
|
std::cout << "DecisionTree::create: expected " << nrChoices
|
||||||
"DecisionTree::create: expected %d values but got %d "
|
<< " values but got " << size << " instead" << std::endl;
|
||||||
"instead") %
|
|
||||||
nrChoices % size
|
|
||||||
<< std::endl;
|
|
||||||
throw std::invalid_argument("DecisionTree::create invalid argument");
|
throw std::invalid_argument("DecisionTree::create invalid argument");
|
||||||
}
|
}
|
||||||
auto choice = std::make_shared<Choice>(begin->first, endY - beginY);
|
auto choice = std::make_shared<Choice>(begin->first, endY - beginY);
|
||||||
|
|
|
@ -22,7 +22,6 @@
|
||||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||||
#include <gtsam/discrete/DiscreteConditional.h>
|
#include <gtsam/discrete/DiscreteConditional.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
@ -79,8 +78,9 @@ namespace gtsam {
|
||||||
const KeyFormatter& formatter) const {
|
const KeyFormatter& formatter) const {
|
||||||
cout << s;
|
cout << s;
|
||||||
cout << " f[";
|
cout << " f[";
|
||||||
for (auto&& key : keys())
|
for (auto&& key : keys()) {
|
||||||
cout << boost::format(" (%1%,%2%),") % formatter(key) % cardinality(key);
|
cout << " (" << formatter(key) << "," << cardinality(key) << "),";
|
||||||
|
}
|
||||||
cout << " ]" << endl;
|
cout << " ]" << endl;
|
||||||
ADT::print("", formatter);
|
ADT::print("", formatter);
|
||||||
}
|
}
|
||||||
|
@ -104,13 +104,12 @@ namespace gtsam {
|
||||||
/* ************************************************************************ */
|
/* ************************************************************************ */
|
||||||
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
|
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
|
||||||
size_t nrFrontals, ADT::Binary op) const {
|
size_t nrFrontals, ADT::Binary op) const {
|
||||||
if (nrFrontals > size())
|
if (nrFrontals > size()) {
|
||||||
throw invalid_argument(
|
throw invalid_argument(
|
||||||
(boost::format(
|
"DecisionTreeFactor::combine: invalid number of frontal "
|
||||||
"DecisionTreeFactor::combine: invalid number of frontal "
|
"keys " +
|
||||||
"keys %d, nr.keys=%d") %
|
std::to_string(nrFrontals) + ", nr.keys=" + std::to_string(size()));
|
||||||
nrFrontals % size())
|
}
|
||||||
.str());
|
|
||||||
|
|
||||||
// sum over nrFrontals keys
|
// sum over nrFrontals keys
|
||||||
size_t i;
|
size_t i;
|
||||||
|
@ -132,13 +131,13 @@ namespace gtsam {
|
||||||
/* ************************************************************************ */
|
/* ************************************************************************ */
|
||||||
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
|
DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(
|
||||||
const Ordering& frontalKeys, ADT::Binary op) const {
|
const Ordering& frontalKeys, ADT::Binary op) const {
|
||||||
if (frontalKeys.size() > size())
|
if (frontalKeys.size() > size()) {
|
||||||
throw invalid_argument(
|
throw invalid_argument(
|
||||||
(boost::format(
|
"DecisionTreeFactor::combine: invalid number of frontal "
|
||||||
"DecisionTreeFactor::combine: invalid number of frontal "
|
"keys " +
|
||||||
"keys %d, nr.keys=%d") %
|
std::to_string(frontalKeys.size()) + ", nr.keys=" +
|
||||||
frontalKeys.size() % size())
|
std::to_string(size()));
|
||||||
.str());
|
}
|
||||||
|
|
||||||
// sum over nrFrontals keys
|
// sum over nrFrontals keys
|
||||||
size_t i;
|
size_t i;
|
||||||
|
@ -193,7 +192,9 @@ namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************ */
|
/* ************************************************************************ */
|
||||||
static std::string valueFormatter(const double& v) {
|
static std::string valueFormatter(const double& v) {
|
||||||
return (boost::format("%4.2g") % v).str();
|
std::stringstream ss;
|
||||||
|
ss << std::setw(4) << std::setprecision(2) << std::fixed << v;
|
||||||
|
return ss.str();
|
||||||
}
|
}
|
||||||
|
|
||||||
/** output to graphviz format, stream version */
|
/** output to graphviz format, stream version */
|
||||||
|
|
|
@ -17,7 +17,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <boost/format.hpp> // for key names
|
|
||||||
#include "DiscreteKey.h"
|
#include "DiscreteKey.h"
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -26,7 +25,7 @@ namespace gtsam {
|
||||||
|
|
||||||
DiscreteKeys::DiscreteKeys(const vector<int>& cs) {
|
DiscreteKeys::DiscreteKeys(const vector<int>& cs) {
|
||||||
for (size_t i = 0; i < cs.size(); i++) {
|
for (size_t i = 0; i < cs.size(); i++) {
|
||||||
string name = boost::str(boost::format("v%1%") % i);
|
string name = "v" + std::to_string(i);
|
||||||
push_back(DiscreteKey(i, cs[i]));
|
push_back(DiscreteKey(i, cs[i]));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,8 +25,6 @@
|
||||||
#include <gtsam/discrete/DecisionTree-inl.h> // for convert only
|
#include <gtsam/discrete/DecisionTree-inl.h> // for convert only
|
||||||
#define DISABLE_TIMING
|
#define DISABLE_TIMING
|
||||||
|
|
||||||
#include <boost/tokenizer.hpp>
|
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
#include <gtsam/discrete/Signature.h>
|
#include <gtsam/discrete/Signature.h>
|
||||||
|
@ -81,9 +79,9 @@ void resetCounts() {
|
||||||
}
|
}
|
||||||
void printCounts(const string& s) {
|
void printCounts(const string& s) {
|
||||||
#ifndef DISABLE_TIMING
|
#ifndef DISABLE_TIMING
|
||||||
cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds %
|
cout << s << ": " << std::setw(3) << muls << " muls, " <<
|
||||||
(1000 * elapsed)
|
std::setw(3) << adds << " adds, " << 1000 * elapsed << " ms."
|
||||||
<< endl;
|
<< endl;
|
||||||
#endif
|
#endif
|
||||||
resetCounts();
|
resetCounts();
|
||||||
}
|
}
|
||||||
|
@ -133,7 +131,9 @@ ADT create(const Signature& signature) {
|
||||||
ADT p(signature.discreteKeys(), signature.cpt());
|
ADT p(signature.discreteKeys(), signature.cpt());
|
||||||
static size_t count = 0;
|
static size_t count = 0;
|
||||||
const DiscreteKey& key = signature.key();
|
const DiscreteKey& key = signature.key();
|
||||||
string DOTfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
|
std::stringstream ss;
|
||||||
|
ss << "CPT-" << std::setw(3) << std::setfill('0') << ++count << "-" << key.first;
|
||||||
|
string DOTfile = ss.str();
|
||||||
dot(p, DOTfile);
|
dot(p, DOTfile);
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
|
|
|
@ -26,6 +26,8 @@
|
||||||
#include <gtsam/discrete/DecisionTree-inl.h>
|
#include <gtsam/discrete/DecisionTree-inl.h>
|
||||||
#include <gtsam/discrete/Signature.h>
|
#include <gtsam/discrete/Signature.h>
|
||||||
|
|
||||||
|
#include <iomanip>
|
||||||
|
|
||||||
using std::vector;
|
using std::vector;
|
||||||
using std::string;
|
using std::string;
|
||||||
using std::map;
|
using std::map;
|
||||||
|
@ -50,7 +52,9 @@ struct CrazyDecisionTree : public DecisionTree<string, Crazy> {
|
||||||
void print(const std::string& s = "") const {
|
void print(const std::string& s = "") const {
|
||||||
auto keyFormatter = [](const std::string& s) { return s; };
|
auto keyFormatter = [](const std::string& s) { return s; };
|
||||||
auto valueFormatter = [](const Crazy& v) {
|
auto valueFormatter = [](const Crazy& v) {
|
||||||
return (boost::format("{%d,%4.2g}") % v.a % v.b).str();
|
std::stringstream ss;
|
||||||
|
ss << "{" << v.a << "," << std::setw(4) << std::setprecision(2) << v.b << "}";
|
||||||
|
return ss.str();
|
||||||
};
|
};
|
||||||
DecisionTree<string, Crazy>::print("", keyFormatter, valueFormatter);
|
DecisionTree<string, Crazy>::print("", keyFormatter, valueFormatter);
|
||||||
}
|
}
|
||||||
|
@ -86,7 +90,7 @@ struct DT : public DecisionTree<string, int> {
|
||||||
void print(const std::string& s = "") const {
|
void print(const std::string& s = "") const {
|
||||||
auto keyFormatter = [](const std::string& s) { return s; };
|
auto keyFormatter = [](const std::string& s) { return s; };
|
||||||
auto valueFormatter = [](const int& v) {
|
auto valueFormatter = [](const int& v) {
|
||||||
return (boost::format("%d") % v).str();
|
return std::to_string(v);
|
||||||
};
|
};
|
||||||
std::cout << s;
|
std::cout << s;
|
||||||
Base::print("", keyFormatter, valueFormatter);
|
Base::print("", keyFormatter, valueFormatter);
|
||||||
|
|
|
@ -20,8 +20,6 @@
|
||||||
#include <gtsam/discrete/DecisionTreeFactor.h>
|
#include <gtsam/discrete/DecisionTreeFactor.h>
|
||||||
#include <gtsam/hybrid/HybridFactorGraph.h>
|
#include <gtsam/hybrid/HybridFactorGraph.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -22,7 +22,6 @@
|
||||||
#include <gtsam/hybrid/HybridFactor.h>
|
#include <gtsam/hybrid/HybridFactor.h>
|
||||||
#include <gtsam/inference/FactorGraph.h>
|
#include <gtsam/inference/FactorGraph.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <unordered_map>
|
#include <unordered_map>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
|
@ -27,7 +27,6 @@
|
||||||
#include <gtsam/nonlinear/Symbol.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
@ -194,7 +193,7 @@ class MixtureFactor : public HybridFactor {
|
||||||
std::cout << "\nMixtureFactor\n";
|
std::cout << "\nMixtureFactor\n";
|
||||||
auto valueFormatter = [](const sharedFactor& v) {
|
auto valueFormatter = [](const sharedFactor& v) {
|
||||||
if (v) {
|
if (v) {
|
||||||
return (boost::format("Nonlinear factor on %d keys") % v->size()).str();
|
return "Nonlinear factor on " + std::to_string(v->size()) + " keys";
|
||||||
} else {
|
} else {
|
||||||
return std::string("nullptr");
|
return std::string("nullptr");
|
||||||
}
|
}
|
||||||
|
|
|
@ -20,7 +20,6 @@
|
||||||
#include <gtsam/inference/Key.h>
|
#include <gtsam/inference/Key.h>
|
||||||
#include <gtsam/inference/LabeledSymbol.h>
|
#include <gtsam/inference/LabeledSymbol.h>
|
||||||
|
|
||||||
#include <boost/lexical_cast.hpp>
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
@ -30,10 +29,12 @@ namespace gtsam {
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
string _defaultKeyFormatter(Key key) {
|
string _defaultKeyFormatter(Key key) {
|
||||||
const Symbol asSymbol(key);
|
const Symbol asSymbol(key);
|
||||||
if (asSymbol.chr() > 0)
|
if (asSymbol.chr() > 0) {
|
||||||
return (string) asSymbol;
|
return (string) asSymbol;
|
||||||
else
|
}
|
||||||
return boost::lexical_cast<string>(key);
|
else {
|
||||||
|
return std::to_string(key);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
@ -48,10 +49,12 @@ string _multirobotKeyFormatter(Key key) {
|
||||||
return (string) asLabeledSymbol;
|
return (string) asLabeledSymbol;
|
||||||
|
|
||||||
const Symbol asSymbol(key);
|
const Symbol asSymbol(key);
|
||||||
if (asLabeledSymbol.chr() > 0)
|
if (asLabeledSymbol.chr() > 0) {
|
||||||
return (string) asSymbol;
|
return (string) asSymbol;
|
||||||
else
|
}
|
||||||
return boost::lexical_cast<string>(key);
|
else {
|
||||||
|
return std::to_string(key);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -17,8 +17,6 @@
|
||||||
|
|
||||||
#include <gtsam/inference/LabeledSymbol.h>
|
#include <gtsam/inference/LabeledSymbol.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <boost/lexical_cast.hpp>
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -73,7 +71,9 @@ void LabeledSymbol::print(const std::string& s) const {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
LabeledSymbol::operator std::string() const {
|
LabeledSymbol::operator std::string() const {
|
||||||
return str(boost::format("%c%c%d") % c_ % label_ % j_);
|
char buf[100];
|
||||||
|
sprintf(buf, "%c%c%zu", c_, label_, j_);
|
||||||
|
return std::string(buf);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -19,8 +19,6 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <gtsam/inference/Ordering.h>
|
#include <gtsam/inference/Ordering.h>
|
||||||
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
|
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
|
||||||
|
|
||||||
|
@ -107,9 +105,9 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
|
||||||
gttic(ccolamd);
|
gttic(ccolamd);
|
||||||
int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
|
int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0],
|
||||||
knobs, stats, &cmember[0]);
|
knobs, stats, &cmember[0]);
|
||||||
if (rv != 1)
|
if (rv != 1) {
|
||||||
throw runtime_error(
|
throw runtime_error("ccolamd failed with return value " + to_string(rv));
|
||||||
(boost::format("ccolamd failed with return value %1%") % rv).str());
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// ccolamd_report(stats);
|
// ccolamd_report(stats);
|
||||||
|
|
|
@ -18,12 +18,10 @@
|
||||||
|
|
||||||
#include <gtsam/inference/Symbol.h>
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
|
||||||
#include <boost/bind/bind.hpp>
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <limits.h>
|
#include <limits.h>
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
#include <sstream>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -40,8 +38,8 @@ Symbol::Symbol(Key key) :
|
||||||
|
|
||||||
Key Symbol::key() const {
|
Key Symbol::key() const {
|
||||||
if (j_ > indexMask) {
|
if (j_ > indexMask) {
|
||||||
boost::format msg("Symbol index is too large, j=%d, indexMask=%d");
|
std::stringstream msg;
|
||||||
msg % j_ % indexMask;
|
msg << "Symbol index is too large, j=" << j_ << ", indexMask=" << indexMask;
|
||||||
throw std::invalid_argument(msg.str());
|
throw std::invalid_argument(msg.str());
|
||||||
}
|
}
|
||||||
Key key = (Key(c_) << indexBits) | j_;
|
Key key = (Key(c_) << indexBits) | j_;
|
||||||
|
@ -57,7 +55,9 @@ bool Symbol::equals(const Symbol& expected, double tol) const {
|
||||||
}
|
}
|
||||||
|
|
||||||
Symbol::operator std::string() const {
|
Symbol::operator std::string() const {
|
||||||
return str(boost::format("%c%d") % c_ % j_);
|
char buf[10];
|
||||||
|
sprintf(buf, "%c%zu", c_, j_);
|
||||||
|
return std::string(buf);
|
||||||
}
|
}
|
||||||
|
|
||||||
static Symbol make(gtsam::Key key) { return Symbol(key);}
|
static Symbol make(gtsam::Key key) { return Symbol(key);}
|
||||||
|
|
|
@ -21,13 +21,10 @@
|
||||||
#include <gtsam/linear/linearExceptions.h>
|
#include <gtsam/linear/linearExceptions.h>
|
||||||
#include <gtsam/hybrid/HybridValues.h>
|
#include <gtsam/hybrid/HybridValues.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
#ifdef __GNUC__
|
#ifdef __GNUC__
|
||||||
#pragma GCC diagnostic push
|
#pragma GCC diagnostic push
|
||||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||||
#endif
|
#endif
|
||||||
#include <boost/lambda/lambda.hpp>
|
|
||||||
#include <boost/lambda/bind.hpp>
|
|
||||||
#ifdef __GNUC__
|
#ifdef __GNUC__
|
||||||
#pragma GCC diagnostic pop
|
#pragma GCC diagnostic pop
|
||||||
#endif
|
#endif
|
||||||
|
@ -104,22 +101,20 @@ namespace gtsam {
|
||||||
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
|
void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
|
||||||
cout << s << " p(";
|
cout << s << " p(";
|
||||||
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
|
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
|
||||||
cout << (boost::format("%1%") % (formatter(*it))).str()
|
cout << formatter(*it) << (nrFrontals() > 1 ? " " : "");
|
||||||
<< (nrFrontals() > 1 ? " " : "");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (nrParents()) {
|
if (nrParents()) {
|
||||||
cout << " |";
|
cout << " |";
|
||||||
for (const_iterator it = beginParents(); it != endParents(); ++it) {
|
for (const_iterator it = beginParents(); it != endParents(); ++it) {
|
||||||
cout << " " << (boost::format("%1%") % (formatter(*it))).str();
|
cout << " " << formatter(*it);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
cout << ")" << endl;
|
cout << ")" << endl;
|
||||||
|
|
||||||
cout << formatMatrixIndented(" R = ", R()) << endl;
|
cout << formatMatrixIndented(" R = ", 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(" S[" + formatter(*it) + "] = ", getA(it)) << endl;
|
||||||
<< endl;
|
|
||||||
}
|
}
|
||||||
cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
|
cout << formatMatrixIndented(" d = ", getb(), true) << "\n";
|
||||||
if (nrParents() == 0) {
|
if (nrParents() == 0) {
|
||||||
|
|
|
@ -17,7 +17,6 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/linear/GaussianDensity.h>
|
#include <gtsam/linear/GaussianDensity.h>
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
using std::cout;
|
using std::cout;
|
||||||
|
@ -37,8 +36,9 @@ GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean,
|
||||||
void GaussianDensity::print(const string& s,
|
void GaussianDensity::print(const string& s,
|
||||||
const KeyFormatter& formatter) const {
|
const KeyFormatter& formatter) const {
|
||||||
cout << s << ": density on ";
|
cout << s << ": density on ";
|
||||||
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it)
|
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
|
||||||
cout << (boost::format("[%1%]") % (formatter(*it))).str() << " ";
|
cout << "[" << formatter(*it) << "] ";
|
||||||
|
}
|
||||||
cout << endl;
|
cout << endl;
|
||||||
gtsam::print(mean(), "mean: ");
|
gtsam::print(mean(), "mean: ");
|
||||||
gtsam::print(covariance(), "covariance: ");
|
gtsam::print(covariance(), "covariance: ");
|
||||||
|
|
|
@ -29,8 +29,6 @@
|
||||||
#include <gtsam/base/ThreadsafeException.h>
|
#include <gtsam/base/ThreadsafeException.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include "gtsam/base/Vector.h"
|
#include "gtsam/base/Vector.h"
|
||||||
|
|
|
@ -31,8 +31,6 @@
|
||||||
#include <gtsam/base/FastMap.h>
|
#include <gtsam/base/FastMap.h>
|
||||||
#include <gtsam/base/cholesky.h>
|
#include <gtsam/base/cholesky.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
|
@ -411,7 +409,7 @@ void JacobianFactor::print(const string& s,
|
||||||
if (!s.empty())
|
if (!s.empty())
|
||||||
cout << s << "\n";
|
cout << s << "\n";
|
||||||
for (const_iterator key = begin(); key != end(); ++key) {
|
for (const_iterator key = begin(); key != end(); ++key) {
|
||||||
cout << boost::format(" A[%1%] = ") % formatter(*key);
|
cout << " A[" << formatter(*key) << "] = ";
|
||||||
cout << getA(key).format(matlabFormat()) << endl;
|
cout << getA(key).format(matlabFormat()) << endl;
|
||||||
}
|
}
|
||||||
cout << formatMatrixIndented(" b = ", getb(), true) << "\n";
|
cout << formatMatrixIndented(" b = ", getb(), true) << "\n";
|
||||||
|
|
|
@ -19,8 +19,6 @@
|
||||||
#include <gtsam/linear/NoiseModel.h>
|
#include <gtsam/linear/NoiseModel.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
|
@ -606,7 +604,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
void Isotropic::print(const string& name) const {
|
void Isotropic::print(const string& name) const {
|
||||||
cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl;
|
cout << "isotropic dim=" << dim() << " sigma=" << sigma_ << endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -18,7 +18,6 @@
|
||||||
|
|
||||||
#include <gtsam/linear/linearExceptions.h>
|
#include <gtsam/linear/linearExceptions.h>
|
||||||
#include <gtsam/inference/Symbol.h>
|
#include <gtsam/inference/Symbol.h>
|
||||||
#include <boost/format.hpp>
|
|
||||||
#include <boost/lexical_cast.hpp>
|
#include <boost/lexical_cast.hpp>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
@ -29,9 +28,8 @@ namespace gtsam {
|
||||||
if(!description_) {
|
if(!description_) {
|
||||||
description_ = String(
|
description_ = String(
|
||||||
"\nIndeterminant linear system detected while working near variable\n"
|
"\nIndeterminant linear system detected while working near variable\n"
|
||||||
+ boost::lexical_cast<String>(j_) +
|
+ std::to_string(j_) +
|
||||||
+ " (Symbol: " + boost::lexical_cast<String>(
|
+ " (Symbol: " + gtsam::DefaultKeyFormatter(gtsam::Symbol(j_)) + ").\n"
|
||||||
gtsam::DefaultKeyFormatter(gtsam::Symbol(j_))) + ").\n"
|
|
||||||
"\n\
|
"\n\
|
||||||
Thrown when a linear system is ill-posed. The most common cause for this\n\
|
Thrown when a linear system is ill-posed. The most common cause for this\n\
|
||||||
error is having underconstrained variables. Mathematically, the system is\n\
|
error is having underconstrained variables. Mathematically, the system is\n\
|
||||||
|
@ -45,22 +43,21 @@ more information.");
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
const char* InvalidNoiseModel::what() const noexcept {
|
const char* InvalidNoiseModel::what() const noexcept {
|
||||||
if(description_.empty())
|
if(description_.empty())
|
||||||
description_ = (boost::format(
|
description_ = "A JacobianFactor was attempted to be constructed or modified to use a\n"
|
||||||
"A JacobianFactor was attempted to be constructed or modified to use a\n"
|
"noise model of incompatible dimension. The JacobianFactor has\n"
|
||||||
"noise model of incompatible dimension. The JacobianFactor has\n"
|
"dimensionality (i.e. length of error vector) " + std::to_string(factorDims) +
|
||||||
"dimensionality (i.e. length of error vector) %d but the provided noise\n"
|
" but the provided noise model has dimensionality " + std::to_string(noiseModelDims) + ".";
|
||||||
"model has dimensionality %d.") % factorDims % noiseModelDims).str();
|
|
||||||
return description_.c_str();
|
return description_.c_str();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
const char* InvalidMatrixBlock::what() const noexcept {
|
const char* InvalidMatrixBlock::what() const noexcept {
|
||||||
if(description_.empty())
|
if(description_.empty()) {
|
||||||
description_ = (boost::format(
|
description_ = "A JacobianFactor was attempted to be constructed with a matrix block of\n"
|
||||||
"A JacobianFactor was attempted to be constructed with a matrix block of\n"
|
"inconsistent dimension. The JacobianFactor has " + std::to_string(factorRows) +
|
||||||
"inconsistent dimension. The JacobianFactor has %d rows (i.e. length of\n"
|
" rows (i.e. length of error vector) but the provided matrix block has " +
|
||||||
"error vector) but the provided matrix block has %d rows.")
|
std::to_string(blockRows) + " rows.";
|
||||||
% factorRows % blockRows).str();
|
}
|
||||||
return description_.c_str();
|
return description_.c_str();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -28,11 +28,10 @@
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
#include <iomanip>
|
||||||
#include <limits>
|
#include <limits>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
|
||||||
|
@ -218,12 +217,12 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
|
||||||
#else
|
#else
|
||||||
double iterationTime = lamda_iteration_timer.elapsed();
|
double iterationTime = lamda_iteration_timer.elapsed();
|
||||||
#endif
|
#endif
|
||||||
if (currentState->iterations == 0)
|
if (currentState->iterations == 0) {
|
||||||
cout << "iter cost cost_change lambda success iter_time" << endl;
|
cout << "iter cost cost_change lambda success iter_time" << endl;
|
||||||
|
}
|
||||||
cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") % currentState->iterations %
|
cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2)
|
||||||
newError % costChange % currentState->lambda % systemSolvedSuccessfully %
|
<< costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4)
|
||||||
iterationTime << endl;
|
<< systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (step_is_successful) {
|
if (step_is_successful) {
|
||||||
|
|
|
@ -18,7 +18,6 @@
|
||||||
|
|
||||||
#include <gtsam/hybrid/HybridValues.h>
|
#include <gtsam/hybrid/HybridValues.h>
|
||||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -96,12 +95,12 @@ NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel(
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
static void check(const SharedNoiseModel& noiseModel, size_t m) {
|
static void check(const SharedNoiseModel& noiseModel, size_t m) {
|
||||||
if (noiseModel && m != noiseModel->dim())
|
if (noiseModel && m != noiseModel->dim()) {
|
||||||
throw std::invalid_argument(
|
throw std::invalid_argument(
|
||||||
boost::str(
|
"NoiseModelFactor: NoiseModel has dimension " +
|
||||||
boost::format(
|
std::to_string(noiseModel->dim()) +
|
||||||
"NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.")
|
" instead of " + std::to_string(m) + ".");
|
||||||
% noiseModel->dim() % m));
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
@ -24,8 +24,6 @@
|
||||||
#include <gtsam_unstable/geometry/Event.h>
|
#include <gtsam_unstable/geometry/Event.h>
|
||||||
#include <gtsam_unstable/slam/TOAFactor.h>
|
#include <gtsam_unstable/slam/TOAFactor.h>
|
||||||
|
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
|
@ -25,7 +25,6 @@
|
||||||
#include <gtsam_unstable/slam/TOAFactor.h>
|
#include <gtsam_unstable/slam/TOAFactor.h>
|
||||||
|
|
||||||
#include <CppUnitLite/TestHarness.h>
|
#include <CppUnitLite/TestHarness.h>
|
||||||
#include <boost/format.hpp>
|
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
Loading…
Reference in New Issue