39 std::ostream&
operator<<(std::ostream& os,
const Eigen::MatrixXd &mat)
43 int rowCount =
static_cast<int>(mat.rows());
45 for (
int row = 0; row < rowCount; ++row)
52 for (
int col = 0; col < mat.cols(); ++col)
54 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << mat(row, col);
57 if (row < rowCount - 1)
68 std::ostream&
operator<<(std::ostream& os,
const Eigen::VectorXd &vec)
71 for (
int dim = 0; dim < vec.rows(); ++dim)
73 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec(dim);
80 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec)
83 for (
size_t dim = 0; dim < vec.size(); ++dim)
85 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec[dim];
92 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec)
95 for (
size_t dim = 0; dim < vec.size(); ++dim)
97 os << std::setiosflags(std::ios::left) << std::setw(3) << (vec[dim] ?
"t" :
"f");
107 namespace FilterUtilities
112 if (!frameId.empty() && frameId.at(0) ==
'/')
114 frameId = frameId.substr(1);
117 if (!tfPrefix.empty() && tfPrefix.at(0) ==
'/')
119 tfPrefix = tfPrefix.substr(1);
123 if (!tfPrefix.empty())
125 frameId = tfPrefix +
"/" + frameId;
131 while (rotation >
PI)
136 while (rotation < -
PI)
void appendPrefix(std::string tfPrefix, std::string &frameId)
Utility method for appending tf2 prefixes cleanly.
std::ostream & operator<<(std::ostream &os, const Eigen::MatrixXd &mat)
double clampRotation(double rotation)
Utility method keeping RPY angles in the range [-pi, pi].
const double PI
Common variables.