00001 /* 00002 * Copyright (c) 2014, 2015, 2016, Charles River Analytics, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions 00007 * are met: 00008 * 00009 * 1. Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * 2. Redistributions in binary form must reproduce the above 00012 * copyright notice, this list of conditions and the following 00013 * disclaimer in the documentation and/or other materials provided 00014 * with the distribution. 00015 * 3. Neither the name of the copyright holder nor the names of its 00016 * contributors may be used to endorse or promote products derived 00017 * from this software without specific prior written permission. 00018 * 00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00020 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00021 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00022 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00023 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00024 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00025 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00026 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00027 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00028 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00029 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00030 * POSSIBILITY OF SUCH DAMAGE. 00031 */ 00032 00033 #ifndef ROBOT_LOCALIZATION_FILTER_UTILITIES_H 00034 #define ROBOT_LOCALIZATION_FILTER_UTILITIES_H 00035 00036 #include <Eigen/Dense> 00037 00038 #include <iomanip> 00039 #include <iostream> 00040 #include <string> 00041 #include <vector> 00042 00043 #define FB_DEBUG(msg) if (getDebug()) { *debugStream_ << msg; } 00044 00045 // Handy methods for debug output 00046 std::ostream& operator<<(std::ostream& os, const Eigen::MatrixXd &mat); 00047 std::ostream& operator<<(std::ostream& os, const Eigen::VectorXd &vec); 00048 std::ostream& operator<<(std::ostream& os, const std::vector<size_t> &vec); 00049 std::ostream& operator<<(std::ostream& os, const std::vector<int> &vec); 00050 00051 namespace RobotLocalization 00052 { 00053 namespace FilterUtilities 00054 { 00055 00060 double clampRotation(double rotation); 00061 00066 void appendPrefix(std::string tfPrefix, std::string &frameId); 00067 00068 } // namespace FilterUtilities 00069 } // namespace RobotLocalization 00070 00071 #endif // ROBOT_LOCALIZATION_FILTER_UTILITIES_H