├── orthomap_viewer ├── package.xml ├── CMakeLists.txt ├── README.md └── src │ └── orthomap_viewer.cpp ├── hawkeye ├── package.xml ├── launch │ ├── hawkeye_rt.launch │ ├── sample_single.launch │ └── sample.launch ├── include │ ├── hawkeye_define.hpp │ ├── util │ │ ├── error.hpp │ │ ├── timer.hpp │ │ ├── arg.hpp │ │ ├── tf_messages.hpp │ │ ├── pose_stamped_helper.hpp │ │ └── pose_synchronize.hpp │ ├── hawkeye_base │ │ ├── histogram.hpp │ │ ├── node.hpp │ │ └── base.hpp │ └── map │ │ └── orthomap.hpp ├── CMakeLists.txt ├── config │ └── hawkeye.yaml ├── src │ ├── util │ │ ├── tf_messages.cpp │ │ └── pose_stamped_helper.cpp │ ├── hawkeye_base │ │ ├── config.cpp │ │ ├── node.cpp │ │ └── base.cpp │ ├── map │ │ ├── orthomap.cpp │ │ └── orthomap_read.cpp │ └── hawkeye_rt │ │ ├── hawkeye_node.cpp │ │ └── hawkeye_ws_node.cpp └── rviz │ ├── hawkeye.rviz │ └── hawkeye_ws.rviz ├── LICENSE └── README.md /orthomap_viewer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orthomap_viewer 4 | 1.0.0 5 | The orthomap_viewer package 6 | Kunihiro Ueda 7 | 8 | BSD 3-Clause 9 | 10 | catkin 11 | 12 | roscpp 13 | std_msgs 14 | nav_msgs 15 | pcl_ros 16 | sensor_msgs 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /hawkeye/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hawkeye 4 | 1.0.0 5 | Hawkeye package 6 | Kunihiro Ueda 7 | 8 | BSD 3-Clause 9 | 10 | catkin 11 | 12 | roscpp 13 | std_msgs 14 | nav_msgs 15 | pcl_ros 16 | sensor_msgs 17 | tf2 18 | tf2_ros 19 | tf2_eigen 20 | tf2_geometry_msgs 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /orthomap_viewer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(orthomap_viewer) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(Boost REQUIRED) 7 | find_package(PCL REQUIRED) 8 | find_package(catkin REQUIRED) 9 | find_package(OpenCV REQUIRED) 10 | 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | std_msgs 14 | nav_msgs 15 | pcl_ros 16 | sensor_msgs 17 | ) 18 | 19 | catkin_package( 20 | #INCLUDE_DIRS include 21 | DEPENDS PCL 22 | DEPENDS OpenCV 23 | ) 24 | 25 | include_directories(include 26 | ${catkin_INCLUDE_DIRS} 27 | ${PCL_INCLUDE_DIRS} 28 | ${OpenCV_INCLUDE_DIRS} 29 | ) 30 | 31 | add_executable(orthomap_viewer 32 | src/orthomap_viewer.cpp 33 | ) 34 | target_link_libraries(orthomap_viewer 35 | ${catkin_LIBRARIES} 36 | ${OpenCV_LIBRARIES} 37 | ) 38 | 39 | add_dependencies(orthomap_viewer ${catkin_EXPORTED_TARGETS}) 40 | -------------------------------------------------------------------------------- /hawkeye/launch/hawkeye_rt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2022, Map IV, Inc. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright notice, 9 | this list of conditions and the following disclaimer in the documentation 10 | and/or other materials provided with the distribution. 11 | * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | may be used to endorse or promote products derived from this software 13 | without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /hawkeye/launch/sample_single.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /hawkeye/include/hawkeye_define.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | 31 | namespace hawkeye 32 | { 33 | using PCPoint_t = pcl::PointXYZI; 34 | using PC_t = pcl::PointCloud; 35 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/launch/sample.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /hawkeye/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(hawkeye) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | find_package(Boost REQUIRED) 7 | find_package(PCL REQUIRED) 8 | find_package(catkin REQUIRED) 9 | find_package(OpenCV REQUIRED) 10 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 11 | link_directories(${PCL_LIBRARY_DIRS}) 12 | link_directories(${OpenCV_LIBRARY_DIRS}) 13 | add_definitions(${PCL_DEFINITIONS}) 14 | add_definitions(${OpenCV_DEFINITIONS}) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | roscpp 18 | std_msgs 19 | nav_msgs 20 | pcl_ros 21 | sensor_msgs 22 | tf2 23 | tf2_ros 24 | tf2_eigen 25 | tf2_geometry_msgs 26 | cv_bridge 27 | ) 28 | 29 | catkin_package( 30 | INCLUDE_DIRS include 31 | DEPENDS PCL 32 | DEPENDS OpenCV 33 | ) 34 | 35 | include_directories(include 36 | ${catkin_INCLUDE_DIRS} 37 | ${PCL_INCLUDE_DIRS} 38 | ${OpenCV_INCLUDE_DIRS} 39 | ${YAML_CPP_INCLUDE_DIRS} 40 | ${Boost_INCLUDE_DIRS} 41 | ) 42 | 43 | set(srcs 44 | src/map/orthomap.cpp 45 | src/map/orthomap_read.cpp 46 | src/util/tf_messages.cpp 47 | src/util/pose_stamped_helper.cpp 48 | ) 49 | 50 | set(hawkeye_srcs 51 | src/hawkeye_base/base.cpp 52 | src/hawkeye_base/node.cpp 53 | src/hawkeye_base/config.cpp 54 | ) 55 | 56 | # hawkeye_rt 57 | add_executable(hawkeye_rt 58 | ${srcs} 59 | ${hawkeye_srcs} 60 | src/hawkeye_rt/hawkeye_node.cpp 61 | ) 62 | target_link_libraries(hawkeye_rt 63 | ${catkin_LIBRARIES} 64 | ${PCL_LIBRARIES} 65 | ${OpenCV_LIBRARIES} 66 | ${YAML_CPP_LIBRARIES} 67 | ) 68 | add_dependencies(hawkeye_rt ${catkin_EXPORTED_TARGETS}) 69 | 70 | # hawkeye_rt_ws 71 | add_executable(hawkeye_rt_ws 72 | ${srcs} 73 | ${hawkeye_srcs} 74 | src/hawkeye_rt/hawkeye_ws_node.cpp 75 | ) 76 | target_link_libraries(hawkeye_rt_ws 77 | ${catkin_LIBRARIES} 78 | ${PCL_LIBRARIES} 79 | ${OpenCV_LIBRARIES} 80 | ${YAML_CPP_LIBRARIES} 81 | ) 82 | add_dependencies(hawkeye_rt_ws ${catkin_EXPORTED_TARGETS}) 83 | 84 | add_library(${PROJECT_NAME}_lib 85 | ${srcs} 86 | ${hawkeye_srcs} 87 | ) 88 | 89 | target_link_libraries(${PROJECT_NAME}_lib 90 | ${catkin_LIBRARIES} 91 | ${PCL_LIBRARIES} 92 | ${OpenCV_LIBRARIES} 93 | ${YAML_CPP_LIBRARIES} 94 | ) 95 | -------------------------------------------------------------------------------- /hawkeye/include/util/error.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace hawkeye 33 | { 34 | constexpr char ERROR_PREFIX[] = "\033[1;31mError: "; 35 | constexpr char ERROR_SUFFIX[] = "\033[0m"; 36 | 37 | inline void exitBadFile(const std::string& filename, const std::string& message = "") 38 | { 39 | if (message.empty()) 40 | { 41 | std::cerr << ERROR_PREFIX << "Bad file: " << std::quoted(filename) << ERROR_SUFFIX << std::endl; 42 | } 43 | else 44 | { 45 | std::cerr << ERROR_PREFIX << "Bad file " << std::quoted(filename) << ": " << message << ERROR_SUFFIX << std::endl; 46 | } 47 | exit(2); 48 | } 49 | 50 | inline void exitOpenError(const std::string& filename) 51 | { 52 | std::cerr << ERROR_PREFIX << std::quoted(filename) << " can not be opened." << ERROR_SUFFIX << std::endl; 53 | exit(2); 54 | } 55 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/include/util/timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | namespace hawkeye 31 | { 32 | namespace chrono = std::chrono; 33 | 34 | class Stopwatch 35 | { 36 | public: 37 | Stopwatch() : counter_{ chrono::system_clock::now() }, latest_duration_{ 0 } {}; 38 | Stopwatch(const Stopwatch& sw) : counter_{ sw.counter_ }, latest_duration_{ sw.latest_duration_ } {}; 39 | 40 | inline double count() 41 | { 42 | auto end = chrono::system_clock::now(); 43 | latest_duration_ = chrono::duration_cast(end - counter_).count() / 1000.; 44 | return latest_duration_; 45 | } 46 | inline void reset() 47 | { 48 | counter_ = chrono::system_clock::now(); 49 | } 50 | inline double get() 51 | { 52 | return latest_duration_; 53 | } 54 | 55 | private: 56 | chrono::system_clock::time_point counter_; 57 | double latest_duration_; 58 | }; 59 | 60 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/config/hawkeye.yaml: -------------------------------------------------------------------------------- 1 | hawkeye: 2 | template_size: [192, 192] # size of template image 3 | map_size: [256, 256] # ROI size of ortho image map for template matching 4 | # the size of histogram filter is map_size - template_size + [1,1] 5 | error_rate: 0.01 # alpha in the paper 6 | coeff_diminish: 0.996 # beta in the paper 7 | coeff_weight: 0.07 # k in the paper 8 | coeff_negative: 0.025 # b in the paper 9 | coeff_gain: 0.3 # g in the paper 10 | center_shift_threshold: 0.5 # The threshold for judgement whether the histogram center should shift. Set the real number in [0,1] closed set. 11 | # If 0, the center shifts by any non-zero difference between the center and the estimated pose. 12 | # If 1, the center shifts when the center is out of the histogram range, which means the center never shifts. 13 | edge_copy_shift: false # Parameter for hawkeye_rt 14 | # When the histogram center shifts, points on the histogram edge are newly created. 15 | # If true, those points are initialized by the nearby point. If false, those are initialized by zero. 16 | histogram_weight: 1 # Parameter for hawkeye_rt_ws 17 | # It configures how the histogram weight affects estimation. Set the real number in [0,1] closed set. 18 | # The histogram weight means the upper limmit of each histogram point. This is calculated as histogram, using the the upper limmit of the matching result instead of the match result. 19 | # It is effective when the histogram points newly created at the center shift are smaller due to a lack of accumulation. 20 | # If 0, the weight is ignored. If 1, the values divided by the histogram weight are used in estimation. 21 | lidar_accumulate: # For making template image, LiDAR data are accumulated to satisfy the following two conditions. 22 | max_count: 30 # Size condition: The count of LiDAR data does not exceed the number. If it exceeds while meeting the second condition, one of the closest continuous pair of LiDAR data is removed. 23 | max_length: 20 # Length threshold [m]: If a LiDAR data which position by Eagleye is farther than the threshold from the current pose is removed. 24 | intensity_accumulate_threshold_min: 0 # Correct outlier points of the measured point cloud in intensity to within a specified range. 25 | intensity_accumulate_threshold_max: 0.98 # The upper and lower limits of the range are set as the cumulative distribution of intensity. 26 | stop_threshold: 0.05 # While the displacement from the previous step is less than this parameter, histogram update are stopped. 27 | 28 | # TF from Eagleye to lidar 29 | gnss: 30 | tf_x: 0.0 31 | tf_y: 0.0 32 | tf_z: 0.0 33 | tf_roll: 0.0 34 | tf_pitch: 0.0 35 | tf_yaw: 0.0 -------------------------------------------------------------------------------- /hawkeye/include/hawkeye_base/histogram.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include "hawkeye_define.hpp" 35 | 36 | namespace hawkeye::hawkeye_base 37 | { 38 | using scale_t = double; 39 | using weight_t = double; 40 | using h_image_t = cv::Mat; // for histogram : CV_64F 41 | using w_image_t = cv::Mat; // for weight : CV_64F 42 | using o_image_t = cv::Mat; // for output : CV_8U 43 | 44 | using template_image_t = std::tuple; 45 | using match_result_t = std::tuple; 46 | using weighted_histogram_t = std::tuple; 47 | 48 | using pose_PC_t = std::pair; 49 | 50 | struct Hawkeye_CopyShiftMode 51 | { 52 | static constexpr bool is_weighted_ = false; 53 | }; 54 | 55 | struct Hawkeye_WeightedShiftMode 56 | { 57 | static constexpr bool is_weighted_ = true; 58 | }; 59 | 60 | using histogram_t = std::tuple; 61 | 62 | } // namespace hawkeye::hawkeye_base -------------------------------------------------------------------------------- /hawkeye/include/util/arg.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | template 31 | struct argLoader 32 | { 33 | constexpr static bool ty_func_ = std::is_function<_Ty>::value; 34 | using func_type_ = std::conditional_t; 35 | 36 | argLoader(int argc, char** argv, _Ty&& error_exiter) 37 | : argc_{ argc }, argv_{ argv }, i_{ 0 }, error_exiter_{ std::forward<_Ty&&>(error_exiter) } {}; 38 | 39 | ~argLoader() 40 | { 41 | end(); 42 | } 43 | 44 | bool valid() 45 | { 46 | return i_ < argc_; 47 | } 48 | 49 | int left() 50 | { 51 | return argc_ - i_; 52 | } 53 | 54 | void drop() 55 | { 56 | if (!valid()) 57 | err(); 58 | ++i_; 59 | } 60 | 61 | char* top() 62 | { 63 | if (!valid()) 64 | err(); 65 | return argv_[i_]; 66 | } 67 | 68 | char* pop() 69 | { 70 | if (!valid()) 71 | err(); 72 | return argv_[i_++]; 73 | } 74 | 75 | void end() 76 | { 77 | if (valid()) 78 | err(); 79 | } 80 | 81 | void err() 82 | { 83 | error_exiter_("Invalid arguments. argc: " + std::to_string(argc_)); 84 | } 85 | 86 | private: 87 | int argc_; 88 | char** argv_; 89 | 90 | func_type_ error_exiter_; 91 | int i_; 92 | }; -------------------------------------------------------------------------------- /hawkeye/include/util/tf_messages.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hawkeye 38 | { 39 | template 40 | void tf2_print(Output_& out, const std::string& name, const tf2::Transform& tf) 41 | { 42 | out << name << " :" << std::endl; 43 | out << tf.getOrigin().x() << ' '; 44 | out << tf.getOrigin().y() << ' '; 45 | out << tf.getOrigin().z() << std::endl; 46 | out << tf.getRotation().x() << ' '; 47 | out << tf.getRotation().y() << ' '; 48 | out << tf.getRotation().z() << ' '; 49 | out << tf.getRotation().w() << std::endl; 50 | } 51 | 52 | class TfTrajPublisher 53 | { 54 | public: 55 | TfTrajPublisher(ros::NodeHandle& node) : node_{ node } 56 | { 57 | } 58 | 59 | bool remove(const std::string& frame_target); 60 | bool reset(const std::string& frame_target); 61 | 62 | bool addNewPublisher(const std::string& frame_target, const std::string& topic_name, const std::string& frame_base); 63 | 64 | void broadcastStatic(const std::string& frame_target, const tf2::Transform& tf_tf2, const ros::Time& time, 65 | const std::string& frame_base); 66 | bool broadcast(const std::string& frame_target, const tf2::Transform& tf_tf2, const ros::Time& time); 67 | 68 | private: 69 | ros::NodeHandle& node_; 70 | 71 | using traj_t = std::pair; 72 | tf2_ros::TransformBroadcaster dynamic_broadcaster_; 73 | tf2_ros::StaticTransformBroadcaster static_broadcaster_; 74 | std::map markers_; 75 | }; 76 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/include/hawkeye_base/node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | #include "hawkeye_define.hpp" 31 | #include "hawkeye_base/histogram.hpp" 32 | #include "map/orthomap.hpp" 33 | 34 | namespace hawkeye::hawkeye_base 35 | { 36 | struct HawkeyeConfig 37 | { 38 | cv::Size template_size_; 39 | cv::Size map_size_; 40 | double error_rate_; 41 | double coeff_diminish_; 42 | double coeff_weight_; 43 | double coeff_negative_; 44 | double coeff_gain_; 45 | size_t lidar_accumulate_max_count_; 46 | double lidar_accumulate_max_length_; 47 | tf2::Transform lidar_pose_; 48 | double center_shift_threshold_; 49 | double histogram_weight_; // for Hawkeye_WeightedShiftMode 50 | bool edge_copy_shift_; // for Hawkeye_CopyShiftMode 51 | double intensity_accumulate_threshold_min_; 52 | double intensity_accumulate_threshold_max_; 53 | double stop_threshold_; 54 | 55 | static HawkeyeConfig defaultConfig(); 56 | template 57 | void readYamlConfig(const std::string& yaml_filename); 58 | }; 59 | 60 | template 61 | class Hawkeye 62 | { 63 | public: 64 | using Config = HawkeyeConfig; 65 | 66 | static Config readYamlConfig(const std::string& yaml_filename); 67 | 68 | Hawkeye(double scale, const Config& config = Config::defaultConfig()); 69 | 70 | tf2::Transform shift(const tf2::Transform& pose) const; 71 | 72 | const tf2::Transform& getLidarTf() const; 73 | 74 | tf2::Transform update(const tf2::Transform& pose, const PC_t::Ptr pc_ptr, OrthoMap& map, bool print_info = true); 75 | double getDuration() const 76 | { 77 | return essential_time_; 78 | }; 79 | 80 | tf2::Transform getHistogramPeak() const; 81 | tf2::Transform getShftedCenter() const; 82 | tf2::Transform getMatchPeak() const; 83 | 84 | cv::Mat getTemplateImage() const; 85 | cv::Mat getMatchImage() const; 86 | cv::Mat getSubmapImage() const; 87 | cv::Mat getWeightImage() const; 88 | 89 | const visualization_msgs::MarkerArray& getHistogramMarkers(const std_msgs::Header& header, bool as_array = false); 90 | 91 | private: 92 | void adjustPCs(); 93 | 94 | private: 95 | histogram_t histogram_; 96 | histogram_t shifted_histogram_; 97 | template_image_t template_image_; 98 | weighted_histogram_t weighted_histogram_; 99 | match_result_t match_result_; 100 | cv::Mat submap_; 101 | 102 | std::list pc_list_; 103 | 104 | cv::Moments moments_; 105 | tf2::Vector3 offset_; 106 | tf2::Vector3 center_shift_; 107 | tf2::Vector3 average_; 108 | tf2::Transform odometry_; 109 | tf2::Transform histogram_center_; 110 | 111 | double essential_time_; 112 | size_t counter_; 113 | 114 | visualization_msgs::MarkerArray marker_array_; 115 | 116 | const Config config_; 117 | const double histogram_weight_; 118 | }; 119 | 120 | extern template class Hawkeye; 121 | extern template class Hawkeye; 122 | 123 | } // namespace hawkeye::hawkeye_base -------------------------------------------------------------------------------- /hawkeye/include/util/pose_stamped_helper.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | namespace hawkeye 36 | { 37 | template 38 | using TimeOrder_t = std::map; 39 | 40 | inline bool comparePS(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) 41 | { 42 | return p1.header.stamp < p2.header.stamp; 43 | } 44 | inline bool equalPS(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) 45 | { 46 | return p1.header.stamp == p2.header.stamp; 47 | } 48 | 49 | enum class find_result 50 | { 51 | NONE = 0, 52 | FOUND_SINGLE = 1 << 0, 53 | FOUND_MULTIPLE = 1 << 1, 54 | INTERPOLATABLE = 1 << 2, 55 | SMALLER = 1 << 3, 56 | LARGER = 1 << 4, 57 | EMPTY = 1 << 5, 58 | 59 | FOUND = FOUND_SINGLE | FOUND_MULTIPLE, 60 | NOT_FOUND = INTERPOLATABLE | SMALLER | LARGER | EMPTY, 61 | 62 | IP_FOUND = FOUND_SINGLE | FOUND_MULTIPLE | INTERPOLATABLE, 63 | IP_NOT_FOUND = SMALLER | LARGER | EMPTY, 64 | }; 65 | 66 | inline find_result operator&(const find_result l, const find_result r) 67 | { 68 | return find_result(int(l) & int(r)); 69 | } 70 | inline find_result operator|(const find_result l, const find_result r) 71 | { 72 | return find_result(int(l) | int(r)); 73 | } 74 | 75 | std::tuple::const_iterator, 76 | typename std::vector::const_iterator> 77 | findBoundWithResult(const std::vector& traj_csv, const ros::Time& stamp); 78 | std::tuple::const_iterator, 79 | typename TimeOrder_t::const_iterator> 80 | findBoundWithResult(const TimeOrder_t& traj_csv, const ros::Time& stamp); 81 | 82 | inline double getInterpolateRate(const ros::Time& target, const ros::Time& before, const ros::Time& after) 83 | { 84 | double passed = (target - before).toNSec(); 85 | double range = (after - before).toNSec(); 86 | return passed / range; 87 | } 88 | 89 | tf2::Transform interpolatePose(const tf2::Transform& before, const tf2::Transform& after, double t); 90 | tf2::Transform interpolatePose(const geometry_msgs::Pose& before, const geometry_msgs::Pose& after, double t); 91 | 92 | std::optional findPose(const std::vector& traj_csv, size_t& prev, 93 | const ros::Time& stamp); 94 | 95 | std::optional findPoseInterpolated(const std::vector& traj_csv, 96 | const ros::Time& stamp); 97 | std::optional findPoseInterpolated(const TimeOrder_t& traj_csv, 98 | const ros::Time& stamp); 99 | 100 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/src/util/tf_messages.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "util/tf_messages.hpp" 27 | 28 | namespace hawkeye 29 | { 30 | bool TfTrajPublisher::addNewPublisher(const std::string& frame_target, const std::string& topic_name, 31 | const std::string& frame_base) 32 | { 33 | if (markers_.find(frame_target) != markers_.end()) 34 | { 35 | return false; 36 | } 37 | auto [iter, b] = 38 | markers_.insert(std::make_pair(frame_target, traj_t{ node_.advertise(topic_name, 1, true), {} })); 39 | auto& path = iter->second.second; 40 | path.header.frame_id = frame_base; 41 | std::cout << std::quoted(frame_target) << "( from " << std::quoted(frame_base) << " ) has been added." << std::endl; 42 | return true; 43 | } 44 | 45 | bool TfTrajPublisher::remove(const std::string& frame_target) 46 | { 47 | auto ret = markers_.erase(frame_target); 48 | return ret != 0; 49 | } 50 | 51 | bool TfTrajPublisher::reset(const std::string& frame_target) 52 | { 53 | auto iter = markers_.find(frame_target); 54 | if (iter == markers_.end()) 55 | { 56 | return false; 57 | } 58 | iter->second.second.poses.clear(); 59 | return true; 60 | } 61 | 62 | void TfTrajPublisher::broadcastStatic(const std::string& frame_target, const tf2::Transform& tf_tf2, 63 | const ros::Time& time, const std::string& frame_base) 64 | { 65 | geometry_msgs::TransformStamped tf_gm; 66 | tf2::convert(tf_tf2, tf_gm.transform); 67 | tf_gm.header.stamp = time; 68 | tf_gm.header.seq = 0; 69 | tf_gm.header.frame_id = frame_base; 70 | tf_gm.child_frame_id = frame_target; 71 | static_broadcaster_.sendTransform(tf_gm); 72 | std::cout << "static " << std::quoted(frame_target) << "( from " << std::quoted(frame_base) << ')' 73 | << " has been broadcasted." << std::endl; 74 | } 75 | 76 | bool TfTrajPublisher::broadcast(const std::string& frame_target, const tf2::Transform& tf_tf2, const ros::Time& time) 77 | { 78 | auto iter = markers_.find(frame_target); 79 | if (iter == markers_.end()) 80 | { 81 | std::cout << std::quoted(frame_target) << " cannot be broadcasted." << std::endl; 82 | return false; 83 | } 84 | geometry_msgs::TransformStamped tf_gm; 85 | tf2::convert(tf_tf2, tf_gm.transform); 86 | tf_gm.header.stamp = time; 87 | tf_gm.header.seq = 0; 88 | tf_gm.header.frame_id = iter->second.second.header.frame_id; 89 | tf_gm.child_frame_id = frame_target; 90 | dynamic_broadcaster_.sendTransform(tf_gm); 91 | 92 | geometry_msgs::PoseStamped pose; 93 | pose.pose.position.x = tf_gm.transform.translation.x; 94 | pose.pose.position.y = tf_gm.transform.translation.y; 95 | pose.pose.position.z = tf_gm.transform.translation.z; 96 | pose.pose.orientation = tf_gm.transform.rotation; 97 | pose.header = tf_gm.header; 98 | auto& path = iter->second.second; 99 | path.poses.push_back(pose); 100 | path.header = tf_gm.header; 101 | iter->second.first.publish(path); 102 | std::cout << std::quoted(frame_target) << " has been broadcasted." << std::endl; 103 | return true; 104 | } 105 | 106 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/include/hawkeye_base/base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | #include "hawkeye_define.hpp" 31 | #include "hawkeye_base/histogram.hpp" 32 | 33 | namespace hawkeye::hawkeye_base 34 | { 35 | template_image_t generateTemplateImage(const std::list& pc_list, const tf2::Transform& lidar_pose, 36 | cv::Size size, scale_t scale, double accum_min = 0, double accum_max = 1); 37 | template_image_t generateTemplateImage(const std::list& pc_list, const tf2::Transform& lidar_pose, 38 | cv::Size size, scale_t scale, const tf2::Vector3& histogram_center, 39 | double accum_min = 0, double accum_max = 1); 40 | 41 | template 42 | histogram_t generateInitialDistribution(cv::Size template_size, cv::Size map_size, scale_t scale); 43 | 44 | template 45 | histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, size_t kernel_size, 46 | double error_coeff); 47 | template 48 | histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, double error_coeff); 49 | 50 | match_result_t matchTemplate(const cv::Mat& map, scale_t map_scale, const template_image_t& template_image, 51 | bool use_mask = false); 52 | 53 | template 54 | histogram_t updateDistribution(const histogram_t& prior, const match_result_t& match_result, double diminish, 55 | double weight, double negative); 56 | 57 | //! @param weight_coeff histogram is divided by (1 - weight_coeff) + weight_coeff * weight_matrix if HawkeyeMode is 58 | //! Hawkeye_WeightedShiftMode 59 | template 60 | weighted_histogram_t getWeightHistogram(const histogram_t& histogram, double weight_coeff = 0); 61 | 62 | cv::Moments getMoments(const weighted_histogram_t& histogram); 63 | cv::Moments getMoments(const match_result_t& histogram); 64 | 65 | template 66 | inline cv::Moments getMoments(const histogram_t& histogram, double weight_coeff = 0) 67 | { 68 | return getMoments(getWeightHistogram(histogram, weight_coeff)); 69 | } 70 | 71 | tf2::Vector3 average(const weighted_histogram_t& histogram, const cv::Moments& moments); 72 | tf2::Vector3 average(const match_result_t& histogram, const cv::Moments& moments); 73 | 74 | tf2::Vector3 updateOffset(const tf2::Vector3& prev, const tf2::Vector3& next, double coeff); 75 | 76 | template 77 | std::tuple shiftCenter(const histogram_t& histogram, 78 | const tf2::Vector3& center, const tf2::Vector3& offset, 79 | double threshold_rate, bool edge_copy_shift = true); 80 | 81 | void setHistogramMarkers(visualization_msgs::MarkerArray& ma, const weighted_histogram_t& histogram, 82 | const tf2::Transform& odometry, const std_msgs::Header& header, bool as_array = false); 83 | 84 | cv::Mat convertHistogram2CVU8C1(const cv::Mat& histogram); 85 | cv::Mat convertHistogram2CVU8C1(const weighted_histogram_t& histogram); 86 | cv::Mat convertHistogram2CVU8C1(const match_result_t& histogram); 87 | 88 | } // namespace hawkeye::hawkeye_base -------------------------------------------------------------------------------- /hawkeye/src/hawkeye_base/config.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | 28 | #include "hawkeye_base/node.hpp" 29 | #include "hawkeye_base/base.hpp" 30 | #include "util/timer.hpp" 31 | #include "util/error.hpp" 32 | 33 | #include 34 | 35 | namespace hawkeye::hawkeye_base 36 | { 37 | HawkeyeConfig HawkeyeConfig::defaultConfig() 38 | { 39 | return { { 192, 192 }, 40 | { 256, 256 }, 41 | 1. / 100, 42 | 0.990, 43 | 0.07, 44 | 0.6, 45 | 0.3, 46 | 30, 47 | 20., 48 | tf2::Transform::getIdentity(), 49 | 0.5, 50 | 1, 51 | true, 52 | 0, 53 | 1, 54 | 0.05 }; 55 | } 56 | 57 | template 58 | void getIfValid(const YAML::Node& node, T& val) 59 | { 60 | if (node.IsDefined()) 61 | { 62 | val = node.as(); 63 | } 64 | } 65 | template <> 66 | void getIfValid(const YAML::Node& node, cv::Size& val) 67 | { 68 | if (node.IsDefined()) 69 | { 70 | std::vector tmp = node.as>(); 71 | if (tmp.size() == 2) 72 | { 73 | val.height = tmp[0]; 74 | val.width = tmp[1]; 75 | } 76 | } 77 | } 78 | 79 | template 80 | void HawkeyeConfig::readYamlConfig(const std::string& yaml_filename) 81 | { 82 | try 83 | { 84 | YAML::Node whole = YAML::LoadFile(yaml_filename); 85 | YAML::Node hawkeye_yaml = whole["hawkeye"]; 86 | getIfValid(hawkeye_yaml["template_size"], template_size_); 87 | getIfValid(hawkeye_yaml["map_size"], map_size_); 88 | getIfValid(hawkeye_yaml["error_rate"], error_rate_); 89 | getIfValid(hawkeye_yaml["coeff_diminish"], coeff_diminish_); 90 | getIfValid(hawkeye_yaml["coeff_weight"], coeff_weight_); 91 | getIfValid(hawkeye_yaml["coeff_negative"], coeff_negative_); 92 | getIfValid(hawkeye_yaml["coeff_gain"], coeff_gain_); 93 | getIfValid(hawkeye_yaml["lidar_accumulate"]["max_count"], lidar_accumulate_max_count_); 94 | getIfValid(hawkeye_yaml["lidar_accumulate"]["max_length"], lidar_accumulate_max_length_); 95 | getIfValid(hawkeye_yaml["center_shift_threshold"], center_shift_threshold_); 96 | if constexpr (HawkeyeMode::is_weighted_) 97 | { 98 | getIfValid(hawkeye_yaml["histogram_weight"], histogram_weight_); 99 | } 100 | else 101 | { 102 | getIfValid(hawkeye_yaml["edge_copy_shift"], edge_copy_shift_); 103 | } 104 | { 105 | double x = 0, y = 0, z = 0; 106 | double roll = 0, pitch = 0, yaw = 0; 107 | getIfValid(whole["gnss"]["tf_x"], x); 108 | getIfValid(whole["gnss"]["tf_y"], y); 109 | getIfValid(whole["gnss"]["tf_z"], z); 110 | getIfValid(whole["gnss"]["tf_roll"], roll); 111 | getIfValid(whole["gnss"]["tf_pitch"], pitch); 112 | getIfValid(whole["gnss"]["tf_yaw"], yaw); 113 | lidar_pose_.setOrigin({ x, y, z }); 114 | lidar_pose_.getBasis().setRPY(roll, pitch, yaw); 115 | } 116 | getIfValid(hawkeye_yaml["intensity_accumulate_threshold_max"], intensity_accumulate_threshold_max_); 117 | getIfValid(hawkeye_yaml["intensity_accumulate_threshold_min"], intensity_accumulate_threshold_min_); 118 | getIfValid(hawkeye_yaml["stop_threshold"], stop_threshold_); 119 | } 120 | catch (YAML::Exception& e) 121 | { 122 | exitBadFile(yaml_filename, e.what()); 123 | } 124 | } 125 | template void HawkeyeConfig::readYamlConfig(const std::string& yaml_filename); 126 | template void HawkeyeConfig::readYamlConfig(const std::string& yaml_filename); 127 | 128 | } // namespace hawkeye::hawkeye_base -------------------------------------------------------------------------------- /hawkeye/include/map/orthomap.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "hawkeye_define.hpp" 35 | 36 | namespace hawkeye 37 | { 38 | inline int divFloor(int dividend, size_t divisor) 39 | { 40 | if (dividend < 0) 41 | { 42 | return ~(~dividend / divisor); 43 | } 44 | return dividend / divisor; 45 | }; 46 | 47 | class OrthoMap 48 | { 49 | public: 50 | OrthoMap(const std::string& orthomap_filename); 51 | 52 | inline tf2::Transform center() const 53 | { 54 | tf2::Transform center_; 55 | center_.setIdentity(); 56 | center_.setOrigin(tf2::Vector3(center_x_, center_y_, 0)); 57 | return center_; 58 | } 59 | 60 | void exportImage(const std::string& filename); 61 | 62 | inline double getScale() const 63 | { 64 | return scale_; 65 | } 66 | inline bool getScaled() const 67 | { 68 | return scaling_.scaled_; 69 | } 70 | inline std::pair clampRange() const 71 | { 72 | return { scaling_.threshold_min_, scaling_.threshold_max_ }; 73 | }; 74 | inline bool getClassified() const 75 | { 76 | return classify_emptiness_; 77 | }; 78 | 79 | const cv::Mat getImage(const tf2::Vector3& pos, int shift_x = 0, int shfit_y = 0) const; 80 | 81 | cv::Mat getImageRange(const tf2::Vector3& center, size_t range_x, size_t range_y) const; 82 | 83 | bool ckeckGridLatest(const tf2::Vector3& pos, size_t range) const; 84 | 85 | const nav_msgs::OccupancyGrid& getGridAround(const std_msgs::Header& header, const tf2::Vector3& pos, 86 | size_t range = 1); 87 | 88 | private: 89 | inline uchar at(tf2::Vector3 pos) const 90 | { 91 | auto [w, h] = getPosOfWholeGrid(pos); 92 | if (!checkValid(w, h)) 93 | { 94 | return 0; 95 | } 96 | auto [map_id, w2, h2] = convertPosToImage(w, h); 97 | if (maps_[map_id].empty()) 98 | { 99 | return 0; 100 | } 101 | return maps_[map_id].at(cv::Point(w2, h2)); 102 | } 103 | 104 | inline std::pair getPosOfWholeGrid(const tf2::Vector3& pos) const 105 | { 106 | int w = std::floor(pos.x() / scale_ + map_width_ * 0.5); 107 | int h = map_height_ - 1 - std::floor(pos.y() / scale_ + map_height_ * 0.5); 108 | return { w, h }; 109 | } 110 | 111 | inline std::pair getPosOfImageGrid(const tf2::Vector3& pos) const 112 | { 113 | auto [w, h] = getPosOfWholeGrid(pos); 114 | return { divFloor(w, image_width_), divFloor(h, image_height_) }; 115 | } 116 | 117 | inline std::pair getPosOfImageGrid(int w, int h) const 118 | { 119 | return { divFloor(w, image_width_), divFloor(h, image_height_) }; 120 | } 121 | 122 | inline bool checkValid(int w, int h) const 123 | { 124 | return (w >= 0 && w < map_width_ && h >= 0 && h < map_height_); 125 | } 126 | 127 | inline std::tuple convertPosToImage(int w, int h) const 128 | { 129 | size_t map_id = (h / image_height_) * map_div_x_ + (w / image_width_); 130 | size_t w2 = w % image_width_; 131 | size_t h2 = h % image_height_; 132 | return { map_id, w2, h2 }; 133 | } 134 | 135 | private: 136 | std::string orthomap_filename_; 137 | std::vector maps_; 138 | size_t image_width_, image_height_; 139 | size_t map_div_x_, map_div_y_; 140 | size_t map_height_, map_width_; 141 | double scale_; 142 | double center_x_; 143 | double center_y_; 144 | struct 145 | { 146 | bool scaled_; 147 | double threshold_min_; 148 | double threshold_max_; 149 | } scaling_; 150 | bool classify_emptiness_; 151 | struct 152 | { 153 | nav_msgs::OccupancyGrid data_; 154 | std::optional> last_info_; 155 | } og_map_; 156 | }; 157 | 158 | } // namespace hawkeye -------------------------------------------------------------------------------- /orthomap_viewer/README.md: -------------------------------------------------------------------------------- 1 | # Ortho Map Viewer 2 | Updated (2022/05/11) 3 | 4 | Publish topic of ortho image map for rivz. 5 | 6 | ## Usage 7 | 8 | ``` 9 | $ rosrun orthomap_viewer orthomap_viewer [ -t ] [ -p ] [ -f ] 10 | ``` 11 | 12 | ### Argument 13 | * ORTHOMAP_FILE : The orthomap file. 14 | * ***The relative path to the ortho images must not be changed.*** 15 | * ***The size of the whole pixels must be somewhat smaller than 1GB if you want to use rviz. The topic of the size larger than 1GB can not be shown in rviz.*** 16 | * the resolution of the large file can be reduced by running ortho_image with a smaller scale in the yaml file. 17 | * TOPIC_NAME : The topic name of the map. 18 | * The default name is `ortho_map`. 19 | * PCD_XYZI_FILE : PCD with XYZI points can be published simultaneously. 20 | * FRAME_NAME : The target frame name of the map. 21 | 22 | ## Format of Orthomap File 23 | Orthomap file is a text file containing information about the ortho images and their placement. 24 | ### Sample 25 | ``` 26 | orthomap 3 27 | "map_" 28 | "_i.tif" 29 | true 30 | 0.125 31 | -18874.938 32 | -93250.062 33 | 6 34 | 5 35 | 4000 36 | 3000 37 | 18 38 | true 1 255 true 39 | 0 1 1 1 1 0 40 | 0 0 0 1 1 0 41 | 0 1 0 0 1 1 42 | 1 1 1 1 1 1 43 | 0 1 1 1 0 0 44 | ``` 45 | 46 | This file means the 18 grayscale image files `map_00_i.tif` ... `map_17_i.tif` in the same directory as this file, which are 3000 pixels hight and 4000 pixels width, constitute the map as the following table. 47 | The table shows the north on top, the coordinate of the left top pixel in the whole map is (-18874.938, -93250.062) in a coordinate system and the scale of the map is 0.125 m/pixel. 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 |

	map_00_i.tif	map_01_i.tif	map_02_i.tif	map_03_i.tif
			map_04_i.tif	map_05_i.tif
	map_06_i.tif			map_07_i.tif	map_08_i.tif
map_09_i.tif	map_10_i.tif	map_11_i.tif	map_12_i.tif	map_13_i.tif	map_14_i.tif
	map_15_i.tif	map_16_i.tif	map_17_i.tif

91 | 92 | ### Format 93 | Line breaks in the samle above are meaningless; each element should be separated by one or more whitespace characters. 94 | 95 | | Number | In the sample | Explanation | 96 | | -----: | :-----------: | :---------- | 97 | | 1 | `orthomap` | Format identifier. It must be `orthomap`. | 98 | | 2 | `3` | Format version. This version is 3. Only the latest version is supported. | 99 | | 3 | `"map_"` | Image filename prefix. It must be in double quotes and can be `""`. See the image names in the sample. | 100 | | 4 | `"_i.tif"` | Image filename suffix. It must be in double quotes and can be `""`. See the image names in the sample.| 101 | | 5 | `true` | Whether to fill the enumerating numbers in file names with zeros and make the number width the same length. It must be `true` or `false`. | 102 | | 6 | `0.125` | Map scale. The images must be north at the top and northward and eastward scales must be the same. The unit is m/pixel. | 103 | | 7 - 8 | `-18874.938 -93250.062` | X and y coordinates of the center of the left top pixel in the whole images in a coordinate system. 7th and 8th elements are same as the 5th element of the world files for the left-most image and the 6th element of those for the top-most image. The coordinate system used in the map cannot be determined from this format. | 104 | | 9 - 10 | `6 5` | Width and height of the image table. | 105 | | 11 - 12 | `4000 3000` | Width and height of each image file. | 106 | | 13 | `18` | The number of image files. | 107 | | 14 - 17 | `true 1 255 true` | About the pixel color. See [Color section](#color). If 14th element is `false`, then the 15th to 17th elements must be omitted. | 108 | | 18 - | `0 1 1 1 1 0`
...
`0 1 1 1 0 0` | Whether an image exists in each position of the image table. The numbers are made into the table in row-major order and each element indicates existence and non-existence by 1 and 0. See the sample above. | 109 | 110 | ### Image Format 111 | The images must consist of a single 8-bit channel. 112 | They are expected to be PNG or TIFF, but other formats may also work. 113 | 114 | #### Color 115 | Since the color of each pixel must be represented in 256 steps, the color in the images may not same scale as the real color. 116 | If the scales are different, set 14th element `true` and set 15th to 17th elements as following. Otherwise, set 14th element `false` and omit 15th to 17th elements. 117 | 118 | * 15th element : The real color that `0`(`1` if 17th element is `true`) in the image shows. 119 | * 16th element : The real color that `255` in the image shows. 120 | * 17th element : Whether `0` is considered blank. If `0` is considered blank, set `true`. Otherwise, set `false`. 121 | 122 | The following table is an example of 14th to 17th elements. 123 | 124 | | 14th | 15th | 16th | 17th | Real color of `0` | Real color of `1` | Real color of `2` | Real color of `255` | 125 | | :--: | :--: | :--: | :--: | :---------------: | :---------------: | :---------------: | :-----------------: | 126 | | `false` | - | - | - | 0 | 1 | 2 | 255 | 127 | | `true` | 0 | 255 | `false` | 0 | 1 | 2 | 255 | 128 | | `true` | 0 | 1000 | `false` | 0 | 3.92 | 7.84 | 1000 | 129 | | `true` | -100 | 100 | `false` | -100 | -99.2 | -98.4 | 100 | 130 | | `true` | 1 | 255 | `true` | (emptiness) | 1 | 2 | 255 | 131 | | `true` | 0 | 1000 | `true` | (emptiness) | 0 | 3.94 | 1000 | -------------------------------------------------------------------------------- /hawkeye/src/util/pose_stamped_helper.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "util/pose_stamped_helper.hpp" 27 | 28 | #include 29 | #include 30 | 31 | namespace hawkeye 32 | { 33 | template 34 | inline find_result checkResult(const Iter_& bg, const Iter_& ed, const Iter_& lb, const Iter_& ub) 35 | { 36 | if (ub == std::next(lb)) 37 | { 38 | return find_result::FOUND_SINGLE; 39 | } 40 | else if (ub != lb) 41 | { 42 | return find_result::FOUND_MULTIPLE; 43 | } 44 | else if (bg == ed) 45 | { 46 | return find_result::EMPTY; 47 | } 48 | else if (lb == bg) 49 | { 50 | return find_result::SMALLER; 51 | } 52 | else if (lb == ed) 53 | { 54 | return find_result::SMALLER; 55 | } 56 | else 57 | { 58 | return find_result::INTERPOLATABLE; 59 | } 60 | } 61 | 62 | std::tuple::const_iterator, 63 | typename std::vector::const_iterator> 64 | findBoundWithResult(const std::vector& traj_csv, const ros::Time& stamp) 65 | { 66 | geometry_msgs::PoseStamped tmp; 67 | tmp.header.stamp = stamp; 68 | auto lb = std::lower_bound(traj_csv.begin(), traj_csv.end(), tmp, comparePS); 69 | auto ub = std::upper_bound(traj_csv.begin(), traj_csv.end(), tmp, comparePS); 70 | find_result result = checkResult(traj_csv.cbegin(), traj_csv.cend(), lb, ub); 71 | return { result, lb, ub }; 72 | } 73 | std::tuple::const_iterator, 74 | typename TimeOrder_t::const_iterator> 75 | findBoundWithResult(const TimeOrder_t& traj_csv, const ros::Time& stamp) 76 | { 77 | auto lb = traj_csv.lower_bound(stamp); 78 | auto ub = traj_csv.upper_bound(stamp); 79 | find_result result = checkResult(traj_csv.cbegin(), traj_csv.cend(), lb, ub); 80 | return { result, lb, ub }; 81 | } 82 | 83 | tf2::Transform interpolatePose(const tf2::Transform& before, const tf2::Transform& after, double t) 84 | { 85 | tf2::Transform pose; 86 | pose.setOrigin(before.getOrigin() * (1 - t) + after.getOrigin() * t); 87 | pose.setRotation(tf2::slerp(before.getRotation(), after.getRotation(), t)); 88 | return pose; 89 | } 90 | 91 | tf2::Transform interpolatePose(const geometry_msgs::Pose& before, const geometry_msgs::Pose& after, double t) 92 | { 93 | tf2::Transform before_tf, after_tf; 94 | tf2::convert(before, before_tf); 95 | tf2::convert(after, after_tf); 96 | return interpolatePose(before_tf, after_tf, t); 97 | } 98 | 99 | std::optional findPoseInterpolated(const std::vector& traj_csv, 100 | const ros::Time& stamp) 101 | { 102 | auto [result, lb, ub] = findBoundWithResult(traj_csv, stamp); 103 | if ((result & find_result::FOUND) != find_result::NONE) 104 | { 105 | tf2::Transform pose; 106 | tf2::convert(lb->pose, pose); 107 | return pose; 108 | } 109 | else if (result == find_result::INTERPOLATABLE) 110 | { 111 | lb--; 112 | return interpolatePose(lb->pose, ub->pose, getInterpolateRate(stamp, lb->header.stamp, ub->header.stamp)); 113 | } 114 | return std::nullopt; 115 | } 116 | std::optional findPoseInterpolated(const TimeOrder_t& traj_csv, 117 | const ros::Time& stamp) 118 | { 119 | auto [result, lb, ub] = findBoundWithResult(traj_csv, stamp); 120 | if ((result & find_result::FOUND) != find_result::NONE) 121 | { 122 | tf2::Transform pose; 123 | tf2::convert(lb->second, pose); 124 | return pose; 125 | } 126 | else if (result == find_result::INTERPOLATABLE) 127 | { 128 | lb--; 129 | return interpolatePose(lb->second, ub->second, getInterpolateRate(stamp, lb->first, ub->first)); 130 | } 131 | return std::nullopt; 132 | } 133 | 134 | std::optional findPose(const std::vector& traj_csv, size_t& prev, 135 | const ros::Time& stamp) 136 | { 137 | size_t id; 138 | if (traj_csv[prev + 1].header.stamp == stamp) 139 | { 140 | id = prev + 1; 141 | prev = id; 142 | } 143 | else 144 | { 145 | size_t l = prev, u = traj_csv.size(); 146 | while (u - l > 1) 147 | { 148 | auto& s = traj_csv[l + (u - l - 1) / 2].header.stamp; 149 | if (s == stamp) 150 | { 151 | break; 152 | } 153 | else if (s < stamp) 154 | { 155 | l = l + (u - l - 1) / 2 + 1; 156 | } 157 | else 158 | { 159 | u = l + (u - l - 1) / 2; 160 | } 161 | } 162 | if (u == l) 163 | { 164 | u++; 165 | } 166 | id = l + (u - l - 1) / 2; 167 | prev = id; 168 | if (traj_csv[id].header.stamp != stamp) 169 | { 170 | return std::nullopt; 171 | } 172 | } 173 | tf2::Transform pose; 174 | tf2::convert(traj_csv[id].pose, pose); 175 | return pose; 176 | } 177 | 178 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/src/map/orthomap.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "map/orthomap.hpp" 27 | #include "util/error.hpp" 28 | #include "util/timer.hpp" 29 | 30 | namespace hawkeye 31 | { 32 | const cv::Mat OrthoMap::getImage(const tf2::Vector3& pos, int shift_x, int shift_y) const 33 | { 34 | auto [w, h] = getPosOfWholeGrid(pos); 35 | w += shift_x * int(image_width_); 36 | h -= shift_y * int(image_height_); 37 | if (!checkValid(w, h)) 38 | { 39 | return cv::Mat(); 40 | } 41 | return maps_[std::get<0>(convertPosToImage(w, h))]; 42 | } 43 | 44 | cv::Mat OrthoMap::getImageRange(const tf2::Vector3& center, size_t range_x, size_t range_y) const 45 | { 46 | cv::Mat ret(cv::Size(range_x, range_y), CV_8U, cv::Scalar(0)); 47 | int w_min, h_min; 48 | { 49 | auto [w, h] = getPosOfWholeGrid(center); 50 | w_min = w - range_x / 2; 51 | h_min = h - range_y / 2; 52 | } 53 | int w_max = w_min + range_x; 54 | int h_max = h_min + range_y; 55 | for (int wi = divFloor(w_min, int(image_width_)); wi * int(image_width_) < w_max; ++wi) 56 | { 57 | if (wi < 0) 58 | { 59 | continue; 60 | } 61 | if (wi >= map_div_x_) 62 | { 63 | break; 64 | } 65 | const int rect_w_min = std::max(w_min, wi * int(image_width_)); 66 | const int rect_w_size = std::min(w_max, (wi + 1) * int(image_width_)) - rect_w_min; 67 | for (int hi = divFloor(h_min, int(image_height_)); hi * int(image_height_) < h_max; ++hi) 68 | { 69 | if (hi < 0) 70 | { 71 | continue; 72 | } 73 | if (hi >= map_div_y_) 74 | { 75 | break; 76 | } 77 | const int rect_h_min = std::max(h_min, hi * int(image_height_)); 78 | const int rect_h_size = std::min(h_max, (hi + 1) * int(image_height_)) - rect_h_min; 79 | const cv::Mat map = maps_[wi + hi * map_div_x_]; 80 | if (map.empty()) 81 | { 82 | continue; 83 | } 84 | const cv::Size roi_size(rect_w_size, rect_h_size); 85 | const cv::Rect roi_ret(cv::Point(rect_w_min - w_min, rect_h_min - h_min), roi_size); 86 | const cv::Rect roi_map(cv::Point(rect_w_min - wi * int(image_width_), rect_h_min - hi * int(image_height_)), 87 | roi_size); 88 | map(roi_map).copyTo(ret(roi_ret)); 89 | } 90 | } 91 | return ret; 92 | } 93 | 94 | bool OrthoMap::ckeckGridLatest(const tf2::Vector3& pos, size_t range) const 95 | { 96 | if (og_map_.last_info_.has_value()) 97 | { 98 | auto [center_w, center_h] = getPosOfImageGrid(pos); 99 | return *og_map_.last_info_ == std::make_tuple(center_w, center_h, range); 100 | } 101 | return false; 102 | } 103 | 104 | const nav_msgs::OccupancyGrid& OrthoMap::getGridAround(const std_msgs::Header& header, const tf2::Vector3& pos, 105 | size_t range) 106 | { 107 | og_map_.data_.header = header; 108 | og_map_.data_.info.origin.position.z = pos.z(); 109 | auto [center_w, center_h] = getPosOfImageGrid(pos); 110 | if (og_map_.last_info_.has_value()) 111 | { 112 | if (*og_map_.last_info_ == std::make_tuple(center_w, center_h, range)) 113 | { 114 | return og_map_.data_; 115 | } 116 | *og_map_.last_info_ = std::make_tuple(center_w, center_h, range); 117 | } 118 | else 119 | { 120 | og_map_.last_info_.emplace(center_w, center_h, range); 121 | } 122 | size_t range2 = 2 * range + 1u; 123 | int rangei = int(range); 124 | size_t width = range2 * image_width_; 125 | size_t height = range2 * image_height_; 126 | { 127 | auto& info = og_map_.data_.info; 128 | info.width = width; 129 | info.height = height; 130 | info.resolution = scale_; 131 | double diff_x = center_w - rangei - map_div_x_ * 0.5; 132 | double diff_y = center_h + (rangei + 1) - map_div_y_ * 0.5; 133 | info.origin.position.x = diff_x * image_width_ * scale_ - scale_ / 2; 134 | info.origin.position.y = -diff_y * image_height_ * scale_ + scale_ / 2; 135 | info.origin.position.z = pos.z(); 136 | info.origin.orientation.x = 0; 137 | info.origin.orientation.y = 0; 138 | info.origin.orientation.z = 0; 139 | info.origin.orientation.w = 1; 140 | 141 | og_map_.data_.data.clear(); 142 | og_map_.data_.data.resize(width * height, -1); 143 | } 144 | { 145 | for (int y = -rangei; y <= rangei; ++y) 146 | { 147 | for (int x = -rangei; x <= rangei; ++x) 148 | { 149 | if (const cv::Mat m = getImage(pos, x, y); !m.empty()) 150 | { 151 | int index_base = width * (image_height_ * (y + rangei + 1) - 1) + image_width_ * (x + rangei); 152 | if (!classify_emptiness_) 153 | { 154 | m.forEach([this, &index_base, &width](uchar c, const int* position) { 155 | auto& h = position[0]; 156 | auto& w = position[1]; 157 | og_map_.data_.data[index_base + w - h * width] = (255 - c) * 100 / 255.; 158 | }); 159 | } 160 | else 161 | { 162 | m.forEach([this, &index_base, &width](uchar c, const int* position) { 163 | if (c != 0) 164 | { 165 | auto& h = position[0]; 166 | auto& w = position[1]; 167 | og_map_.data_.data[index_base + w - h * width] = (255 - c) * 100 / 254.; 168 | } 169 | }); 170 | } 171 | } 172 | } 173 | } 174 | } 175 | return og_map_.data_; 176 | } 177 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/src/map/orthomap_read.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "map/orthomap.hpp" 27 | #include "util/error.hpp" 28 | #include "util/timer.hpp" 29 | 30 | #include 31 | #include 32 | 33 | template 34 | void getFileElements(std::ifstream& ifs, const std::string& filename, T&& ref) 35 | { 36 | if (!(ifs >> ref)) 37 | { 38 | hawkeye::exitBadFile(filename); 39 | } 40 | } 41 | 42 | namespace hawkeye 43 | { 44 | constexpr int orthomap_version = 3; 45 | OrthoMap::OrthoMap(const std::string& orthomap_filename) : orthomap_filename_{ orthomap_filename } 46 | { 47 | std::string prefix; 48 | std::string suffix; 49 | bool fill_zero; 50 | double scale; 51 | double pos_x; 52 | double pos_y; 53 | int div_x; 54 | int div_y; 55 | int width; 56 | int height; 57 | int num_images; 58 | bool scaling_scaled; 59 | double scaling_threshold_min; 60 | double scaling_threshold_max; 61 | bool classify_emptiness; 62 | std::vector> index_image; 63 | { 64 | std::ifstream ifs(orthomap_filename_); 65 | if (!ifs) 66 | { 67 | exitOpenError(orthomap_filename_); 68 | } 69 | { 70 | std::string ext_name; 71 | int version; 72 | getFileElements(ifs, orthomap_filename_, ext_name); 73 | getFileElements(ifs, orthomap_filename_, version); 74 | if (ext_name != "orthomap") 75 | { 76 | exitBadFile(orthomap_filename_, "Not an orthomap file"); 77 | } 78 | if (version != orthomap_version) 79 | { 80 | exitBadFile(orthomap_filename_, "version = " + std::to_string(version)); 81 | } 82 | } 83 | getFileElements(ifs, orthomap_filename_, std::quoted(prefix)); 84 | getFileElements(ifs, orthomap_filename_, std::quoted(suffix)); 85 | ifs >> std::boolalpha; 86 | getFileElements(ifs, orthomap_filename_, fill_zero); 87 | ifs >> std::noboolalpha; 88 | getFileElements(ifs, orthomap_filename_, scale); 89 | getFileElements(ifs, orthomap_filename_, pos_x); 90 | getFileElements(ifs, orthomap_filename_, pos_y); 91 | getFileElements(ifs, orthomap_filename_, div_x); 92 | getFileElements(ifs, orthomap_filename_, div_y); 93 | getFileElements(ifs, orthomap_filename_, width); 94 | getFileElements(ifs, orthomap_filename_, height); 95 | getFileElements(ifs, orthomap_filename_, num_images); 96 | ifs >> std::boolalpha; 97 | getFileElements(ifs, orthomap_filename_, scaling_scaled); 98 | ifs >> std::noboolalpha; 99 | if (scaling_scaled) 100 | { 101 | getFileElements(ifs, orthomap_filename_, scaling_threshold_min); 102 | getFileElements(ifs, orthomap_filename_, scaling_threshold_max); 103 | ifs >> std::boolalpha; 104 | getFileElements(ifs, orthomap_filename_, classify_emptiness); 105 | ifs >> std::noboolalpha; 106 | } 107 | else 108 | { 109 | classify_emptiness_ = false; 110 | } 111 | { 112 | std::vector v; 113 | double buf; 114 | while (ifs >> buf) 115 | { 116 | v.push_back(buf); 117 | } 118 | if (v.size() != div_x * div_y) 119 | { 120 | exitBadFile(orthomap_filename_); 121 | } 122 | index_image.reserve(num_images); 123 | for (int row = 0; row < div_y; ++row) 124 | { 125 | for (int col = 0; col < div_x; ++col) 126 | { 127 | int id = row * div_x + col; 128 | if (v[id] == 1) 129 | { 130 | index_image.emplace_back(row, col); 131 | } 132 | } 133 | } 134 | } 135 | { 136 | auto filename_length = boost::filesystem::path(orthomap_filename_).filename().string().size(); 137 | prefix = orthomap_filename_.substr(0, orthomap_filename_.size() - filename_length) + prefix; 138 | } 139 | } 140 | 141 | scale_ = scale; 142 | map_div_x_ = div_x; 143 | map_div_y_ = div_y; 144 | image_height_ = height; 145 | image_width_ = width; 146 | map_height_ = height * div_y; 147 | map_width_ = width * div_x; 148 | scaling_.scaled_ = scaling_scaled; 149 | scaling_.threshold_min_ = scaling_threshold_min; 150 | scaling_.threshold_max_ = scaling_threshold_max; 151 | classify_emptiness_ = classify_emptiness; 152 | center_x_ = pos_x + ((map_width_ - 1) * 0.5) * scale; 153 | center_y_ = pos_y - ((map_height_ - 1) * 0.5) * scale; 154 | 155 | maps_.resize(div_y * div_x); 156 | 157 | auto filename_length = (prefix + std::to_string(num_images - 1) + suffix).size(); 158 | auto pos_length = std::to_string(std::max(div_x, div_y) - 1).size() + 1; 159 | for (int i = 0; i < num_images; ++i) 160 | { 161 | std::string image_name; 162 | if (fill_zero) 163 | { 164 | std::ostringstream oss; 165 | oss << prefix << std::setw(std::to_string(num_images - 1).size()) << std::setfill('0') << i << suffix; 166 | image_name = oss.str(); 167 | } 168 | else 169 | { 170 | image_name = prefix + std::to_string(i) + suffix; 171 | } 172 | const auto& [row_image, col_image] = index_image[i]; 173 | std::cout << "loading " << std::setw(filename_length) << image_name; 174 | std::cout << " for [" << std::setw(pos_length) << row_image << ',' << std::setw(pos_length) << col_image << ']'; 175 | std::cout << " ..." << std::flush; 176 | cv::Mat image = cv::imread(image_name, 0); 177 | if (image.empty()) 178 | { 179 | exitOpenError(image_name); 180 | } 181 | if (image.size() != cv::Size(width, height)) 182 | { 183 | exitBadFile(image_name, "The image size [" + std::to_string(width) + ',' + std::to_string(height) + 184 | "] is different from the size in the orthomap file."); 185 | } 186 | image.copyTo(maps_[row_image * div_x + col_image]); 187 | // auto& maps_map = maps_[row_image * div_x + col_image]; 188 | // cv::Mat image_scaled = (255 - image) * 100 / (classify_emptiness ? 254. : 255.); 189 | // image_scaled.convertTo(maps_map, CV_8S); 190 | // if (classify_emptiness) 191 | // { 192 | // bitwise_or(maps_map, -1, maps_map, image == 0); 193 | // } 194 | std::cout << " success." << std::endl; 195 | } 196 | } 197 | 198 | void OrthoMap::exportImage(const std::string& filename) 199 | { 200 | cv::Mat map = cv::Mat::zeros(cv::Size(map_width_, map_height_), CV_8U); 201 | for (int h = 0; h < map_div_y_; ++h) 202 | { 203 | for (int w = 0; w < map_div_x_; ++w) 204 | { 205 | if (int map_id = h * map_div_x_ + w; !maps_[map_id].empty()) 206 | { 207 | cv::Rect roi(cv::Point(image_width_ * w, image_height_ * h), cv::Size(image_width_, image_height_)); 208 | maps_[map_id].copyTo(map(roi)); 209 | } 210 | } 211 | } 212 | cv::imwrite(filename, map); 213 | } 214 | 215 | } // namespace hawkeye -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Hawkeye 2 | 3 | Hawkeye - Localization with LiDAR, GNSS/INS and ortho image map 4 | 5 | [Demo Video](https://youtu.be/cXr0e-J7uIU) 6 | 7 | [![](http://img.youtube.com/vi/cXr0e-J7uIU/0.jpg)](https://youtu.be/cXr0e-J7uIU "Hawkeye") 8 | 9 | [Demo Video (with Ground Segmentation)](https://youtu.be/HDGBGOr2-rA) 10 | 11 | [![](http://img.youtube.com/vi/HDGBGOr2-rA/0.jpg)](https://youtu.be/HDGBGOr2-rA "Hawkeye with Ground Segmentation") 12 | 13 | ## Update 14 | ### 2023/03/22 15 | **Hawkeye and the config file for the sample are updated.** 16 | **To use the updated Hawkeye, please re-download the config file.** 17 | 18 | ## What is Hawkeye 19 | This is a limited re-implementation of [this method](https://doi.org/10.11351/jsaeronbun.51.824). This estimates the pose with LiDAR, [Eagleye](https://github.com/MapIV/eagleye.git) and ortho image map ***without Millimeter Wave Radar***. This implementation is extended to accommodate cases where GNSS/INS(Eagleye) errors accumulate for some reason. 20 | 21 | ### Method 22 | This method corrects GNSS/INS position by maching the reflection intensity between the ground and a pre-made map. 23 | Only the x and y coordinates are corrected. The roll, the pitch and the yaw of GNSS/INS is not corrected and the z coordinate is ignored. 24 | The paper also uses Millimeter Wave Radar and the map of it as same as reflection intensity by LiDAR, but it is not implemented. 25 | 26 | * A histogram filter centered on the GNSS/INSS position is used. The GNSS/INS pose is corrected by the center of gravity in the histogram. 27 | * A map of the ground reflection intensity around the vehicle is generated from the accumulated LiDAR observations. 28 | * This filter and map are of the same resolution and orientation as the pre-made map, independent of the vehicle orientation. 29 | * The histogram is updated by the results of template matching between the runtime map and the pre-made map. 30 | * Please see [this paper](https://doi.org/10.11351/jsaeronbun.51.824) for details. 31 | 32 | As an extension, the center of the histogram optionally shifts when the estimated position is far from the center of the histogram. The histogram points newly made at the center shift are initialized by the nearby point. This can be invalid by setting the parameter `CENTER_SHIFT_THRESHOLD` or `center_shift_threshold` zero. 33 | 34 | ## Recommended Environment 35 | Ubuntu 18.04 - with ROS melodic 36 | 37 | ## Sample 38 | You can download the following sample files from [here](https://www.dropbox.com/sh/rfx1ef0ag2v3uf8/AADYxuWFQE80wF7IkfpltA6ea?dl=0). Download all files and unzip ortho_image.zip. 39 | 40 | *This sample does not perform ground segmentation of LiDAR point cloud. To increase accuracy, place a ground segmentation node (not implemented in this repository) between the LiDAR driver and Hawkeye.* 41 | 42 | * Clone this repository. 43 | ``` 44 | mkdir -p hawkeye_ws/src 45 | cd hawkeye_ws/src 46 | git clone https://github.com/MapIV/hawkeye.git 47 | ``` 48 | 49 | * Clone [Eagleye](https://github.com/MapIV/eagleye.git). 50 | We tested with [the commit 9ca1c91af97cb22ab81c2f3eb1d412247bbc56f0](https://github.com/MapIV/eagleye/tree/9ca1c91af97cb22ab81c2f3eb1d412247bbc56f0). 51 | ``` 52 | git clone --recursive https://github.com/MapIV/eagleye.git 53 | git clone https://github.com/MapIV/rtklib_ros_bridge.git 54 | git clone https://github.com/MapIV/nmea_comms.git 55 | git clone https://github.com/MapIV/nmea_ros_bridge.git 56 | git clone https://github.com/MapIV/gnss_compass_ros.git 57 | ``` 58 | 59 | * Install MapIV's fork of RTKLIB by following [the README of Eagleye](https://github.com/MapIV/eagleye/blob/e40555433b219d3f7df5e502c69ec04b54bcfc62/README.md#rtklib). 60 | 61 | * Clone [Hesai LiDAR driver](https://github.com/MapIV/hesai_pandar.git). 62 | ``` 63 | git clone https://github.com/MapIV/hesai_pandar.git 64 | ``` 65 | 66 | * Download ROS dependencies and build. 67 | ``` 68 | cd .. 69 | rosdep install --from-paths src --ignore-src -r -y 70 | catkin_make -DCMAKE_BUILD_TYPE=Release 71 | ``` 72 | 73 | * Launch Roscore. 74 | ``` 75 | roscore 76 | ``` 77 | 78 | * Play rosbag file. 79 | ``` 80 | cd 81 | rosparam set use_sim_time true 82 | rosbag play --clock rosbag/2021-05-18-13-37-24_0.bag 83 | ``` 84 | 85 | * Launch sample.launch. 86 | ``` 87 | roslaunch hawkeye sample.launch sample_dir:="" 88 | ``` 89 | 90 | ## Realtime Hawkeye Node 91 | 92 | ### Usage 93 | ``` 94 | $ rosrun hawkeye hawkeye_rt \ 95 | [-e ] \ 96 | [-d ] \ 97 | [-w ] \ 98 | [-n ] \ 99 | [-g ] \ 100 | [-a ] \ 101 | [-A ] \ 102 | [-s ] \ 103 | [-c|-C] \ 104 | [-o] 105 | ``` 106 | 107 | This works with eagleye_rt.launch and eagleye_fix2pose.launch. `/eagleye/pose` and `/eagleye/fix` are subscribed. 108 | 109 | ### Arguments and Parameters 110 | * `ORTHOMAP_INFO` : An orthomap file. For more information, See [README of orthomap_viewer](orthomap_viewer/README.md). 111 | * `YAML_FILE` : A configuration YAML file. See [this example](hawkeye/config/hawkeye.yaml). 112 | * `LIDAR_TOPIC_NAME` : A topic name for LiDAR data. The topic type must be sensor_msgs/PointCloud2. The points should be classified by whether they are ground or not and be only the points of ground. If they are not classified, it may be less accuracy. 113 | * Some optional parameters overwrite the corresponding configurations in `YAML_FILE`. The explanations of the parameters are on [the expample yaml file](hawkeye/config/hawkeye.yaml). 114 | * `ERROR_RATE` and `error_rate`: $\alpha$ in the paper 115 | * `COEFF_DIMIMISH` and `coeff_diminish`: $\beta$ in the paper 116 | * `COEFF_WEIGHT` and `coeff_weight`: $k$ in the paper 117 | * `COEFF_NEGATIVE` and `coeff_negative`: $b$ in the paper 118 | * `COEFF_GAIN` and `coeff_gain`: $g$ in the paper 119 | * `ACCUMULATION_COUNT` and `lidar_accumulate/max_count`: The threshold count of LiDAR accumulation 120 | * `ACCUMULATION_LENGTH` and `lidar_accumulate/max_length`: The threshold length of LiDAR accumulation 121 | * `CENTER_SHIFT_THRESHOLD` and `center_shift_threshold`: The threshold distance for the shift of the histogram center 122 | * `hawkeye/edge_copy_shift` can be overwritten by `-c` (true) or `-C` (false): If true, The histogram points newly made are initialized by the nearby point. Otherwise, those are initialized by zero. 123 | * Small overhead `-o` : This option stops publishing some topics for visualization. 124 | 125 | ### Output 126 | * Console 127 | * Estimated pose relative to the center of the map 128 | * x, y and yaw 129 | * Elapsed time for each step 130 | * The whole step time and the mean are also calculated. `Essential` time means the time without I/O and process for visualization. 131 | * Topic for RVIZ 132 | * Static TF by tf2 133 | * `local_map` from `map`: The center of the map 134 | * TF by tf2 and trajectory(nav_msgs/Path) 135 | * `raw_tf` and `raw_path` from `local_map`: The Eagleye pose synchronized to the estimated pose 136 | * `estimated` and `estimated_path` from `local_map`: The estimated pose 137 | * `histogram_center` and `histogram_center_path` from `local_map`: The center of the histogram 138 | * `histogram_peak` and `histogram_peak_path` from `local_map`: The average pose of the histogram 139 | * `match_peak` and `match_peak_path` from `local_map`: The average pose of the match result 140 | * `ortho_map`(nav_msgs/OccupancyGrid): Grayscale map around the estimated pose 141 | * This is not published very often but takes much time to publish. The time increases and the frequency decrease with the size of each ortho image. 142 | * `point_cloud`(sensor_msgs/PointCloud2): LiDAR point cloud 143 | * Only the points of the last LiDAR data. 144 | * `histogram`(visualization_msgs/MarkerArray): The histogram filter 145 | * It is gradated from red to green. 146 | * Points not used for average estimation are darker than used ones. 147 | * Grayscale images(sensor_msgs/Image) 148 | * `template_image` 149 | * `submap_image` 150 | * `match_image`: The result of the maching between the above two images. 151 | 152 | ## Realtime Hawkeye Node with Weighted Histogram 153 | Another extention for the method. Only the initialization of the shifted histogram is different. 154 | In this implementation, 155 | The histogram points newly made at the center shift are initialized by zero and 156 | the histogram is weighted by the accumulated time. 157 | 158 | ### Usage 159 | ``` 160 | $ rosrun hawkeye hawkeye_rt_ws \ 161 | [-e ] \ 162 | [-d ] \ 163 | [-w ] \ 164 | [-n ] \ 165 | [-g ] \ 166 | [-a ] \ 167 | [-A ] \ 168 | [-s ] \ 169 | [-H ] \ 170 | [-o] 171 | ``` 172 | 173 | ### Arguments and Parameters 174 | `hawkeye/edge_copy_shift` is invalid and `HISTOGRAM_WEIGHT` and `histogram_weight` are valid. This parameter means how the newly created histograms are weighted by the accumulated time. The detail is on [the expample yaml file](hawkeye/config/hawkeye.yaml). 175 | 176 | ### Output 177 | A grayscale image `weight_image`(sensor_msgs/Image) is puplished additionaly. This image shows the accumulated time. 178 | 179 | ## Research Papers 180 | 1. D Hirano, K Yoneda, R Yanase, A Mohammad, N Suganuma, "LiDAR and Radar Sensor Fusion for Localizing Autonomous Vehicles", Transactions of Society of Automotive Engineers of Japan 51(5) 824-829, 2020 [Link](https://www.jstage.jst.go.jp/article/jsaeronbun/51/5/51_20204428/_article/-char/en) 181 | 182 | ## License 183 | Hawkeye is provided under the [BSD 3-Clause](https://github.com/MapIV/hawkeye/blob/main/LICENSE) License. 184 | 185 | ## Contacts 186 | If you have further question, email to map4@tier4.jp. 187 | -------------------------------------------------------------------------------- /hawkeye/include/util/pose_synchronize.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | #include "hawkeye_define.hpp" 31 | #include "util/pose_stamped_helper.hpp" 32 | 33 | #include 34 | 35 | namespace hawkeye 36 | { 37 | class PoseInterpolationSubscriber 38 | { 39 | public: 40 | using this_type = PoseInterpolationSubscriber; 41 | 42 | PoseInterpolationSubscriber(bool check_eagleye_error = false) 43 | : check_eageye_error_{ check_eagleye_error }, check_eageye_error2_{ false } {}; 44 | 45 | bool addPose(const geometry_msgs::PoseStamped& pose_stamped) 46 | { 47 | if (check_eageye_error_ && pose_stamped.pose.position.x == 0 && pose_stamped.pose.position.y == 0 && 48 | pose_stamped.pose.position.z == 0) 49 | { 50 | return false; 51 | } 52 | if (check_eageye_error2_) 53 | { 54 | if (eagleye_enabled_time_.is_zero()) 55 | { 56 | return false; 57 | } 58 | if (eagleye_enabled_time_ > pose_stamped.header.stamp) 59 | { 60 | std::cout << pose_stamped.header.stamp.toNSec() << " is not valid" << std::endl; 61 | return false; 62 | } 63 | } 64 | if (!pose_frame_.empty() && pose_stamped.header.frame_id != pose_frame_) 65 | { 66 | return false; 67 | } 68 | pose_map_.insert_or_assign(pose_stamped.header.stamp, pose_stamped.pose); 69 | return true; 70 | } 71 | 72 | void addPoseSubscriber(ros::NodeHandle& nh, const std::string& name, uint32_t queue_size, 73 | const std::string& frame_name = "") 74 | { 75 | pose_subsciber_ = nh.subscribe(name, queue_size, &this_type::callbackPose, this); 76 | pose_frame_ = frame_name; 77 | } 78 | 79 | void addEagleyeCheckSubscriber(ros::NodeHandle& nh, const std::string& name, uint32_t queue_size) 80 | { 81 | check_eageye_error2_ = true; 82 | eagleye_check_subscriber_ = nh.subscribe(name, queue_size, &this_type::callbackChecker, this); 83 | } 84 | 85 | protected: 86 | using interpolated_pose_iter = std::pair::const_iterator, tf2::Transform>; 87 | 88 | interpolated_pose_iter findForStamp_NoCheck(const ros::Time& stamp) const 89 | { 90 | auto [result, before, after] = findBoundWithResult(pose_map_, stamp); 91 | tf2::Transform ret; 92 | if (result == find_result::INTERPOLATABLE) 93 | { 94 | before--; 95 | ret = interpolatePose(before->second, after->second, getInterpolateRate(stamp, before->first, after->first)); 96 | } 97 | else 98 | { 99 | tf2::convert(before->second, ret); 100 | } 101 | return { before, ret }; 102 | } 103 | 104 | private: 105 | void callbackPose(const geometry_msgs::PoseStamped::Ptr ptr) 106 | { 107 | addPose(*ptr); 108 | } 109 | 110 | void callbackChecker(const sensor_msgs::NavSatFix::Ptr ptr) 111 | { 112 | if (eagleye_enabled_time_.is_zero() && check_eageye_error2_ && ptr->status.service == 0) 113 | { 114 | eagleye_enabled_time_ = ptr->header.stamp; 115 | std::cout << "eagleye is valid after " << eagleye_enabled_time_.toNSec() << std::endl; 116 | } 117 | } 118 | 119 | protected: 120 | TimeOrder_t pose_map_; 121 | 122 | private: 123 | bool check_eageye_error_; 124 | bool check_eageye_error2_; 125 | std::string pose_frame_; 126 | ros::Subscriber pose_subsciber_; 127 | ros::Subscriber eagleye_check_subscriber_; 128 | ros::Time eagleye_enabled_time_; 129 | }; 130 | 131 | // --- PoseSychronizer --- 132 | // synchronize geometry_msgs/PoseStamped and topic with header by inpterpolating poses and return tf2/Transform 133 | template 134 | class PoseSychronizerSub : public PoseInterpolationSubscriber 135 | { 136 | public: 137 | using this_type = PoseSychronizerSub; 138 | using topic_ptr_type = typename _Ty::Ptr; 139 | using topic_raw_type = _Ty; 140 | using topic_return_type = std::conditional_t; 141 | using return_tuple_type = std::tuple; 142 | 143 | PoseSychronizerSub(bool check_eagleye_error = false) : PoseInterpolationSubscriber{ check_eagleye_error } {}; 144 | 145 | bool haveSynchronized() const 146 | { 147 | return !(topic_map_.empty() || pose_map_.empty() || pose_map_.begin()->first > topic_map_.rbegin()->first || 148 | pose_map_.rbegin()->first < topic_map_.begin()->first); 149 | } 150 | 151 | bool addTopic(const topic_return_type& topic) 152 | { 153 | std_msgs::Header const* const header = getTopicHeaderPtr(topic); 154 | if (!topic_frame_.empty() && header->frame_id != topic_frame_) 155 | { 156 | return false; 157 | } 158 | topic_map_.insert_or_assign(header->stamp, topic); 159 | return true; 160 | } 161 | bool addTopicPtr(const topic_ptr_type& topic_ptr) 162 | { 163 | if constexpr (return_ptr) 164 | { 165 | return addTopic(topic_ptr); 166 | } 167 | else 168 | { 169 | return addTopic(*topic_ptr); 170 | } 171 | } 172 | 173 | void addTopicSubscriber(ros::NodeHandle& nh, const std::string& name, uint32_t queue_size, 174 | const std::string& frame_name = "") 175 | { 176 | topic_subsciber_ = nh.subscribe(name, queue_size, &this_type::callbackTopic, this); 177 | topic_frame_ = frame_name; 178 | } 179 | 180 | std::optional getOldest() const 181 | { 182 | if (!haveSynchronized()) 183 | { 184 | return std::nullopt; 185 | } 186 | auto [pose_it, topic_it] = getOldest_NoCheck(); 187 | return std::optional(std::in_place, pose_it.second, topic_it->second, topic_it->first); 188 | } 189 | std::optional getLatest() const 190 | { 191 | if (!haveSynchronized()) 192 | { 193 | return std::nullopt; 194 | } 195 | auto [pose_it, topic_it] = getLatest_NoCheck(); 196 | return std::optional(std::in_place, pose_it.second, topic_it->second, topic_it->first); 197 | } 198 | 199 | void dropInvalidTopics() 200 | { 201 | auto topic_it = topic_map_.upper_bound(pose_map_.begin()->first); 202 | topic_map_.erase(topic_map_.begin(), topic_it); 203 | } 204 | 205 | void dropToOldest() 206 | { 207 | if (!haveSynchronized()) 208 | { 209 | dropInvalidTopics(); 210 | return; 211 | } 212 | auto [pose_it, topic_it] = getOldest_NoCheck(); 213 | topic_map_.erase(topic_map_.begin(), topic_it++); 214 | pose_map_.erase(pose_map_.begin(), pose_it.first); 215 | } 216 | void dropToLatest() 217 | { 218 | if (!haveSynchronized()) 219 | { 220 | dropInvalidTopics(); 221 | return; 222 | } 223 | auto [pose_it, topic_it] = getLatest_NoCheck(); 224 | topic_map_.erase(topic_map_.begin(), topic_it++); 225 | pose_map_.erase(pose_map_.begin(), pose_it.first); 226 | } 227 | 228 | private: 229 | using const_topic_iter = typename TimeOrder_t::const_iterator; 230 | using mid_result_t = std::pair; 231 | 232 | static const std_msgs::Header* const getTopicHeaderPtr(const topic_ptr_type& topic) 233 | { 234 | return &(topic->header); 235 | } 236 | static const std_msgs::Header* const getTopicHeaderPtr(const topic_raw_type& topic) 237 | { 238 | return &(topic.header); 239 | } 240 | 241 | mid_result_t getOldest_NoCheck() const 242 | { 243 | auto topic_it = topic_map_.lower_bound(pose_map_.begin()->first); 244 | return { findForStamp_NoCheck(topic_it->first), topic_it }; 245 | } 246 | mid_result_t getLatest_NoCheck() const 247 | { 248 | auto topic_it = topic_map_.upper_bound(pose_map_.rbegin()->first); 249 | topic_it--; 250 | return { findForStamp_NoCheck(topic_it->first), topic_it }; 251 | } 252 | 253 | void callbackTopic(const topic_ptr_type ptr) 254 | { 255 | addTopicPtr(ptr); 256 | } 257 | 258 | private: 259 | TimeOrder_t topic_map_; 260 | std::string topic_frame_; 261 | ros::Subscriber topic_subsciber_; 262 | }; 263 | 264 | template 265 | class PoseSychronizer : public PoseSychronizerSub<_Ty, false> 266 | { 267 | public: 268 | PoseSychronizer(bool check_eagleye_error = false) : PoseSychronizerSub<_Ty, false>{ check_eagleye_error } 269 | { 270 | } 271 | }; 272 | 273 | template 274 | class PoseSychronizer> : public PoseSychronizerSub<_Ty, true> 275 | { 276 | public: 277 | PoseSychronizer(bool check_eagleye_error = false) : PoseSychronizerSub<_Ty, true>{ check_eagleye_error } 278 | { 279 | } 280 | }; 281 | 282 | } // namespace hawkeye -------------------------------------------------------------------------------- /hawkeye/rviz/hawkeye.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.5 9 | Tree Height: 835 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Class: rviz/TF 36 | Enabled: true 37 | Frame Timeout: 15 38 | Frames: 39 | All Enabled: true 40 | Marker Scale: 1 41 | Name: TF 42 | Show Arrows: true 43 | Show Axes: true 44 | Show Names: true 45 | Update Interval: 0 46 | Value: true 47 | - Alpha: 1 48 | Buffer Length: 1 49 | Class: rviz/Path 50 | Color: 114; 159; 207 51 | Enabled: true 52 | Head Diameter: 0.3 53 | Head Length: 0.2 54 | Length: 0.3 55 | Line Style: Lines 56 | Line Width: 0.03 57 | Name: Raw 58 | Offset: 59 | X: 0 60 | Y: 0 61 | Z: 0 62 | Pose Color: 255; 85; 255 63 | Pose Style: None 64 | Radius: 0.03 65 | Shaft Diameter: 0.1 66 | Shaft Length: 0.1 67 | Topic: /raw_path 68 | Unreliable: false 69 | Value: true 70 | - Alpha: 1 71 | Buffer Length: 1 72 | Class: rviz/Path 73 | Color: 173; 127; 168 74 | Enabled: true 75 | Head Diameter: 0.3 76 | Head Length: 0.2 77 | Length: 0.3 78 | Line Style: Lines 79 | Line Width: 0.03 80 | Name: Estimated 81 | Offset: 82 | X: 0 83 | Y: 0 84 | Z: 0 85 | Pose Color: 255; 85; 255 86 | Pose Style: None 87 | Radius: 0.03 88 | Shaft Diameter: 0.1 89 | Shaft Length: 0.1 90 | Topic: /estimated_path 91 | Unreliable: false 92 | Value: true 93 | - Alpha: 0.7 94 | Class: rviz/Map 95 | Color Scheme: map 96 | Draw Behind: false 97 | Enabled: true 98 | Name: Map 99 | Topic: /ortho_map 100 | Unreliable: false 101 | Use Timestamp: false 102 | Value: true 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 10 107 | Min Value: -10 108 | Value: true 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/PointCloud2 112 | Color: 255; 255; 255 113 | Color Transformer: Intensity 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Min Color: 0; 0; 0 119 | Name: PointCloud2 120 | Position Transformer: XYZ 121 | Queue Size: 10 122 | Selectable: true 123 | Size (Pixels): 3 124 | Size (m): 0.01 125 | Style: Points 126 | Topic: /point_cloud 127 | Unreliable: false 128 | Use Fixed Frame: true 129 | Use rainbow: true 130 | Value: true 131 | - Class: rviz/MarkerArray 132 | Enabled: true 133 | Marker Topic: /histogram 134 | Name: Histogram 135 | Namespaces: 136 | histogram: true 137 | Queue Size: 100 138 | Value: true 139 | - Alpha: 1 140 | Buffer Length: 1 141 | Class: rviz/Path 142 | Color: 252; 233; 79 143 | Enabled: false 144 | Head Diameter: 0.3 145 | Head Length: 0.2 146 | Length: 0.3 147 | Line Style: Lines 148 | Line Width: 0.03 149 | Name: Histogram Peak 150 | Offset: 151 | X: 0 152 | Y: 0 153 | Z: 0 154 | Pose Color: 255; 85; 255 155 | Pose Style: None 156 | Radius: 0.03 157 | Shaft Diameter: 0.1 158 | Shaft Length: 0.1 159 | Topic: /histogram_peak_path 160 | Unreliable: false 161 | Value: false 162 | - Alpha: 1 163 | Buffer Length: 1 164 | Class: rviz/Path 165 | Color: 240; 50; 230 166 | Enabled: false 167 | Head Diameter: 0.3 168 | Head Length: 0.2 169 | Length: 0.3 170 | Line Style: Lines 171 | Line Width: 0.03 172 | Name: Match Peak 173 | Offset: 174 | X: 0 175 | Y: 0 176 | Z: 0 177 | Pose Color: 255; 85; 255 178 | Pose Style: None 179 | Radius: 0.03 180 | Shaft Diameter: 0.1 181 | Shaft Length: 0.1 182 | Topic: /match_peak_path 183 | Unreliable: false 184 | Value: false 185 | - Alpha: 1 186 | Buffer Length: 1 187 | Class: rviz/Path 188 | Color: 245; 121; 0 189 | Enabled: false 190 | Head Diameter: 0.3 191 | Head Length: 0.2 192 | Length: 0.3 193 | Line Style: Lines 194 | Line Width: 0.03 195 | Name: Histogram Center 196 | Offset: 197 | X: 0 198 | Y: 0 199 | Z: 0 200 | Pose Color: 255; 85; 255 201 | Pose Style: None 202 | Radius: 0.03 203 | Shaft Diameter: 0.1 204 | Shaft Length: 0.1 205 | Topic: /histogram_center_path 206 | Unreliable: false 207 | Value: false 208 | - Class: rviz/Image 209 | Enabled: true 210 | Image Topic: /match_image 211 | Max Value: 1 212 | Median window: 5 213 | Min Value: 0 214 | Name: Match Image 215 | Normalize Range: true 216 | Queue Size: 2 217 | Transport Hint: raw 218 | Unreliable: false 219 | Value: true 220 | - Class: rviz/Image 221 | Enabled: true 222 | Image Topic: /template_image 223 | Max Value: 1 224 | Median window: 5 225 | Min Value: 0 226 | Name: Template Image 227 | Normalize Range: true 228 | Queue Size: 2 229 | Transport Hint: raw 230 | Unreliable: false 231 | Value: true 232 | - Class: rviz/Image 233 | Enabled: true 234 | Image Topic: /submap_image 235 | Max Value: 1 236 | Median window: 5 237 | Min Value: 0 238 | Name: Map Image 239 | Normalize Range: true 240 | Queue Size: 2 241 | Transport Hint: raw 242 | Unreliable: false 243 | Value: true 244 | Enabled: true 245 | Global Options: 246 | Background Color: 48; 48; 48 247 | Default Light: true 248 | Fixed Frame: map 249 | Frame Rate: 30 250 | Name: root 251 | Tools: 252 | - Class: rviz/Interact 253 | Hide Inactive Objects: true 254 | - Class: rviz/MoveCamera 255 | - Class: rviz/Select 256 | - Class: rviz/FocusCamera 257 | - Class: rviz/Measure 258 | - Class: rviz/SetInitialPose 259 | Theta std deviation: 0.2617993950843811 260 | Topic: /initialpose 261 | X std deviation: 0.5 262 | Y std deviation: 0.5 263 | - Class: rviz/SetGoal 264 | Topic: /move_base_simple/goal 265 | - Class: rviz/PublishPoint 266 | Single click: true 267 | Topic: /clicked_point 268 | Value: true 269 | Views: 270 | Current: 271 | Class: rviz/Orbit 272 | Distance: 20 273 | Enable Stereo Rendering: 274 | Stereo Eye Separation: 0.06 275 | Stereo Focal Distance: 1 276 | Swap Stereo Eyes: false 277 | Value: false 278 | Focal Point: 279 | X: 0 280 | Y: 0 281 | Z: 0 282 | Focal Shape Fixed Size: true 283 | Focal Shape Size: 0.05 284 | Invert Z Axis: false 285 | Name: Current View 286 | Near Clip Distance: 0.01 287 | Pitch: 1.5697963237762451 288 | Target Frame: estimated 289 | Value: Orbit (rviz) 290 | Yaw: 4.713385581970215 291 | Saved: ~ 292 | Window Geometry: 293 | Displays: 294 | collapsed: true 295 | Height: 1025 296 | Hide Left Dock: true 297 | Hide Right Dock: false 298 | Map Image: 299 | collapsed: false 300 | Match Image: 301 | collapsed: false 302 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003cefc0200000009fb000000100044006900730070006c0061007900730000000016000003ce000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800570065006900670068007400200049006d006100670065020000058d0000009b00000280000001e00000000100000160000003cefc0200000007fb00000012004d0061007000200049006d0061006700650100000016000001410000001600fffffffb0000001c00540065006d0070006c00610074006500200049006d006100670065010000015d000001440000001600fffffffb00000016004d006100740063006800200049006d00610067006501000002a70000013d0000001600fffffffb0000001200530065006c0065006300740069006f006e0000000016000003ce0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000016000003ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000780000000c9fc0100000002fb0000000800540069006d006500000000000000073d000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005d7000003ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 303 | Selection: 304 | collapsed: false 305 | Template Image: 306 | collapsed: false 307 | Time: 308 | collapsed: false 309 | Tool Properties: 310 | collapsed: false 311 | Views: 312 | collapsed: false 313 | Width: 1853 314 | X: 67 315 | Y: 27 316 | -------------------------------------------------------------------------------- /hawkeye/rviz/hawkeye_ws.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.5 9 | Tree Height: 835 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Class: rviz/TF 36 | Enabled: true 37 | Frame Timeout: 15 38 | Frames: 39 | All Enabled: true 40 | Marker Scale: 1 41 | Name: TF 42 | Show Arrows: true 43 | Show Axes: true 44 | Show Names: true 45 | Update Interval: 0 46 | Value: true 47 | - Alpha: 1 48 | Buffer Length: 1 49 | Class: rviz/Path 50 | Color: 114; 159; 207 51 | Enabled: true 52 | Head Diameter: 0.3 53 | Head Length: 0.2 54 | Length: 0.3 55 | Line Style: Lines 56 | Line Width: 0.03 57 | Name: Raw 58 | Offset: 59 | X: 0 60 | Y: 0 61 | Z: 0 62 | Pose Color: 255; 85; 255 63 | Pose Style: None 64 | Radius: 0.03 65 | Shaft Diameter: 0.1 66 | Shaft Length: 0.1 67 | Topic: /raw_path 68 | Unreliable: false 69 | Value: true 70 | - Alpha: 1 71 | Buffer Length: 1 72 | Class: rviz/Path 73 | Color: 173; 127; 168 74 | Enabled: true 75 | Head Diameter: 0.3 76 | Head Length: 0.2 77 | Length: 0.3 78 | Line Style: Lines 79 | Line Width: 0.03 80 | Name: Estimated 81 | Offset: 82 | X: 0 83 | Y: 0 84 | Z: 0 85 | Pose Color: 255; 85; 255 86 | Pose Style: None 87 | Radius: 0.03 88 | Shaft Diameter: 0.1 89 | Shaft Length: 0.1 90 | Topic: /estimated_path 91 | Unreliable: false 92 | Value: true 93 | - Alpha: 0.7 94 | Class: rviz/Map 95 | Color Scheme: map 96 | Draw Behind: false 97 | Enabled: true 98 | Name: Map 99 | Topic: /ortho_map 100 | Unreliable: false 101 | Use Timestamp: false 102 | Value: true 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 10 107 | Min Value: -10 108 | Value: true 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/PointCloud2 112 | Color: 255; 255; 255 113 | Color Transformer: Intensity 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Min Color: 0; 0; 0 119 | Name: PointCloud2 120 | Position Transformer: XYZ 121 | Queue Size: 10 122 | Selectable: true 123 | Size (Pixels): 3 124 | Size (m): 0.01 125 | Style: Points 126 | Topic: /point_cloud 127 | Unreliable: false 128 | Use Fixed Frame: true 129 | Use rainbow: true 130 | Value: true 131 | - Class: rviz/MarkerArray 132 | Enabled: true 133 | Marker Topic: /histogram 134 | Name: Histogram 135 | Namespaces: 136 | histogram: true 137 | Queue Size: 100 138 | Value: true 139 | - Alpha: 1 140 | Buffer Length: 1 141 | Class: rviz/Path 142 | Color: 252; 233; 79 143 | Enabled: false 144 | Head Diameter: 0.3 145 | Head Length: 0.2 146 | Length: 0.3 147 | Line Style: Lines 148 | Line Width: 0.03 149 | Name: Histogram Peak 150 | Offset: 151 | X: 0 152 | Y: 0 153 | Z: 0 154 | Pose Color: 255; 85; 255 155 | Pose Style: None 156 | Radius: 0.03 157 | Shaft Diameter: 0.1 158 | Shaft Length: 0.1 159 | Topic: /histogram_peak_path 160 | Unreliable: false 161 | Value: false 162 | - Alpha: 1 163 | Buffer Length: 1 164 | Class: rviz/Path 165 | Color: 240; 50; 230 166 | Enabled: false 167 | Head Diameter: 0.3 168 | Head Length: 0.2 169 | Length: 0.3 170 | Line Style: Lines 171 | Line Width: 0.03 172 | Name: Match Peak 173 | Offset: 174 | X: 0 175 | Y: 0 176 | Z: 0 177 | Pose Color: 255; 85; 255 178 | Pose Style: None 179 | Radius: 0.03 180 | Shaft Diameter: 0.1 181 | Shaft Length: 0.1 182 | Topic: /match_peak_path 183 | Unreliable: false 184 | Value: false 185 | - Alpha: 1 186 | Buffer Length: 1 187 | Class: rviz/Path 188 | Color: 245; 121; 0 189 | Enabled: false 190 | Head Diameter: 0.3 191 | Head Length: 0.2 192 | Length: 0.3 193 | Line Style: Lines 194 | Line Width: 0.03 195 | Name: Histogram Center 196 | Offset: 197 | X: 0 198 | Y: 0 199 | Z: 0 200 | Pose Color: 255; 85; 255 201 | Pose Style: None 202 | Radius: 0.03 203 | Shaft Diameter: 0.1 204 | Shaft Length: 0.1 205 | Topic: /histogram_center_path 206 | Unreliable: false 207 | Value: false 208 | - Class: rviz/Image 209 | Enabled: true 210 | Image Topic: /match_image 211 | Max Value: 1 212 | Median window: 5 213 | Min Value: 0 214 | Name: Match Image 215 | Normalize Range: true 216 | Queue Size: 2 217 | Transport Hint: raw 218 | Unreliable: false 219 | Value: true 220 | - Class: rviz/Image 221 | Enabled: true 222 | Image Topic: /template_image 223 | Max Value: 1 224 | Median window: 5 225 | Min Value: 0 226 | Name: Template Image 227 | Normalize Range: true 228 | Queue Size: 2 229 | Transport Hint: raw 230 | Unreliable: false 231 | Value: true 232 | - Class: rviz/Image 233 | Enabled: true 234 | Image Topic: /weight_image 235 | Max Value: 1 236 | Median window: 5 237 | Min Value: 0 238 | Name: Weight Image 239 | Normalize Range: true 240 | Queue Size: 2 241 | Transport Hint: raw 242 | Unreliable: false 243 | Value: true 244 | - Class: rviz/Image 245 | Enabled: true 246 | Image Topic: /submap_image 247 | Max Value: 1 248 | Median window: 5 249 | Min Value: 0 250 | Name: Map Image 251 | Normalize Range: true 252 | Queue Size: 2 253 | Transport Hint: raw 254 | Unreliable: false 255 | Value: true 256 | Enabled: true 257 | Global Options: 258 | Background Color: 48; 48; 48 259 | Default Light: true 260 | Fixed Frame: map 261 | Frame Rate: 30 262 | Name: root 263 | Tools: 264 | - Class: rviz/Interact 265 | Hide Inactive Objects: true 266 | - Class: rviz/MoveCamera 267 | - Class: rviz/Select 268 | - Class: rviz/FocusCamera 269 | - Class: rviz/Measure 270 | - Class: rviz/SetInitialPose 271 | Theta std deviation: 0.2617993950843811 272 | Topic: /initialpose 273 | X std deviation: 0.5 274 | Y std deviation: 0.5 275 | - Class: rviz/SetGoal 276 | Topic: /move_base_simple/goal 277 | - Class: rviz/PublishPoint 278 | Single click: true 279 | Topic: /clicked_point 280 | Value: true 281 | Views: 282 | Current: 283 | Class: rviz/Orbit 284 | Distance: 20 285 | Enable Stereo Rendering: 286 | Stereo Eye Separation: 0.06 287 | Stereo Focal Distance: 1 288 | Swap Stereo Eyes: false 289 | Value: false 290 | Focal Point: 291 | X: 0 292 | Y: 0 293 | Z: 0 294 | Focal Shape Fixed Size: true 295 | Focal Shape Size: 0.05 296 | Invert Z Axis: false 297 | Name: Current View 298 | Near Clip Distance: 0.01 299 | Pitch: 1.5697963237762451 300 | Target Frame: estimated 301 | Value: Orbit (rviz) 302 | Yaw: 4.713385581970215 303 | Saved: ~ 304 | Window Geometry: 305 | Displays: 306 | collapsed: true 307 | Height: 1025 308 | Hide Left Dock: true 309 | Hide Right Dock: false 310 | Map Image: 311 | collapsed: false 312 | Match Image: 313 | collapsed: false 314 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003cefc0200000009fb000000100044006900730070006c0061007900730000000016000003ce000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800570065006900670068007400200049006d0061006700650300000058000002ab0000016b000001680000000100000160000003cefc0200000007fb00000012004d0061007000200049006d0061006700650100000016000001410000001600fffffffb0000001c00540065006d0070006c00610074006500200049006d006100670065010000015d000001440000001600fffffffb00000016004d006100740063006800200049006d00610067006501000002a70000013d0000001600fffffffb0000001200530065006c0065006300740069006f006e0000000016000003ce0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000016000003ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000780000000c9fc0100000002fb0000000800540069006d006500000000000000073d000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005d7000003ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 315 | Selection: 316 | collapsed: false 317 | Template Image: 318 | collapsed: false 319 | Time: 320 | collapsed: false 321 | Tool Properties: 322 | collapsed: false 323 | Views: 324 | collapsed: false 325 | Weight Image: 326 | collapsed: true 327 | Width: 1853 328 | X: 67 329 | Y: 27 330 | -------------------------------------------------------------------------------- /orthomap_viewer/src/orthomap_viewer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #define SHOW_PARAM(val) std::cout << #val " : " << val << std::endl 41 | #define SHOW_PARAM2(val, exp) std::cout << #val " : " << exp << std::endl 42 | 43 | constexpr int valid_version = 3; 44 | 45 | void exitArg(const std::string& message = "") 46 | { 47 | std::cerr << "\033[1;31mError: Invalid arguments. " << message << "\033[0m" << std::endl; 48 | std::cerr << "./ortho_publisher [" 49 | << " -t |" 50 | << " -p |" 51 | << " -f ]*" << std::endl; 52 | exit(1); 53 | } 54 | 55 | void exitBadFile(const std::string& filename, const std::string& message = "") 56 | { 57 | std::cerr << "\033[1;31mError: Bad file: " << filename << ' ' << message << "\033[0m" << std::endl; 58 | exit(2); 59 | } 60 | 61 | template 62 | void getFileElements(std::ifstream& ifs, const std::string& filename, T&& ref) 63 | { 64 | if (!(ifs >> ref)) 65 | { 66 | exitBadFile(filename); 67 | } 68 | } 69 | 70 | std::optional draw_one(const std::string& topic_name, const std::string& tif_name, int seq); 71 | 72 | int main(int argc, char** argv) 73 | { 74 | ros::init(argc, argv, "orthomap_view"); 75 | std::string input_file; 76 | std::string topic_name = "ortho_map"; 77 | bool topic_name_param; 78 | std::optional pcd_name; 79 | std::optional frame_name; 80 | if (argc < 2) 81 | { 82 | exitArg("argc: " + std::to_string(argc)); 83 | } 84 | input_file = argv[1]; 85 | for (int i = 2; i < argc;) 86 | { 87 | if (!strcmp(argv[i], "-t")) 88 | { 89 | if (topic_name_param || argc < i + 2) 90 | { 91 | exitArg(); 92 | } 93 | topic_name_param = true; 94 | topic_name = argv[i + 1]; 95 | i += 2; 96 | } 97 | else if (!strcmp(argv[i], "-p")) 98 | { 99 | if (pcd_name || argc < i + 2) 100 | { 101 | exitArg(); 102 | } 103 | pcd_name = argv[i + 1]; 104 | i += 2; 105 | } 106 | else if (!strcmp(argv[i], "-f")) 107 | { 108 | if (frame_name || argc < i + 2) 109 | { 110 | exitArg(); 111 | } 112 | frame_name = argv[i + 1]; 113 | i += 2; 114 | } 115 | else 116 | { 117 | exitArg(); 118 | } 119 | } 120 | if (!frame_name) 121 | { 122 | frame_name = "map"; 123 | } 124 | SHOW_PARAM(topic_name); 125 | SHOW_PARAM(input_file); 126 | if (pcd_name) 127 | { 128 | SHOW_PARAM2(pcd_name, *pcd_name); 129 | } 130 | SHOW_PARAM2(frame_name, *frame_name); 131 | 132 | std::string prefix; 133 | std::string suffix; 134 | bool fill_zero; 135 | double scale; 136 | double pos_x; 137 | double pos_y; 138 | int div_x; 139 | int div_y; 140 | int width; 141 | int height; 142 | int num_images; 143 | bool scaled; 144 | double threshold_min; 145 | double threshold_max; 146 | bool classify_emptiness; 147 | std::vector> index_image; 148 | { 149 | std::ifstream ifs(input_file); 150 | if (!ifs) 151 | { 152 | std::cerr << "\033[1;31mError: Could not open " << input_file << "\033[0m" << std::endl; 153 | exit(2); 154 | } 155 | { 156 | std::string ext_name; 157 | int version; 158 | getFileElements(ifs, input_file, ext_name); 159 | getFileElements(ifs, input_file, version); 160 | if (ext_name != "orthomap") 161 | { 162 | exitBadFile(input_file, "ext_name = " + ext_name); 163 | } 164 | if (version != valid_version) 165 | { 166 | exitBadFile(input_file, "version = " + std::to_string(version)); 167 | } 168 | } 169 | getFileElements(ifs, input_file, std::quoted(prefix)); 170 | getFileElements(ifs, input_file, std::quoted(suffix)); 171 | ifs >> std::boolalpha; 172 | getFileElements(ifs, input_file, fill_zero); 173 | ifs >> std::noboolalpha; 174 | getFileElements(ifs, input_file, scale); 175 | getFileElements(ifs, input_file, pos_x); 176 | getFileElements(ifs, input_file, pos_y); 177 | getFileElements(ifs, input_file, div_x); 178 | getFileElements(ifs, input_file, div_y); 179 | getFileElements(ifs, input_file, width); 180 | getFileElements(ifs, input_file, height); 181 | getFileElements(ifs, input_file, num_images); 182 | ifs >> std::boolalpha; 183 | getFileElements(ifs, input_file, scaled); 184 | ifs >> std::noboolalpha; 185 | if (scaled) 186 | { 187 | getFileElements(ifs, input_file, threshold_min); 188 | getFileElements(ifs, input_file, threshold_min); 189 | ifs >> std::boolalpha; 190 | getFileElements(ifs, input_file, classify_emptiness); 191 | ifs >> std::noboolalpha; 192 | } 193 | else 194 | { 195 | classify_emptiness = false; 196 | } 197 | { 198 | std::vector v; 199 | double buf; 200 | while (ifs >> buf) 201 | { 202 | v.push_back(buf); 203 | } 204 | if (v.size() != div_x * div_y) 205 | { 206 | exitBadFile(input_file, " (" + std::to_string(v.size()) + ")"); 207 | } 208 | index_image.reserve(num_images); 209 | for (int row = 0; row < div_y; ++row) 210 | { 211 | for (int col = 0; col < div_x; ++col) 212 | { 213 | int id = row * div_x + col; 214 | if (v[id] == 1) 215 | { 216 | index_image.emplace_back(row, col); 217 | } 218 | } 219 | } 220 | } 221 | { 222 | auto filename_length = boost::filesystem::path(input_file).filename().string().size(); 223 | prefix = input_file.substr(0, input_file.size() - filename_length) + prefix; 224 | } 225 | } 226 | 227 | SHOW_PARAM2(prefix, std::quoted(prefix)); 228 | SHOW_PARAM2(suffix, std::quoted(suffix)); 229 | SHOW_PARAM2(fill_zero, std::boolalpha << fill_zero << std::noboolalpha); 230 | SHOW_PARAM(scale); 231 | SHOW_PARAM(pos_x); 232 | SHOW_PARAM(pos_y); 233 | SHOW_PARAM(div_x); 234 | SHOW_PARAM(div_y); 235 | SHOW_PARAM(width); 236 | SHOW_PARAM(height); 237 | SHOW_PARAM(num_images); 238 | SHOW_PARAM2(classify_emptiness, std::boolalpha << classify_emptiness << std::noboolalpha); 239 | 240 | ros::NodeHandle n("~"); 241 | ros::Publisher lane_publisher = n.advertise("ortho_images", 1, true); 242 | nav_msgs::OccupancyGrid grid; 243 | 244 | grid.info.width = width * div_x; 245 | grid.info.height = height * div_y; 246 | grid.info.resolution = scale; 247 | grid.data.resize(grid.info.width * grid.info.height, -1); 248 | grid.header.frame_id = *frame_name; 249 | 250 | grid.info.origin.position.x = pos_x - scale / 2; 251 | grid.info.origin.position.y = pos_y + scale / 2 - height * div_y * scale; 252 | grid.info.origin.position.z = 0; 253 | grid.info.origin.orientation.x = 0; 254 | grid.info.origin.orientation.y = 0; 255 | grid.info.origin.orientation.z = 0; 256 | grid.info.origin.orientation.w = 1; 257 | 258 | auto filename_length = (prefix + std::to_string(num_images - 1) + suffix).size(); 259 | auto pos_length = std::to_string(std::max(div_x, div_y) - 1).size() + 1; 260 | for (int i = 0; i < num_images; ++i) 261 | { 262 | std::string image_name; 263 | if (fill_zero) 264 | { 265 | std::ostringstream oss; 266 | oss << prefix << std::setw(std::to_string(num_images - 1).size()) << std::setfill('0') << i << suffix; 267 | image_name = oss.str(); 268 | } 269 | else 270 | { 271 | image_name = prefix + std::to_string(i) + suffix; 272 | } 273 | const auto& [row_image, col_image] = index_image[i]; 274 | std::cout << "loading " << std::setw(filename_length) << image_name; 275 | std::cout << " for [" << std::setw(pos_length) << row_image << ',' << std::setw(pos_length) << col_image << ']'; 276 | std::cout << " ..." << std::flush; 277 | cv::Mat image = cv::imread(image_name, 0); 278 | if (image.data == nullptr) 279 | { 280 | std::cerr << "\033[1;31mError: Could not open " << image_name << "\033[0m" << std::endl; 281 | exit(2); 282 | } 283 | const int row_base = row_image * height; 284 | const int row_max = height * div_y - 1; 285 | const int col_base = col_image * width; 286 | const int line_length = width * div_x; 287 | for (int row = 0; row < image.rows; ++row) 288 | { 289 | for (int col = 0; col < image.cols; ++col) 290 | { 291 | auto index = (row_max - (row + row_base)) * line_length + (col + col_base); 292 | if (classify_emptiness) 293 | { 294 | if (image.at(row, col) != 0) 295 | { 296 | grid.data[index] = (255 - image.at(row, col)) * 100 / 254.; 297 | } 298 | } 299 | else 300 | { 301 | grid.data[index] = (255 - image.at(row, col)) * 100 / 255.; 302 | } 303 | } 304 | } 305 | std::cout << " success." << std::endl; 306 | } 307 | lane_publisher.publish(grid); 308 | pcl::PointCloud pc; 309 | ros::Publisher pc_pub; 310 | if (pcd_name) 311 | { 312 | if (pcl::io::loadPCDFile(*pcd_name, pc) < 0) 313 | { 314 | std::cerr << "\033[1;31mError: Could not open " << *pcd_name << " as pcl::PointCloud \033[0m" 315 | << std::endl; 316 | exit(2); 317 | } 318 | for (auto& p : pc) 319 | { 320 | p.z = 0.000001; 321 | } 322 | pc_pub = n.advertise>("pcd_points", 1, true); 323 | pc.header.frame_id = *frame_name; 324 | pc_pub.publish(pc); 325 | std::cout << "publish point cloud" << std::endl; 326 | } 327 | std::cout << "finish and spin" << std::endl; 328 | ros::spin(); 329 | std::cout << std::endl; 330 | return 0; 331 | } -------------------------------------------------------------------------------- /hawkeye/src/hawkeye_base/node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | 28 | #include "hawkeye_base/node.hpp" 29 | #include "hawkeye_base/base.hpp" 30 | #include "util/timer.hpp" 31 | 32 | namespace hawkeye::hawkeye_base 33 | { 34 | constexpr bool use_cv_tm_mask_ = true; 35 | 36 | double getTrueHistogramWeight(double histogram_weight, double coeff_diminish, double coeff_weight, 37 | double coeff_negative) 38 | { 39 | if (histogram_weight == 0) 40 | { 41 | return 0; 42 | } 43 | double max_val = (coeff_weight - coeff_negative) / (1 - coeff_diminish); 44 | return histogram_weight / (max_val * (1 - histogram_weight) + histogram_weight); 45 | } 46 | 47 | template 48 | typename Hawkeye::Config Hawkeye::readYamlConfig(const std::string& yaml_filename) 49 | { 50 | Config conf = Config::defaultConfig(); 51 | conf.readYamlConfig(yaml_filename); 52 | return conf; 53 | } 54 | 55 | template 56 | Hawkeye::Hawkeye(double scale, const Config& config) 57 | : essential_time_{ 0 } 58 | , offset_{ 0, 0, 0 } 59 | , center_shift_{ 0, 0, 0 } 60 | , config_{ config } 61 | , counter_{ 0 } 62 | , histogram_weight_{ getTrueHistogramWeight(config.histogram_weight_, config.coeff_diminish_, config.coeff_weight_, 63 | config.coeff_negative_) } 64 | { 65 | histogram_ = generateInitialDistribution(config_.template_size_, config_.map_size_, scale); 66 | if constexpr (HawkeyeMode::is_weighted_) 67 | { 68 | std::cout << "histogram weight used in calculation : " << histogram_weight_ << std::endl; 69 | } 70 | } 71 | 72 | template 73 | tf2::Transform Hawkeye::shift(const tf2::Transform& pose) const 74 | { 75 | if (counter_ == 0) 76 | { 77 | return pose; 78 | } 79 | tf2::Transform ret = pose; 80 | ret.getOrigin() += offset_ + center_shift_; 81 | return ret; 82 | } 83 | 84 | template 85 | const tf2::Transform& Hawkeye::getLidarTf() const 86 | { 87 | return config_.lidar_pose_; 88 | } 89 | 90 | template 91 | void Hawkeye::adjustPCs() 92 | { 93 | if (config_.lidar_accumulate_max_length_ > 0. && pc_list_.size() > 1) 94 | { 95 | auto last_pos = pc_list_.back().second.getOrigin(); 96 | pc_list_.remove_if([&last_pos, threshold = config_.lidar_accumulate_max_length_](auto p) { 97 | return p.second.getOrigin().distance(last_pos) > threshold; 98 | }); 99 | if (config_.lidar_accumulate_max_count_ > 0) 100 | { 101 | while (pc_list_.size() > config_.lidar_accumulate_max_count_) 102 | { 103 | double min_d = config_.lidar_accumulate_max_length_ * 2; 104 | auto min_it = pc_list_.begin(); 105 | for (auto it = pc_list_.begin(), next = std::next(it); next != pc_list_.end(); ++it, ++next) 106 | { 107 | if (double d = tf2::tf2Distance(it->second.getOrigin(), next->second.getOrigin()); d < min_d) 108 | { 109 | min_d = d; 110 | min_it = it; 111 | } 112 | } 113 | pc_list_.erase(min_it); 114 | } 115 | } 116 | } 117 | else if (config_.lidar_accumulate_max_count_ > 0) 118 | { 119 | while (config_.lidar_accumulate_max_count_ < pc_list_.size()) 120 | { 121 | pc_list_.pop_front(); 122 | } 123 | } 124 | } 125 | 126 | template 127 | tf2::Transform Hawkeye::update(const tf2::Transform& pose, const PC_t::Ptr pc_ptr, OrthoMap& map, 128 | bool print_info) 129 | { 130 | Stopwatch sw; 131 | essential_time_ = 0; 132 | tf2::Transform ret; 133 | 134 | // updating point cloud list -------------------------------------------------- 135 | if (print_info) 136 | { 137 | std::cout << "updating point cloud list ... " << std::flush; 138 | } 139 | sw.reset(); 140 | { 141 | pc_list_.emplace_back(pc_ptr, pose); 142 | adjustPCs(); 143 | } 144 | essential_time_ += sw.count(); 145 | if (print_info) 146 | { 147 | std::cout << "done in " << sw.get() << " ms ( size : " << pc_list_.size() << " )" << std::endl; 148 | } 149 | 150 | // skip if stopping -------------------------------------------------- 151 | if ((odometry_.getOrigin() - pose.getOrigin()).length() < config_.stop_threshold_ && counter_ != 0) 152 | { 153 | if (print_info) 154 | { 155 | std::cout << "skip histgram update while stopping" << std::endl; 156 | } 157 | ret = pose; 158 | ret.getOrigin() += center_shift_ + offset_; 159 | } 160 | else 161 | { 162 | if (counter_ != 0) 163 | { 164 | // generating prior -------------------------------------------------- 165 | if (print_info) 166 | { 167 | std::cout << "generating prior distribution ... " << std::flush; 168 | } 169 | sw.reset(); 170 | histogram_ = hawkeye_base::generatePrior(shifted_histogram_, odometry_.inverseTimes(pose), 171 | config_.error_rate_); 172 | essential_time_ += sw.count(); 173 | if (print_info) 174 | { 175 | std::cout << "done in " << sw.get() << " ms" << std::endl; 176 | } 177 | } 178 | 179 | // generating template image -------------------------------------------------- 180 | if (print_info) 181 | { 182 | std::cout << "generating template image ... " << std::flush; 183 | } 184 | sw.reset(); 185 | histogram_center_ = pose; 186 | histogram_center_.getOrigin() += center_shift_; 187 | template_image_ = generateTemplateImage( 188 | pc_list_, config_.lidar_pose_, config_.template_size_, std::get<2>(histogram_), pose.getOrigin(), 189 | config_.intensity_accumulate_threshold_min_, config_.intensity_accumulate_threshold_max_); 190 | essential_time_ += sw.count(); 191 | if (print_info) 192 | { 193 | std::cout << "done in " << sw.get() << " ms" << std::endl; 194 | } 195 | 196 | // generating map image -------------------------------------------------- 197 | if (print_info) 198 | { 199 | std::cout << "generating map image ... " << std::flush; 200 | } 201 | sw.reset(); 202 | submap_ = map.getImageRange(histogram_center_.getOrigin(), config_.map_size_.width, config_.map_size_.height); 203 | essential_time_ += sw.count(); 204 | if (print_info) 205 | { 206 | std::cout << "done in " << sw.get() << " ms" << std::endl; 207 | std::cout << "histogram center : " << histogram_center_.getOrigin().x() << ", " 208 | << histogram_center_.getOrigin().y(); 209 | std::cout << " ( " << center_shift_.x() << ", " << center_shift_.y(); 210 | std::cout << " / " << offset_.x() << ", " << offset_.y() << " )" << std::endl; 211 | } 212 | 213 | // matching -------------------------------------------------- 214 | if (print_info) 215 | { 216 | std::cout << "matching ... " << std::flush; 217 | } 218 | sw.reset(); 219 | match_result_ = matchTemplate(submap_, map.getScale(), template_image_, true); 220 | essential_time_ += sw.count(); 221 | if (print_info) 222 | { 223 | double min, max; 224 | cv::minMaxIdx(std::get<0>(match_result_), &min, &max); 225 | std::cout << "done in " << sw.get() << " ms ... [ " << min << " , " << max << " ]" << std::endl; 226 | } 227 | 228 | // updating distribution -------------------------------------------------- 229 | if (print_info) 230 | { 231 | std::cout << "updating distribution ... " << std::flush; 232 | } 233 | sw.reset(); 234 | histogram_ = updateDistribution(histogram_, match_result_, config_.coeff_diminish_, 235 | config_.coeff_weight_, config_.coeff_negative_); 236 | essential_time_ += sw.count(); 237 | if (print_info) 238 | { 239 | std::cout << "done in " << sw.get() << " ms" << std::endl; 240 | } 241 | 242 | // updating offset -------------------------------------------------- 243 | if (print_info) 244 | { 245 | std::cout << "updating offset ... " << std::flush; 246 | } 247 | sw.reset(); 248 | weighted_histogram_ = getWeightHistogram(histogram_, config_.histogram_weight_); 249 | moments_ = getMoments(weighted_histogram_); 250 | average_ = average(weighted_histogram_, moments_); 251 | offset_ = updateOffset(offset_, average_, config_.coeff_gain_); 252 | essential_time_ += sw.count(); 253 | if (print_info) 254 | { 255 | std::cout << "done in " << sw.get() << " ms " 256 | << "( " << average_.x() << ", " << average_.y() << " " 257 | << "/ " << offset_.x() << ", " << offset_.y() << " )" << std::endl; 258 | } 259 | 260 | // updating histogram center -------------------------------------------------- 261 | if (print_info) 262 | { 263 | std::cout << "updating histogram center ... " << std::flush; 264 | } 265 | sw.reset(); 266 | tf2::Vector3 new_offset; 267 | std::tie(shifted_histogram_, center_shift_, new_offset) = shiftCenter( 268 | histogram_, center_shift_, offset_, config_.center_shift_threshold_, config_.edge_copy_shift_); 269 | essential_time_ += sw.count(); 270 | if (print_info) 271 | { 272 | std::cout << "done in " << sw.get() << " ms" << std::endl; 273 | } 274 | odometry_ = pose; 275 | ret = histogram_center_; 276 | ret.setOrigin(ret.getOrigin() + offset_); 277 | offset_ = new_offset; 278 | } 279 | 280 | ++counter_; 281 | return ret; 282 | } 283 | 284 | template 285 | tf2::Transform Hawkeye::getHistogramPeak() const 286 | { 287 | tf2::Transform ret = histogram_center_; 288 | ret.getOrigin() += average_; 289 | return ret; 290 | } 291 | 292 | template 293 | tf2::Transform Hawkeye::getShftedCenter() const 294 | { 295 | return histogram_center_; 296 | } 297 | 298 | template 299 | tf2::Transform Hawkeye::getMatchPeak() const 300 | { 301 | auto& [hist, weight, scale] = match_result_; 302 | cv::Mat positive(hist.size(), hist.type()); 303 | cv::threshold(hist, positive, 0, 0, cv::THRESH_TOZERO); 304 | tf2::Transform ret = histogram_center_; 305 | match_result_t positive_result(positive, weight, scale); 306 | ret.getOrigin() += average(positive_result, getMoments(positive_result)); 307 | return ret; 308 | } 309 | 310 | template 311 | cv::Mat Hawkeye::getTemplateImage() const 312 | { 313 | return std::get<0>(template_image_); 314 | } 315 | template 316 | cv::Mat Hawkeye::getMatchImage() const 317 | { 318 | return convertHistogram2CVU8C1(match_result_); 319 | } 320 | template 321 | cv::Mat Hawkeye::getSubmapImage() const 322 | { 323 | return submap_; 324 | } 325 | template 326 | cv::Mat Hawkeye::getWeightImage() const 327 | { 328 | return convertHistogram2CVU8C1(std::get<1>(histogram_)); 329 | } 330 | 331 | template 332 | const visualization_msgs::MarkerArray& Hawkeye::getHistogramMarkers(const std_msgs::Header& header, 333 | bool as_array) 334 | { 335 | setHistogramMarkers(marker_array_, weighted_histogram_, histogram_center_, header, as_array); 336 | return marker_array_; 337 | } 338 | 339 | template class Hawkeye; 340 | template class Hawkeye; 341 | 342 | } // namespace hawkeye::hawkeye_base -------------------------------------------------------------------------------- /hawkeye/src/hawkeye_rt/hawkeye_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY DIRECT, 19 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "hawkeye_define.hpp" 35 | #include "hawkeye_base/node.hpp" 36 | #include "map/orthomap.hpp" 37 | #include "util/arg.hpp" 38 | #include "util/timer.hpp" 39 | #include "util/error.hpp" 40 | #include "util/tf_messages.hpp" 41 | #include "util/pose_synchronize.hpp" 42 | 43 | using namespace hawkeye; 44 | 45 | double getYaw(const tf2::Transform& tf) 46 | { 47 | double roll, pitch, yaw; 48 | tf.getBasis().getRPY(roll, pitch, yaw); 49 | return yaw; 50 | } 51 | 52 | class HawkeyeM4Node 53 | { 54 | public: 55 | HawkeyeM4Node(const std::string& orthomap_filename, const std::string& config_filename, 56 | const std::string& lidar_topic_name, 57 | const hawkeye_base::HawkeyeConfig& config = hawkeye_base::HawkeyeConfig::defaultConfig()) 58 | : node_handle_{ "" }, counter_{ 0 }, pose_synchronizer_{ false }, times_{ 0 }, essential_times_{ 0 } 59 | { 60 | Stopwatch sw; 61 | 62 | std::cout << "initialization ... " << std::endl; 63 | sw.reset(); 64 | 65 | // making map -------------------------------------------------- 66 | map_.emplace(orthomap_filename); 67 | 68 | // setting ros publishers -------------------------------------------------- 69 | pose_synchronizer_.addPoseSubscriber(node_handle_, "/eagleye/pose", 100); 70 | pose_synchronizer_.addEagleyeCheckSubscriber(node_handle_, "/eagleye/fix", 100); 71 | pose_synchronizer_.addTopicSubscriber(node_handle_, lidar_topic_name, 4); 72 | 73 | broadcaster_.emplace(node_handle_); 74 | broadcaster_->addNewPublisher("raw_tf", "raw_path", "local_map"); 75 | broadcaster_->addNewPublisher("histogram_center", "histogram_center_path", "local_map"); 76 | broadcaster_->addNewPublisher("estimated", "estimated_path", "local_map"); 77 | broadcaster_->addNewPublisher("histogram_peak", "histogram_peak_path", "local_map"); 78 | broadcaster_->addNewPublisher("match_peak", "match_peak_path", "local_map"); 79 | 80 | pc_publisher_ = node_handle_.advertise("point_cloud", 5, false); 81 | map_publisher_ = node_handle_.advertise("ortho_map", 1, false); 82 | histogram_publisher_ = node_handle_.advertise("histogram", 5, false); 83 | template_image_publisher_ = node_handle_.advertise("template_image", 5); 84 | match_image_publisher_ = node_handle_.advertise("match_image", 5); 85 | submap_image_publisher_ = node_handle_.advertise("submap_image", 5); 86 | 87 | broadcaster_->broadcastStatic("local_map", map_->center(), ros::Time::now(), "map"); 88 | 89 | // setting filter -------------------------------------------------- 90 | hawkeye_.emplace(map_->getScale(), config); 91 | 92 | // other variables -------------------------------------------------- 93 | last_updated_ = false; 94 | 95 | sw.count(); 96 | std::cout << "initialization done in " << sw.get() << " ms" << std::endl; 97 | } 98 | 99 | bool runOnce(bool small_overhead = false) 100 | { 101 | auto&& synced = pose_synchronizer_.getLatest(); 102 | if (synced) 103 | { 104 | auto& [odometry, ptr, stamp] = *synced; 105 | update(ptr, odometry, stamp, small_overhead); 106 | std::cout << "average time : " << getAverageTime() << " ms (essential : " << getAverageEssential() << " ms)" 107 | << std::endl; 108 | pose_synchronizer_.dropToLatest(); 109 | last_updated_ = true; 110 | } 111 | else 112 | { 113 | pose_synchronizer_.dropInvalidTopics(); 114 | last_updated_ = false; 115 | } 116 | return true; 117 | } 118 | 119 | void run(double rate = 10, bool small_overhead = false) 120 | { 121 | std::cout << "start" << std::endl; 122 | ros::Rate r(rate); 123 | while (ros::ok()) 124 | { 125 | ros::spinOnce(); 126 | runOnce(small_overhead); 127 | r.sleep(); 128 | } 129 | } 130 | 131 | double getAverageTime() const 132 | { 133 | return counter_ == 0 ? -1 : (times_ / counter_); 134 | } 135 | double getAverageEssential() const 136 | { 137 | return counter_ == 0 ? -1 : (essential_times_ / counter_); 138 | } 139 | 140 | private: 141 | void update(const sensor_msgs::PointCloud2::Ptr& ptr, const tf2::Transform& odometry, const ros::Time stamp, 142 | bool small_overhead = false) 143 | { 144 | Stopwatch sw, loop_sw; 145 | double essential_time = 0; 146 | std::cout << "******** " << counter_ << "th loop ********" << std::endl; 147 | loop_sw.reset(); 148 | 149 | // ground segmentation -------------------------------------------------- 150 | PC_t::Ptr ground(new PC_t); 151 | pcl::fromROSMsg(*ptr, *ground); 152 | 153 | tf2::Transform pose = map_->center().inverseTimes(odometry); 154 | pose.getOrigin().setZ(0); 155 | tf2::Transform ans; 156 | 157 | // hawkeye -------------------------------------------------- 158 | ans = hawkeye_->update(pose, ground, *map_); 159 | essential_time += hawkeye_->getDuration(); 160 | std::cout << "estimated pose : " << ans.getOrigin().x() << " , " << ans.getOrigin().y() 161 | << " ( dir : " << getYaw(ans) << " )" << std::endl; 162 | 163 | { 164 | // publish tf and traj -------------------------------------------------- 165 | std::cout << "publish tf and traj ... " << std::endl; 166 | sw.reset(); 167 | 168 | ans.getOrigin().setZ(0); 169 | broadcaster_->broadcast("raw_tf", pose, stamp); 170 | broadcaster_->broadcast("estimated", ans, stamp); 171 | 172 | if (!small_overhead) 173 | { 174 | auto hcenter = hawkeye_->getShftedCenter(); 175 | auto hpeak = hawkeye_->getHistogramPeak(); 176 | auto mpeak = hawkeye_->getMatchPeak(); 177 | hcenter.getOrigin().setZ(0); 178 | hpeak.getOrigin().setZ(0); 179 | mpeak.getOrigin().setZ(0); 180 | broadcaster_->broadcast("histogram_center", hcenter, stamp); 181 | broadcaster_->broadcast("histogram_peak", hpeak, stamp); 182 | broadcaster_->broadcast("match_peak", mpeak, stamp); 183 | } 184 | 185 | sw.count(); 186 | std::cout << "publish tf and traj done in " << sw.get() << " ms" << std::endl; 187 | } 188 | if (!small_overhead) 189 | { 190 | { 191 | // publish ground point cloud -------------------------------------------------- 192 | std::cout << "publish point cloud ... " << std::flush; 193 | sw.reset(); 194 | geometry_msgs::TransformStamped tf_gm; 195 | PC_t shift_pc; 196 | sensor_msgs::PointCloud2 pcl_pc; 197 | tf2::convert(ans * hawkeye_->getLidarTf(), tf_gm.transform); 198 | // tf_gm.transform.translation.z = 0; 199 | pcl::transformPointCloud(*ground, shift_pc, tf2::transformToEigen(tf_gm).matrix()); 200 | pcl::toROSMsg(shift_pc, pcl_pc); 201 | pcl_pc.header = ptr->header; 202 | pcl_pc.header.frame_id = "local_map"; 203 | pc_publisher_.publish(pcl_pc); 204 | sw.count(); 205 | std::cout << "done in " << sw.get() << " ms" << std::endl; 206 | } 207 | { 208 | // publish images -------------------------------------------------- 209 | std::cout << "publish images ... " << std::flush; 210 | sw.reset(); 211 | std_msgs::Header h; 212 | h.frame_id = "raw_tf"; 213 | h.seq = 0; 214 | h.stamp = stamp; 215 | template_image_publisher_.publish( 216 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getTemplateImage()).toImageMsg()); 217 | match_image_publisher_.publish( 218 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getMatchImage()).toImageMsg()); 219 | submap_image_publisher_.publish( 220 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getSubmapImage()).toImageMsg()); 221 | sw.count(); 222 | std::cout << "done in " << sw.get() << " ms" << std::endl; 223 | } 224 | } 225 | { 226 | std_msgs::Header h; 227 | h.frame_id = "local_map"; 228 | h.seq = 0; 229 | h.stamp = stamp; 230 | { 231 | // publish histogram -------------------------------------------------- 232 | std::cout << "publish histogram ... " << std::flush; 233 | sw.reset(); 234 | 235 | histogram_publisher_.publish(hawkeye_->getHistogramMarkers(h)); 236 | 237 | sw.count(); 238 | std::cout << "done in " << sw.get() << " ms" << std::endl; 239 | } 240 | { 241 | // publish map -------------------------------------------------- 242 | std::cout << "publish map ... " << std::flush; 243 | sw.reset(); 244 | 245 | bool update_map = !map_->ckeckGridLatest(ans.getOrigin(), 1); 246 | if (update_map) 247 | { 248 | map_publisher_.publish(map_->getGridAround(h, ans.getOrigin(), 1)); 249 | } 250 | 251 | sw.count(); 252 | std::cout << "done in " << sw.get() << " ms"; 253 | if (!update_map) 254 | { 255 | std::cout << " ( not updated )"; 256 | } 257 | std::cout << std::endl; 258 | } 259 | } 260 | loop_sw.count(); 261 | std::cout << counter_++ << "th loop : totally " << loop_sw.get() << " ms " 262 | << "(essential : " << essential_time << " ms)" << std::endl; 263 | times_ += loop_sw.get(); 264 | essential_times_ += essential_time; 265 | } 266 | 267 | private: 268 | ros::NodeHandle node_handle_; 269 | std::optional map_; 270 | std::optional> hawkeye_; 271 | 272 | size_t counter_; 273 | 274 | bool last_updated_; 275 | 276 | PoseSychronizer pose_synchronizer_; 277 | 278 | std::optional broadcaster_; 279 | ros::Publisher pc_publisher_; 280 | ros::Publisher map_publisher_; 281 | ros::Publisher histogram_publisher_; 282 | ros::Publisher template_image_publisher_; 283 | ros::Publisher match_image_publisher_; 284 | ros::Publisher submap_image_publisher_; 285 | 286 | double times_; 287 | double essential_times_; 288 | }; 289 | 290 | void exitArg(const std::string& message = "") 291 | { 292 | std::cerr << ERROR_PREFIX << "Invalid arguments. " << message << ERROR_SUFFIX << std::endl; 293 | std::cerr << "./hawkeye_rt" 294 | << " " 295 | << " " 296 | << " " 297 | << " | -e " 298 | << " | -d " 299 | << " | -w " 300 | << " | -n " 301 | << " | -r " 302 | << " | -a " 303 | << " | -A " 304 | << " | -s " 305 | << " | -c" 306 | << " | -C" 307 | << " | -o" 308 | << " ]*" << std::endl; 309 | exit(1); 310 | } 311 | 312 | int main(int argc, char** argv) 313 | { 314 | ros::init(argc, argv, "hawkeye_rt"); 315 | char* orthomap_filename; 316 | char* config_filename; 317 | char* lidar_topic_name; 318 | hawkeye_base::HawkeyeConfig config = hawkeye_base::HawkeyeConfig::defaultConfig(); 319 | bool small_overhead = false; 320 | { 321 | argLoader param_loader(argc, argv, exitArg); 322 | param_loader.drop(); 323 | orthomap_filename = param_loader.pop(); 324 | config_filename = param_loader.pop(); 325 | lidar_topic_name = param_loader.pop(); 326 | 327 | std::cout << " orthomap_filename : " << std::quoted(orthomap_filename) << std::endl; 328 | std::cout << " config_filename : " << std::quoted(config_filename) << std::endl; 329 | std::cout << " lidar_topic_name : " << std::quoted(lidar_topic_name) << std::endl; 330 | 331 | config.readYamlConfig(config_filename); 332 | while (param_loader.left() > 0) 333 | { 334 | char* name = param_loader.pop(); 335 | if (name[0] != '-') 336 | { 337 | param_loader.err(); 338 | } 339 | switch (name[1]) 340 | { 341 | case 'e': 342 | { 343 | config.error_rate_ = std::atof(param_loader.pop()); 344 | break; 345 | } 346 | case 'd': 347 | { 348 | config.coeff_diminish_ = std::atof(param_loader.pop()); 349 | break; 350 | } 351 | case 'w': 352 | { 353 | config.coeff_weight_ = std::atof(param_loader.pop()); 354 | break; 355 | } 356 | case 'n': 357 | { 358 | config.coeff_negative_ = std::atof(param_loader.pop()); 359 | break; 360 | } 361 | case 'r': 362 | { 363 | config.coeff_gain_ = std::atof(param_loader.pop()); 364 | break; 365 | } 366 | case 'a': 367 | { 368 | config.lidar_accumulate_max_count_ = std::max(0l, std::atol(param_loader.pop())); 369 | break; 370 | } 371 | case 'A': 372 | { 373 | config.lidar_accumulate_max_length_ = std::max(0., std::atof(param_loader.pop())); 374 | break; 375 | } 376 | case 's': 377 | { 378 | config.center_shift_threshold_ = std::clamp(std::atof(param_loader.pop()), 0., 1.); 379 | break; 380 | } 381 | case 'o': 382 | { 383 | small_overhead = true; 384 | break; 385 | } 386 | case 'c': 387 | { 388 | config.edge_copy_shift_ = true; 389 | break; 390 | } 391 | case 'C': 392 | { 393 | config.edge_copy_shift_ = false; 394 | break; 395 | } 396 | default: 397 | { 398 | param_loader.err(); 399 | } 400 | } 401 | } 402 | } 403 | 404 | std::cout << "(-e) error_rate : " << config.error_rate_ << std::endl; 405 | std::cout << "(-d) coeff_diminish : " << config.coeff_diminish_ << std::endl; 406 | std::cout << "(-w) coeff_weight : " << config.coeff_weight_ << std::endl; 407 | std::cout << "(-n) coeff_negative : " << config.coeff_negative_ << std::endl; 408 | std::cout << "(-g) coeff_gain : " << config.coeff_gain_ << std::endl; 409 | std::cout << "(-a) accumulation_count : " << config.lidar_accumulate_max_count_ << std::endl; 410 | std::cout << "(-A) accumulation_length : " << config.lidar_accumulate_max_length_ << std::endl; 411 | std::cout << "(-s) center_shift_th : " << config.center_shift_threshold_ << std::endl; 412 | std::cout << "(-cC) edge_copy_shift : " << std::boolalpha << config.edge_copy_shift_ << std::noboolalpha 413 | << std::endl; 414 | std::cout << "intensity_threshold_min : " << config.intensity_accumulate_threshold_min_ << std::endl; 415 | std::cout << "intensity_threshold_max : " << config.intensity_accumulate_threshold_max_ << std::endl; 416 | std::cout << "stop_threshold : " << config.stop_threshold_ << std::endl; 417 | std::cout << "(-o) small_overhead : " << std::boolalpha << small_overhead << std::noboolalpha << std::endl; 418 | 419 | HawkeyeM4Node node(orthomap_filename, config_filename, lidar_topic_name, config); 420 | 421 | node.run(10, small_overhead); 422 | 423 | return 0; 424 | } -------------------------------------------------------------------------------- /hawkeye/src/hawkeye_rt/hawkeye_ws_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "hawkeye_define.hpp" 35 | #include "hawkeye_base/node.hpp" 36 | #include "map/orthomap.hpp" 37 | #include "util/arg.hpp" 38 | #include "util/timer.hpp" 39 | #include "util/error.hpp" 40 | #include "util/tf_messages.hpp" 41 | #include "util/pose_synchronize.hpp" 42 | 43 | using namespace hawkeye; 44 | 45 | double getYaw(const tf2::Transform& tf) 46 | { 47 | double roll, pitch, yaw; 48 | tf.getBasis().getRPY(roll, pitch, yaw); 49 | return yaw; 50 | } 51 | 52 | class HawkeyeWSM4Node 53 | { 54 | public: 55 | HawkeyeWSM4Node(const std::string& orthomap_filename, const std::string& config_filename, 56 | const std::string& lidar_topic_name, 57 | const hawkeye_base::HawkeyeConfig& config = hawkeye_base::HawkeyeConfig::defaultConfig()) 58 | : node_handle_{ "" }, counter_{ 0 }, pose_synchronizer_{ false }, times_{ 0 }, essential_times_{ 0 } 59 | { 60 | Stopwatch sw; 61 | 62 | std::cout << "initialization ... " << std::endl; 63 | sw.reset(); 64 | 65 | // making map -------------------------------------------------- 66 | map_.emplace(orthomap_filename); 67 | 68 | // setting ros publishers -------------------------------------------------- 69 | pose_synchronizer_.addPoseSubscriber(node_handle_, "/eagleye/pose", 100); 70 | pose_synchronizer_.addEagleyeCheckSubscriber(node_handle_, "/eagleye/fix", 100); 71 | pose_synchronizer_.addTopicSubscriber(node_handle_, lidar_topic_name, 4); 72 | 73 | broadcaster_.emplace(node_handle_); 74 | broadcaster_->addNewPublisher("raw_tf", "raw_path", "local_map"); 75 | broadcaster_->addNewPublisher("histogram_center", "histogram_center_path", "local_map"); 76 | broadcaster_->addNewPublisher("estimated", "estimated_path", "local_map"); 77 | broadcaster_->addNewPublisher("histogram_peak", "histogram_peak_path", "local_map"); 78 | broadcaster_->addNewPublisher("match_peak", "match_peak_path", "local_map"); 79 | 80 | pc_publisher_ = node_handle_.advertise("point_cloud", 5, false); 81 | map_publisher_ = node_handle_.advertise("ortho_map", 1, false); 82 | histogram_publisher_ = node_handle_.advertise("histogram", 5, false); 83 | template_image_publisher_ = node_handle_.advertise("template_image", 5); 84 | match_image_publisher_ = node_handle_.advertise("match_image", 5); 85 | submap_image_publisher_ = node_handle_.advertise("submap_image", 5); 86 | weight_image_publisher_ = node_handle_.advertise("weight_image", 5); 87 | 88 | broadcaster_->broadcastStatic("local_map", map_->center(), ros::Time::now(), "map"); 89 | 90 | // setting filter -------------------------------------------------- 91 | hawkeye_.emplace(map_->getScale(), config); 92 | 93 | // other variables -------------------------------------------------- 94 | last_updated_ = false; 95 | 96 | sw.count(); 97 | std::cout << "initialization done in " << sw.get() << " ms" << std::endl; 98 | } 99 | 100 | bool runOnce(bool small_overhead = false) 101 | { 102 | auto&& synced = pose_synchronizer_.getLatest(); 103 | if (synced) 104 | { 105 | auto& [odometry, ptr, stamp] = *synced; 106 | update(ptr, odometry, stamp, small_overhead); 107 | std::cout << "average time : " << getAverageTime() << " ms (essential : " << getAverageEssential() << " ms)" 108 | << std::endl; 109 | pose_synchronizer_.dropToLatest(); 110 | last_updated_ = true; 111 | } 112 | else 113 | { 114 | pose_synchronizer_.dropInvalidTopics(); 115 | last_updated_ = false; 116 | } 117 | return true; 118 | } 119 | 120 | void run(double rate = 10, bool small_overhead = false) 121 | { 122 | std::cout << "start" << std::endl; 123 | ros::Rate r(rate); 124 | while (ros::ok()) 125 | { 126 | ros::spinOnce(); 127 | runOnce(small_overhead); 128 | r.sleep(); 129 | } 130 | } 131 | 132 | double getAverageTime() const 133 | { 134 | return counter_ == 0 ? -1 : (times_ / counter_); 135 | } 136 | double getAverageEssential() const 137 | { 138 | return counter_ == 0 ? -1 : (essential_times_ / counter_); 139 | } 140 | 141 | private: 142 | void update(const sensor_msgs::PointCloud2::Ptr& ptr, const tf2::Transform& odometry, const ros::Time stamp, 143 | bool small_overhead = false) 144 | { 145 | Stopwatch sw, loop_sw; 146 | double essential_time = 0; 147 | std::cout << "******** " << counter_ << "th loop ********" << std::endl; 148 | loop_sw.reset(); 149 | 150 | // ground segmentation -------------------------------------------------- 151 | PC_t::Ptr ground(new PC_t); 152 | pcl::fromROSMsg(*ptr, *ground); 153 | 154 | tf2::Transform pose = map_->center().inverseTimes(odometry); 155 | pose.getOrigin().setZ(0); 156 | tf2::Transform ans; 157 | 158 | // hawkeye -------------------------------------------------- 159 | ans = hawkeye_->update(pose, ground, *map_); 160 | essential_time += hawkeye_->getDuration(); 161 | std::cout << "estimated pose : " << ans.getOrigin().x() << " , " << ans.getOrigin().y() 162 | << " ( dir : " << getYaw(ans) << " )" << std::endl; 163 | 164 | { 165 | // publish tf and traj -------------------------------------------------- 166 | std::cout << "publish tf and traj ... " << std::endl; 167 | sw.reset(); 168 | 169 | ans.getOrigin().setZ(0); 170 | broadcaster_->broadcast("raw_tf", pose, stamp); 171 | broadcaster_->broadcast("estimated", ans, stamp); 172 | 173 | if (!small_overhead) 174 | { 175 | auto hcenter = hawkeye_->getShftedCenter(); 176 | auto hpeak = hawkeye_->getHistogramPeak(); 177 | auto mpeak = hawkeye_->getMatchPeak(); 178 | hcenter.getOrigin().setZ(0); 179 | hpeak.getOrigin().setZ(0); 180 | mpeak.getOrigin().setZ(0); 181 | broadcaster_->broadcast("histogram_center", hcenter, stamp); 182 | broadcaster_->broadcast("histogram_peak", hpeak, stamp); 183 | broadcaster_->broadcast("match_peak", mpeak, stamp); 184 | } 185 | sw.count(); 186 | std::cout << "publish tf and traj done in " << sw.get() << " ms" << std::endl; 187 | } 188 | if (!small_overhead) 189 | { 190 | { 191 | // publish ground point cloud -------------------------------------------------- 192 | std::cout << "publish point cloud ... " << std::flush; 193 | sw.reset(); 194 | geometry_msgs::TransformStamped tf_gm; 195 | PC_t shift_pc; 196 | sensor_msgs::PointCloud2 pcl_pc; 197 | tf2::convert(ans * hawkeye_->getLidarTf(), tf_gm.transform); 198 | // tf_gm.transform.translation.z = 0; 199 | pcl::transformPointCloud(*ground, shift_pc, tf2::transformToEigen(tf_gm).matrix()); 200 | pcl::toROSMsg(shift_pc, pcl_pc); 201 | pcl_pc.header = ptr->header; 202 | pcl_pc.header.frame_id = "local_map"; 203 | pc_publisher_.publish(pcl_pc); 204 | sw.count(); 205 | std::cout << "done in " << sw.get() << " ms" << std::endl; 206 | } 207 | { 208 | // publish images -------------------------------------------------- 209 | std::cout << "publish images ... " << std::flush; 210 | sw.reset(); 211 | std_msgs::Header h; 212 | h.frame_id = "raw_tf"; 213 | h.seq = 0; 214 | h.stamp = stamp; 215 | template_image_publisher_.publish( 216 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getTemplateImage()).toImageMsg()); 217 | match_image_publisher_.publish( 218 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getMatchImage()).toImageMsg()); 219 | submap_image_publisher_.publish( 220 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getSubmapImage()).toImageMsg()); 221 | weight_image_publisher_.publish( 222 | cv_bridge::CvImage(h, sensor_msgs::image_encodings::TYPE_8UC1, hawkeye_->getWeightImage()).toImageMsg()); 223 | sw.count(); 224 | std::cout << "done in " << sw.get() << " ms" << std::endl; 225 | } 226 | } 227 | { 228 | std_msgs::Header h; 229 | h.frame_id = "local_map"; 230 | h.seq = 0; 231 | h.stamp = stamp; 232 | { 233 | // publish histogram -------------------------------------------------- 234 | std::cout << "publish histogram ... " << std::flush; 235 | sw.reset(); 236 | 237 | histogram_publisher_.publish(hawkeye_->getHistogramMarkers(h)); 238 | 239 | sw.count(); 240 | std::cout << "done in " << sw.get() << " ms" << std::endl; 241 | } 242 | { 243 | // publish map -------------------------------------------------- 244 | std::cout << "publish map ... " << std::flush; 245 | sw.reset(); 246 | 247 | bool update_map = !map_->ckeckGridLatest(ans.getOrigin(), 1); 248 | if (update_map) 249 | { 250 | map_publisher_.publish(map_->getGridAround(h, ans.getOrigin(), 1)); 251 | } 252 | 253 | sw.count(); 254 | std::cout << "done in " << sw.get() << " ms"; 255 | if (!update_map) 256 | { 257 | std::cout << " ( not updated )"; 258 | } 259 | std::cout << std::endl; 260 | } 261 | } 262 | loop_sw.count(); 263 | std::cout << counter_++ << "th loop : totally " << loop_sw.get() << " ms " 264 | << "(essential : " << essential_time << " ms)" << std::endl; 265 | times_ += loop_sw.get(); 266 | essential_times_ += essential_time; 267 | } 268 | 269 | private: 270 | ros::NodeHandle node_handle_; 271 | std::optional map_; 272 | std::optional> hawkeye_; 273 | 274 | size_t counter_; 275 | 276 | bool last_updated_; 277 | 278 | PoseSychronizer pose_synchronizer_; 279 | 280 | std::optional broadcaster_; 281 | ros::Publisher pc_publisher_; 282 | ros::Publisher map_publisher_; 283 | ros::Publisher histogram_publisher_; 284 | ros::Publisher template_image_publisher_; 285 | ros::Publisher match_image_publisher_; 286 | ros::Publisher submap_image_publisher_; 287 | ros::Publisher weight_image_publisher_; 288 | 289 | double times_; 290 | double essential_times_; 291 | 292 | size_t shift_count; 293 | }; 294 | 295 | void exitArg(const std::string& message = "") 296 | { 297 | std::cerr << ERROR_PREFIX << "Invalid arguments. " << message << ERROR_SUFFIX << std::endl; 298 | std::cerr << "./hawkeye_rt_ws" 299 | << " " 300 | << " " 301 | << " " 302 | << " | -e " 303 | << " | -d " 304 | << " | -w " 305 | << " | -n " 306 | << " | -r " 307 | << " | -a " 308 | << " | -A " 309 | << " | -s " 310 | << " | -H " 311 | << " | -o" 312 | << " ]*" << std::endl; 313 | exit(1); 314 | } 315 | 316 | int main(int argc, char** argv) 317 | { 318 | ros::init(argc, argv, "hawkeye_rt_ws"); 319 | char* orthomap_filename; 320 | char* config_filename; 321 | char* lidar_topic_name; 322 | hawkeye_base::HawkeyeConfig config = hawkeye_base::HawkeyeConfig::defaultConfig(); 323 | bool small_overhead = false; 324 | { 325 | argLoader param_loader(argc, argv, exitArg); 326 | param_loader.drop(); 327 | orthomap_filename = param_loader.pop(); 328 | config_filename = param_loader.pop(); 329 | lidar_topic_name = param_loader.pop(); 330 | 331 | std::cout << " orthomap_filename : " << std::quoted(orthomap_filename) << std::endl; 332 | std::cout << " config_filename : " << std::quoted(config_filename) << std::endl; 333 | std::cout << " lidar_topic_name : " << std::quoted(lidar_topic_name) << std::endl; 334 | 335 | config.readYamlConfig(config_filename); 336 | while (param_loader.left() > 0) 337 | { 338 | char* name = param_loader.pop(); 339 | if (name[0] != '-') 340 | { 341 | param_loader.err(); 342 | } 343 | switch (name[1]) 344 | { 345 | case 'e': 346 | { 347 | config.error_rate_ = std::atof(param_loader.pop()); 348 | break; 349 | } 350 | case 'd': 351 | { 352 | config.coeff_diminish_ = std::atof(param_loader.pop()); 353 | break; 354 | } 355 | case 'w': 356 | { 357 | config.coeff_weight_ = std::atof(param_loader.pop()); 358 | break; 359 | } 360 | case 'n': 361 | { 362 | config.coeff_negative_ = std::atof(param_loader.pop()); 363 | break; 364 | } 365 | case 'r': 366 | { 367 | config.coeff_gain_ = std::atof(param_loader.pop()); 368 | break; 369 | } 370 | case 'a': 371 | { 372 | config.lidar_accumulate_max_count_ = std::max(0l, std::atol(param_loader.pop())); 373 | break; 374 | } 375 | case 'A': 376 | { 377 | config.lidar_accumulate_max_length_ = std::max(0., std::atof(param_loader.pop())); 378 | break; 379 | } 380 | case 's': 381 | { 382 | config.center_shift_threshold_ = std::clamp(std::atof(param_loader.pop()), 0., 1.); 383 | break; 384 | } 385 | case 'H': 386 | { 387 | config.histogram_weight_ = std::clamp(std::atof(param_loader.pop()), 0., 1.); 388 | break; 389 | } 390 | case 'o': 391 | { 392 | small_overhead = true; 393 | break; 394 | } 395 | default: 396 | { 397 | param_loader.err(); 398 | } 399 | } 400 | } 401 | } 402 | 403 | std::cout << "(-e) error_rate : " << config.error_rate_ << std::endl; 404 | std::cout << "(-d) coeff_diminish : " << config.coeff_diminish_ << std::endl; 405 | std::cout << "(-w) coeff_weight : " << config.coeff_weight_ << std::endl; 406 | std::cout << "(-n) coeff_negative : " << config.coeff_negative_ << std::endl; 407 | std::cout << "(-g) coeff_gain : " << config.coeff_gain_ << std::endl; 408 | std::cout << "(-a) accumulation_count : " << config.lidar_accumulate_max_count_ << std::endl; 409 | std::cout << "(-A) accumulation_length : " << config.lidar_accumulate_max_length_ << std::endl; 410 | std::cout << "(-s) center_shift_th : " << config.center_shift_threshold_ << std::endl; 411 | std::cout << "(-H) histogram_weight : " << config.histogram_weight_ << std::endl; 412 | std::cout << "intensity_threshold_min : " << config.intensity_accumulate_threshold_min_ << std::endl; 413 | std::cout << "intensity_threshold_max : " << config.intensity_accumulate_threshold_max_ << std::endl; 414 | std::cout << "stop_threshold : " << config.stop_threshold_ << std::endl; 415 | std::cout << "(-o) small_overhead : " << std::boolalpha << small_overhead << std::noboolalpha << std::endl; 416 | 417 | HawkeyeWSM4Node node(orthomap_filename, config_filename, lidar_topic_name, config); 418 | 419 | node.run(10, small_overhead); 420 | 421 | return 0; 422 | } -------------------------------------------------------------------------------- /hawkeye/src/hawkeye_base/base.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022, Map IV, Inc. 2 | // All rights reserved. 3 | 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright notice, 9 | // this list of conditions and the following disclaimer in the documentation 10 | // and/or other materials provided with the distribution. 11 | // * Neither the name of the Map IV, Inc. nor the names of its contributors 12 | // may be used to endorse or promote products derived from this software 13 | // without specific prior written permission. 14 | 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | #include "hawkeye_base/base.hpp" 27 | #include "util/error.hpp" 28 | 29 | namespace hawkeye::hawkeye_base 30 | { 31 | constexpr double rate_interpolate = 0.666; 32 | 33 | template_image_t generateTemplateImage(const std::list& pc_list, const tf2::Transform& lidar_pose, 34 | cv::Size size, scale_t scale, double accum_min, double accum_max) 35 | { 36 | if (pc_list.empty()) 37 | { 38 | std::cerr << ERROR_PREFIX << "No point cloud." << ERROR_SUFFIX << std::endl; 39 | exit(4); 40 | } 41 | return generateTemplateImage(pc_list, lidar_pose, size, scale, pc_list.back().second.getOrigin(), accum_min, 42 | accum_max); 43 | } 44 | 45 | template_image_t generateTemplateImage(const std::list& pc_list, const tf2::Transform& lidar_pose, 46 | cv::Size size, scale_t scale, const tf2::Vector3& histogram_center, 47 | double accum_min, double accum_max) 48 | { 49 | accum_min = std::clamp(accum_min, 0., 1.); 50 | accum_max = std::clamp(accum_max, 0., 1.); 51 | bool use_thesh = accum_min != 0 || accum_max != 1; 52 | cv::Mat ret(size, CV_8U, cv::Scalar(0)); 53 | float accumulate_min, accumulate_range; 54 | if (use_thesh) 55 | { 56 | std::map intensity_accumulate_dist; 57 | size_t total_count = 0; 58 | { 59 | std::map intensity_dist; 60 | for (auto& [ptr, tf] : pc_list) 61 | { 62 | for (auto& p : *ptr) 63 | { 64 | ++intensity_dist[p.intensity]; 65 | } 66 | } 67 | for (auto& [key, val] : intensity_dist) 68 | { 69 | total_count += val; 70 | intensity_accumulate_dist.emplace_hint(intensity_accumulate_dist.end(), total_count, key); 71 | } 72 | } 73 | float accumulate_max; 74 | accumulate_min = intensity_accumulate_dist.lower_bound(std::floor(accum_min * total_count))->second; 75 | accumulate_max = intensity_accumulate_dist.upper_bound(std::ceil(accum_max * total_count) - 1)->second; 76 | if (accumulate_min >= accumulate_max) 77 | { 78 | accumulate_min = accumulate_max - 1; 79 | } 80 | accumulate_range = accumulate_max - accumulate_min; 81 | } 82 | 83 | for (auto& [ptr, tf] : pc_list) 84 | { 85 | tf2::Transform shift_rotate = tf * lidar_pose; 86 | shift_rotate.getOrigin() -= histogram_center; 87 | for (auto& p : *ptr) 88 | { 89 | tf2::Vector3 tf = shift_rotate * tf2::Vector3(p.x, p.y, 0); 90 | int w = std::floor(tf.x() / scale + size.width * 0.5); 91 | int h = size.height - 1 - std::floor(tf.y() / scale + size.height * 0.5); 92 | if (w >= 0 && w < size.width && h >= 0 && h < size.height) 93 | { 94 | if (use_thesh) 95 | { 96 | ret.at(cv::Point(w, h)) = 97 | std::round(std::clamp(p.intensity - accumulate_min, 0.f, accumulate_range) * 254. / accumulate_range + 1); 98 | } 99 | else 100 | { 101 | ret.at(cv::Point(w, h)) = std::clamp(p.intensity, 0, 255) * 254. / 255 + 1; 102 | } 103 | } 104 | } 105 | } 106 | return { ret, scale }; 107 | } 108 | 109 | template <> 110 | histogram_t generateInitialDistribution(cv::Size template_size, cv::Size map_size, scale_t scale) 111 | { 112 | return { cv::Mat(map_size - template_size + cv::Size(1, 1), CV_64F, cv::Scalar(0)), cv::Mat(), scale }; 113 | } 114 | template <> 115 | histogram_t generateInitialDistribution(cv::Size template_size, cv::Size map_size, 116 | scale_t scale) 117 | { 118 | return { cv::Mat(map_size - template_size + cv::Size(1, 1), CV_64F, cv::Scalar(0)), 119 | cv::Mat(map_size - template_size + cv::Size(1, 1), CV_64F, cv::Scalar(0)), scale }; 120 | } 121 | 122 | double getSigma(const tf2::Transform& shift, double error_coeff, double scale) 123 | { 124 | return std::hypot(shift.getOrigin().x(), shift.getOrigin().y()) * error_coeff / scale; 125 | } 126 | 127 | template 128 | histogram_t generatePrior_(const histogram_t& before, size_t kernel_size, double sigma) 129 | { 130 | auto& [hist_mat, weight_mat, scale] = before; 131 | cv::Mat ret_h(hist_mat.size(), hist_mat.type()); 132 | cv::GaussianBlur(hist_mat, ret_h, cv::Size(kernel_size, kernel_size), sigma, cv::BORDER_REPLICATE); 133 | if constexpr (HawkeyeMode::is_weighted_) 134 | { 135 | cv::Mat ret_w(weight_mat.size(), weight_mat.type()); 136 | cv::GaussianBlur(weight_mat, ret_w, cv::Size(kernel_size, kernel_size), sigma, cv::BORDER_REPLICATE); 137 | return { ret_h, ret_w, scale }; 138 | } 139 | else 140 | { 141 | return { ret_h, weight_mat, scale }; 142 | } 143 | } 144 | 145 | template 146 | histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, size_t kernel_size, 147 | double error_coeff) 148 | { 149 | return generatePrior_(before, kernel_size, getSigma(shift, error_coeff, std::get<2>(before))); 150 | } 151 | template histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, 152 | size_t kernel_size, double error_coeff); 153 | template histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, 154 | size_t kernel_size, double error_coeff); 155 | 156 | template 157 | histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, double error_coeff) 158 | { 159 | double sigma = getSigma(shift, error_coeff, std::get<2>(before)); 160 | size_t estimated_half = std::max(0., std::ceil((sigma - 0.5) / 0.3)); // inversed sigma calculation 161 | size_t kernel_size = std::clamp(estimated_half, 1, 5) * 2 + 1; 162 | return generatePrior_(before, kernel_size, sigma); 163 | } 164 | template histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, 165 | double error_coeff); 166 | template histogram_t generatePrior(const histogram_t& before, const tf2::Transform& shift, 167 | double error_coeff); 168 | 169 | match_result_t matchTemplate(const cv::Mat& map, scale_t map_scale, const template_image_t& template_image, 170 | bool use_mask) 171 | { 172 | if (std::get<1>(template_image) != map_scale) 173 | { 174 | std::cerr << ERROR_PREFIX 175 | << "the scale of the map image" 176 | " and the scale of the template image" 177 | " are different." 178 | << ERROR_SUFFIX << std::endl; 179 | exit(4); 180 | } 181 | cv::Mat ret_h; 182 | double ret_w; 183 | 184 | const cv::Mat& template_mat = std::get<0>(template_image); 185 | if (use_mask) 186 | { 187 | cv::Mat template_mask; 188 | template_mat.convertTo(template_mask, CV_32F); 189 | cv::threshold(template_mask, template_mask, 0, 1, cv::THRESH_BINARY); 190 | cv::matchTemplate(map, template_mat, ret_h, cv::TM_CCORR_NORMED, template_mask); // 32bit 191 | ret_w = 1; 192 | } 193 | else 194 | { 195 | cv::matchTemplate(map, template_mat, ret_h, cv::TM_CCORR_NORMED); // 32bit 196 | ret_w = 1; 197 | } 198 | if (false) 199 | { 200 | double color_min, color_max; 201 | cv::minMaxIdx(ret_h, &color_min, &color_max); 202 | std::cout << color_min << '~' << color_max; 203 | } 204 | ret_h.convertTo(ret_h, CV_64F); 205 | return { ret_h, ret_w, map_scale }; 206 | } 207 | 208 | template 209 | histogram_t updateDistribution(const histogram_t& prior, const match_result_t& match_result, double diminish, 210 | double weight, double negative) 211 | { 212 | auto& [prior_h, prior_w, prior_scale] = prior; 213 | auto& [match_h, match_w, match_scale] = match_result; 214 | if (prior_scale != match_scale) 215 | { 216 | std::cerr << ERROR_PREFIX 217 | << "the scale of the prior distribution" 218 | " and the scale of the match result" 219 | " are different." 220 | << ERROR_SUFFIX << std::endl; 221 | exit(4); 222 | } 223 | cv::Mat ret_h = prior_h * diminish + weight * (match_h - negative); 224 | cv::threshold(ret_h, ret_h, 0, 0, cv::ThresholdTypes::THRESH_TOZERO); 225 | if constexpr (HawkeyeMode::is_weighted_) 226 | { 227 | cv::Mat ret_w = prior_w * diminish + weight * (match_w - negative); 228 | return { ret_h, ret_w, prior_scale }; 229 | } 230 | else 231 | { 232 | return { ret_h, prior_w, prior_scale }; 233 | } 234 | } 235 | template histogram_t updateDistribution(const histogram_t& prior, 236 | const match_result_t& match_result, double diminish, 237 | double weight, double negative); 238 | template histogram_t updateDistribution(const histogram_t& prior, 239 | const match_result_t& match_result, double diminish, 240 | double weight, double negative); 241 | 242 | template <> 243 | weighted_histogram_t getWeightHistogram(const histogram_t& histogram, double weight_coeff) 244 | { 245 | auto& [hist, weight, scale] = histogram; 246 | return { hist, scale }; 247 | } 248 | template <> 249 | weighted_histogram_t getWeightHistogram(const histogram_t& histogram, double weight_coeff) 250 | { 251 | auto& [hist, weight, scale] = histogram; 252 | if (weight_coeff == 0) 253 | { 254 | return { hist, scale }; 255 | } 256 | return { hist / ((1 - weight_coeff) + weight_coeff * weight), scale }; 257 | } 258 | 259 | cv::Moments getMoments_(const cv::Mat& histogram) 260 | { 261 | cv::Mat thres_hist; 262 | { 263 | double color_min, color_max; 264 | cv::minMaxIdx(histogram, &color_min, &color_max); 265 | // cv::threshold(histogram.mul(histogram) - std::pow(color_min + (color_max - color_min) * rate_interpolate, 2), 266 | // thres_hist, 0, 0, cv::THRESH_TOZERO); 267 | cv::threshold(histogram - (color_min + (color_max - color_min) * rate_interpolate), thres_hist, 0, 0, 268 | cv::THRESH_TOZERO); 269 | } 270 | return cv::moments(thres_hist); 271 | } 272 | 273 | cv::Moments getMoments(const weighted_histogram_t& histogram) 274 | { 275 | return getMoments_(std::get<0>(histogram)); 276 | } 277 | cv::Moments getMoments(const match_result_t& histogram) 278 | { 279 | return getMoments_(std::get<0>(histogram)); 280 | } 281 | 282 | tf2::Vector3 average_(const cv::Size size, double scale, const cv::Moments& moments) 283 | { 284 | auto& m = moments; 285 | if (m.m00 == 0) 286 | { 287 | return { 0, 0, 0 }; 288 | } 289 | double gx = m.m10 / m.m00 - (size.width - 1) * 0.5; 290 | double gy = m.m01 / m.m00 - (size.height - 1) * 0.5; 291 | return { gx * scale, -gy * scale, 0 }; 292 | } 293 | 294 | tf2::Vector3 average(const weighted_histogram_t& histogram, const cv::Moments& moments) 295 | { 296 | auto& [hist, scale] = histogram; 297 | return average_(hist.size(), scale, moments); 298 | } 299 | tf2::Vector3 average(const match_result_t& histogram, const cv::Moments& moments) 300 | { 301 | auto& [hist, weight, scale] = histogram; 302 | return average_(hist.size(), scale, moments); 303 | } 304 | 305 | tf2::Vector3 updateOffset(const tf2::Vector3& prev, const tf2::Vector3& next, double coeff) 306 | { 307 | return (1 - coeff) * prev + coeff * next; 308 | } 309 | 310 | template 311 | std::tuple shiftCenter(const histogram_t& histogram, 312 | const tf2::Vector3& center, const tf2::Vector3& offset, 313 | double threshold_rate, bool edge_copy_shift) 314 | { 315 | auto& [hist, weight, scale] = histogram; 316 | auto size = hist.size(); 317 | double th_x = size.width * scale * 0.5 * threshold_rate; 318 | double th_y = size.height * scale * 0.5 * threshold_rate; 319 | double x = offset.getX(); 320 | double y = offset.getY(); 321 | if (std::abs(x) <= th_x && std::abs(y) <= th_y) 322 | { 323 | return { histogram, center, offset }; 324 | } 325 | cv::Mat next_hist, next_weight; 326 | if constexpr (HawkeyeMode::is_weighted_) 327 | { 328 | next_hist = cv::Mat(size, hist.type(), cv::Scalar(0)); 329 | next_weight = cv::Mat(size, hist.type(), cv::Scalar(0)); 330 | } 331 | else 332 | { 333 | if (edge_copy_shift) 334 | { 335 | next_hist = hist.clone(); 336 | } 337 | else 338 | { 339 | // double min, max; 340 | // cv::minMaxIdx(hist, &min, &max); 341 | // next_hist = cv::Mat(size, hist.type(), cv::Scalar(min)); 342 | next_hist = cv::Mat(size, hist.type(), cv::Scalar(0)); 343 | } 344 | } 345 | cv::Size rect_size = size; 346 | cv::Point rect_p_prev(0, 0), rect_p_next(0, 0); 347 | tf2::Vector3 center_next = center; 348 | tf2::Vector3 offset_next = offset; 349 | if (std::abs(x) > th_x) 350 | { 351 | rect_size.width -= 1; 352 | if (x < 0) 353 | { 354 | rect_p_next.x = 1; 355 | center_next.setX(center_next.getX() - scale); 356 | offset_next.setX(offset_next.getX() + scale); 357 | } 358 | else 359 | { 360 | rect_p_prev.x = 1; 361 | center_next.setX(center_next.getX() + scale); 362 | offset_next.setX(offset_next.getX() - scale); 363 | } 364 | } 365 | if (std::abs(y) > th_y) 366 | { 367 | rect_size.height -= 1; 368 | if (y < 0) 369 | { 370 | rect_p_prev.y = 1; 371 | center_next.setY(center_next.getY() - scale); 372 | offset_next.setY(offset_next.getY() + scale); 373 | } 374 | else 375 | { 376 | rect_p_next.y = 1; 377 | center_next.setY(center_next.getY() + scale); 378 | offset_next.setY(offset_next.getY() - scale); 379 | } 380 | } 381 | hist(cv::Rect(rect_p_prev, rect_size)).copyTo(next_hist(cv::Rect(rect_p_next, rect_size))); 382 | if constexpr (HawkeyeMode::is_weighted_) 383 | { 384 | weight(cv::Rect(rect_p_prev, rect_size)).copyTo(next_weight(cv::Rect(rect_p_next, rect_size))); 385 | } 386 | return { { next_hist, next_weight, scale }, center_next, offset_next }; 387 | } 388 | template std::tuple 389 | shiftCenter(const histogram_t& histogram, const tf2::Vector3& center, const tf2::Vector3& offset, 390 | double threshold_rate, bool edge_copy_shift); 391 | template std::tuple 392 | shiftCenter(const histogram_t& histogram, const tf2::Vector3& center, 393 | const tf2::Vector3& offset, double threshold_rate, bool edge_copy_shift); 394 | 395 | cv::Mat convertHistogram2CVU8C1(const cv::Mat& histogram) 396 | { 397 | double color_min, color_max; 398 | cv::minMaxIdx(histogram, &color_min, &color_max); 399 | 400 | double abs_max = std::max(std::abs(color_min), std::abs(color_max)); 401 | if (abs_max == 0) 402 | { 403 | abs_max = 1; 404 | } 405 | cv::Mat ret; 406 | { 407 | // ret = histogram * 255 / std::max(color_max, 1.); 408 | ret = (histogram - color_min) * 255 / (color_max - color_min); 409 | // ret = histogram * 127 / abs_max + 128; 410 | } 411 | ret.convertTo(ret, CV_8U); 412 | return ret; 413 | } 414 | cv::Mat convertHistogram2CVU8C1(const weighted_histogram_t& histogram) 415 | { 416 | auto& [hist, scale] = histogram; 417 | return convertHistogram2CVU8C1(hist); 418 | } 419 | cv::Mat convertHistogram2CVU8C1(const match_result_t& histogram) 420 | { 421 | auto& [hist, weight, scale] = histogram; 422 | return convertHistogram2CVU8C1(hist); 423 | } 424 | 425 | void setHistogramMarkers(visualization_msgs::MarkerArray& ma, const weighted_histogram_t& histogram, 426 | const tf2::Transform& odometry, const std_msgs::Header& header, bool as_array) 427 | { 428 | ma.markers.clear(); 429 | auto& [image, scale] = histogram; 430 | double max_weight, min_weight; 431 | { 432 | cv::minMaxIdx(image, &min_weight, &max_weight); 433 | if (max_weight == min_weight) 434 | { 435 | min_weight = max_weight - 1; 436 | } 437 | } 438 | cv::Size size = image.size(); 439 | auto f = [min_weight, max_weight, base_point = odometry.getOrigin(), size, scale = scale](double val, cv::Point at) { 440 | geometry_msgs::Point point_out; 441 | std_msgs::ColorRGBA color_out; 442 | double rank = (val - min_weight) / (max_weight - min_weight); 443 | // rank *= rank; 444 | color_out.r = std::clamp(2 * rank, 0., 1.); 445 | color_out.g = std::clamp(2 * (1 - rank), 0., 1.); 446 | if (val < min_weight + (max_weight - min_weight) * rate_interpolate) 447 | { 448 | color_out.r *= 0.5; 449 | color_out.g *= 0.5; 450 | } 451 | color_out.b = 0; 452 | color_out.a = 1; 453 | point_out.x = base_point.x() + (at.x - (size.width - 1) * 0.5) * scale; 454 | point_out.y = base_point.y() - (at.y - (size.height - 1) * 0.5) * scale; 455 | point_out.z = 0; 456 | return std::make_pair(point_out, color_out); 457 | }; 458 | auto checker = [min_weight, max_weight](double val) { return true; }; 459 | if (as_array) 460 | { 461 | ma.markers.reserve(size.width * size.height); 462 | 463 | visualization_msgs::Marker marker; 464 | { 465 | marker.header = header; 466 | marker.ns = "histogram"; 467 | marker.action = visualization_msgs::Marker::MODIFY; 468 | marker.type = visualization_msgs::Marker::ARROW; 469 | 470 | marker.scale.x = scale; 471 | marker.scale.y = scale; 472 | marker.scale.z = scale; 473 | 474 | marker.id = 0; 475 | } 476 | 477 | for (int w = 0; w < size.width; ++w) 478 | { 479 | for (int h = 0; h < size.height; ++h) 480 | { 481 | if (double val = image.at(cv::Point(w, h)); checker(val)) 482 | { 483 | std::tie(marker.pose.position, marker.color) = f(val, cv::Point(w, h)); 484 | ma.markers.push_back(marker); 485 | ++marker.id; 486 | } 487 | } 488 | } 489 | } 490 | else 491 | { 492 | auto& marker = ma.markers.emplace_back(); 493 | { 494 | marker.header = header; 495 | marker.ns = "histogram"; 496 | marker.action = visualization_msgs::Marker::MODIFY; 497 | marker.type = visualization_msgs::Marker::POINTS; 498 | 499 | marker.color.r = 0; 500 | marker.color.g = 1; 501 | marker.color.b = 0; 502 | marker.color.a = 1; 503 | 504 | marker.scale.x = scale; 505 | marker.scale.y = scale; 506 | marker.scale.z = scale; 507 | 508 | marker.id = 0; 509 | marker.points.reserve(size.width * size.height); 510 | marker.colors.reserve(size.width * size.height); 511 | } 512 | for (int w = 0; w < size.width; ++w) 513 | { 514 | for (int h = 0; h < size.height; ++h) 515 | { 516 | if (double val = image.at(cv::Point(w, h)); checker(val)) 517 | { 518 | auto [point, color] = f(val, cv::Point(w, h)); 519 | marker.points.emplace_back(point); 520 | marker.colors.emplace_back(color); 521 | } 522 | } 523 | } 524 | } 525 | return; 526 | } 527 | 528 | } // namespace hawkeye::hawkeye_base --------------------------------------------------------------------------------