33 #ifndef ROBOT_LOCALIZATION_FILTER_UTILITIES_H 34 #define ROBOT_LOCALIZATION_FILTER_UTILITIES_H 36 #include <Eigen/Dense> 43 #define FB_DEBUG(msg) if (getDebug()) { *debugStream_ << msg; } 46 std::ostream&
operator<<(std::ostream& os,
const Eigen::MatrixXd &mat);
47 std::ostream&
operator<<(std::ostream& os,
const Eigen::VectorXd &vec);
48 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec);
49 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec);
53 namespace FilterUtilities
66 void appendPrefix(std::string tfPrefix, std::string &frameId);
71 #endif // ROBOT_LOCALIZATION_FILTER_UTILITIES_H 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].