Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "robot_localization/filter_utilities.h"
00034 #include "robot_localization/filter_common.h"
00035
00036 #include <string>
00037 #include <vector>
00038
00039 std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat)
00040 {
00041 os << "[";
00042
00043 int rowCount = static_cast<int>(mat.rows());
00044
00045 for (int row = 0; row < rowCount; ++row)
00046 {
00047 if (row > 0)
00048 {
00049 os << " ";
00050 }
00051
00052 for (int col = 0; col < mat.cols(); ++col)
00053 {
00054 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << mat(row, col);
00055 }
00056
00057 if (row < rowCount - 1)
00058 {
00059 os << "\n";
00060 }
00061 }
00062
00063 os << "]\n";
00064
00065 return os;
00066 }
00067
00068 std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec)
00069 {
00070 os << "[";
00071 for (int dim = 0; dim < vec.rows(); ++dim)
00072 {
00073 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec(dim);
00074 }
00075 os << "]\n";
00076
00077 return os;
00078 }
00079
00080 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec)
00081 {
00082 os << "[";
00083 for (size_t dim = 0; dim < vec.size(); ++dim)
00084 {
00085 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec[dim];
00086 }
00087 os << "]\n";
00088
00089 return os;
00090 }
00091
00092 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec)
00093 {
00094 os << "[";
00095 for (size_t dim = 0; dim < vec.size(); ++dim)
00096 {
00097 os << std::setiosflags(std::ios::left) << std::setw(3) << (vec[dim] ? "t" : "f");
00098 }
00099 os << "]\n";
00100
00101 return os;
00102 }
00103
00104 namespace RobotLocalization
00105 {
00106
00107 namespace FilterUtilities
00108 {
00109 void appendPrefix(std::string tfPrefix, std::string &frameId)
00110 {
00111
00112 if (!frameId.empty() && frameId.at(0) == '/')
00113 {
00114 frameId = frameId.substr(1);
00115 }
00116
00117 if (!tfPrefix.empty() && tfPrefix.at(0) == '/')
00118 {
00119 tfPrefix = tfPrefix.substr(1);
00120 }
00121
00122
00123 if (!tfPrefix.empty())
00124 {
00125 frameId = tfPrefix + "/" + frameId;
00126 }
00127 }
00128
00129 double clampRotation(double rotation)
00130 {
00131 while (rotation > PI)
00132 {
00133 rotation -= TAU;
00134 }
00135
00136 while (rotation < -PI)
00137 {
00138 rotation += TAU;
00139 }
00140
00141 return rotation;
00142 }
00143
00144 }
00145
00146 }