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 std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat)
00037 {
00038 os << "[";
00039
00040 int rowCount = static_cast<int>(mat.rows());
00041
00042 for (int row = 0; row < rowCount; ++row)
00043 {
00044 if (row > 0)
00045 {
00046 os << " ";
00047 }
00048
00049 for (int col = 0; col < mat.cols(); ++col)
00050 {
00051 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << mat(row, col);
00052 }
00053
00054 if (row < rowCount - 1)
00055 {
00056 os << "\n";
00057 }
00058 }
00059
00060 os << "]\n";
00061
00062 return os;
00063 }
00064
00065 std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec)
00066 {
00067 os << "[";
00068 for (int dim = 0; dim < vec.rows(); ++dim)
00069 {
00070 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec(dim);
00071 }
00072 os << "]\n";
00073
00074 return os;
00075 }
00076
00077 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec)
00078 {
00079 os << "[";
00080 for (size_t dim = 0; dim < vec.size(); ++dim)
00081 {
00082 os << std::setiosflags(std::ios::left) << std::setw(12) << std::setprecision(5) << vec[dim];
00083 }
00084 os << "]\n";
00085
00086 return os;
00087 }
00088
00089 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec)
00090 {
00091 os << "[";
00092 for (size_t dim = 0; dim < vec.size(); ++dim)
00093 {
00094 os << std::setiosflags(std::ios::left) << std::setw(3) << (vec[dim] ? "t" : "f");
00095 }
00096 os << "]\n";
00097
00098 return os;
00099 }
00100
00101 namespace RobotLocalization
00102 {
00103 namespace FilterUtilities
00104 {
00105 double clampRotation(double rotation)
00106 {
00107 while(rotation > PI)
00108 {
00109 rotation -= TAU;
00110 }
00111
00112 while(rotation < -PI)
00113 {
00114 rotation += TAU;
00115 }
00116
00117 return rotation;
00118 }
00119 }
00120 }