├── .clang-format ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── apps ├── globalmap_server_nodelet.cpp └── hdl_localization_nodelet.cpp ├── data ├── figs │ ├── localization1.png │ ├── localization2.png │ └── packages.png └── map.pcd ├── docker ├── melodic │ └── Dockerfile ├── melodic_llvm │ └── Dockerfile ├── noetic │ └── Dockerfile └── noetic_llvm │ └── Dockerfile ├── include ├── hdl_localization │ ├── delta_estimater.hpp │ ├── odom_system.hpp │ ├── pose_estimator.hpp │ └── pose_system.hpp └── kkl │ └── alg │ └── unscented_kalman_filter.hpp ├── launch └── hdl_localization.launch ├── msg └── ScanMatchingStatus.msg ├── nodelet_plugins.xml ├── package.xml ├── rviz └── hdl_localization.rviz ├── scripts └── plot_status.py └── src └── hdl_localization └── pose_estimator.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: AlwaysBreak 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Left 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: false 13 | AllowAllConstructorInitializersOnNextLine: false 14 | AllowAllParametersOfDeclarationOnNextLine: false 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: Inline 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: WithoutElse 20 | AllowShortLoopsOnASingleLine: true 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: true 24 | AlwaysBreakTemplateDeclarations: Yes 25 | BinPackArguments: false 26 | BinPackParameters: false 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 180 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 57 | ConstructorInitializerIndentWidth: 0 58 | ContinuationIndentWidth: 2 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Regroup 70 | IncludeCategories: 71 | - Regex: '^' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^<.*\.h>' 75 | Priority: 1 76 | SortPriority: 0 77 | - Regex: '^<.*' 78 | Priority: 2 79 | SortPriority: 0 80 | - Regex: '.*' 81 | Priority: 3 82 | SortPriority: 0 83 | IncludeIsMainRegex: '([-_](test|unittest))?$' 84 | IncludeIsMainSourceRegex: '' 85 | IndentCaseLabels: true 86 | IndentGotoLabels: true 87 | IndentPPDirectives: None 88 | IndentWidth: 2 89 | IndentWrappedFunctionNames: false 90 | JavaScriptQuotes: Leave 91 | JavaScriptWrapImports: true 92 | KeepEmptyLinesAtTheStartOfBlocks: false 93 | MacroBlockBegin: '' 94 | MacroBlockEnd: '' 95 | MaxEmptyLinesToKeep: 1 96 | NamespaceIndentation: None 97 | ObjCBinPackProtocolList: Never 98 | ObjCBlockIndentWidth: 2 99 | ObjCSpaceAfterProperty: false 100 | ObjCSpaceBeforeProtocolList: true 101 | PenaltyBreakAssignment: 2 102 | PenaltyBreakBeforeFirstCallParameter: 1 103 | PenaltyBreakComment: 300 104 | PenaltyBreakFirstLessLess: 120 105 | PenaltyBreakString: 1000 106 | PenaltyBreakTemplateDeclaration: 10 107 | PenaltyExcessCharacter: 1000000 108 | PenaltyReturnTypeOnItsOwnLine: 200 109 | PointerAlignment: Left 110 | RawStringFormats: 111 | - Language: Cpp 112 | Delimiters: 113 | - cc 114 | - CC 115 | - cpp 116 | - Cpp 117 | - CPP 118 | - 'c++' 119 | - 'C++' 120 | CanonicalDelimiter: '' 121 | BasedOnStyle: google 122 | - Language: TextProto 123 | Delimiters: 124 | - pb 125 | - PB 126 | - proto 127 | - PROTO 128 | EnclosingFunctions: 129 | - EqualsProto 130 | - EquivToProto 131 | - PARSE_PARTIAL_TEXT_PROTO 132 | - PARSE_TEST_PROTO 133 | - PARSE_TEXT_PROTO 134 | - ParseTextOrDie 135 | - ParseTextProtoOrDie 136 | CanonicalDelimiter: '' 137 | BasedOnStyle: google 138 | ReflowComments: true 139 | SortIncludes: false 140 | SortUsingDeclarations: true 141 | SpaceAfterCStyleCast: false 142 | SpaceAfterLogicalNot: false 143 | SpaceAfterTemplateKeyword: true 144 | SpaceBeforeAssignmentOperators: true 145 | SpaceBeforeCpp11BracedList: false 146 | SpaceBeforeCtorInitializerColon: true 147 | SpaceBeforeInheritanceColon: true 148 | SpaceBeforeParens: ControlStatements 149 | SpaceBeforeRangeBasedForLoopColon: true 150 | SpaceInEmptyBlock: false 151 | SpaceInEmptyParentheses: false 152 | SpacesBeforeTrailingComments: 2 153 | SpacesInAngles: false 154 | SpacesInConditionalStatement: false 155 | SpacesInContainerLiterals: true 156 | SpacesInCStyleCastParentheses: false 157 | SpacesInParentheses: false 158 | SpacesInSquareBrackets: false 159 | SpaceBeforeSquareBrackets: false 160 | Standard: Auto 161 | StatementMacros: 162 | - Q_UNUSED 163 | - QT_REQUIRE_VERSION 164 | TabWidth: 8 165 | UseCRLF: false 166 | UseTab: Never 167 | ... 168 | 169 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | language: generic 3 | dist: focal 4 | services: 5 | - docker 6 | matrix: 7 | include: 8 | - name: Focal Fossa noetic 9 | env: ROS_DISTRO=noetic 10 | - name: Focal Fossa noetic_llvm 11 | env: ROS_DISTRO=noetic_llvm 12 | - name: Bionic melodic_llvm 13 | env: ROS_DISTRO=melodic_llvm 14 | - name: Bionic melodic 15 | env: ROS_DISTRO=melodic 16 | script: 17 | - echo "$DOCKER_PASSWORD" | docker login --username "$DOCKER_USERNAME" --password-stdin || true 18 | - docker build -f ./docker/$ROS_DISTRO/Dockerfile . 19 | env: 20 | global: 21 | - secure: S1eT1VgXSDwXPI24WCXidW5aQBEFvHZhRuwbLX52IBR9Abv3OLVhqcy+p/DVyfGgffdvflCTzupyfKpJtEhYvW9Poxg58aNe60acyAgaDeJ8wLbA1piOaBm3i4/Kx/rpbhHyRp+EANe1AC5W4o6z7+EB+IlFCjVq7C0t1ZfImWmJM/jD99l9g7oFu7eq+6+YyI7ZDQ41i7Jn3aid4OwOpFyW+JZ9WV+vAoCloNWWJb+cO0Gpeip2ei4tZonr67fQRcrWeFn0PJPmfhHzSINl0NgYq1Z7dOE7ngLS8CYGVYeZmUSy6+DJHYPfFFR6tPXkgp4BKEtG5JK0UFP2IA7Niv2jMvmvCAl7w/6+KF6kJV6K0F1MpSl3Zft/WtcsRzH1hKWQxegAG7mjrSLf2OreuE9D7TbYfFCinGAq3czyTB0EbMmq/qtkpzLk+mPrRZNQ8PVGf81Zd20Sg0sxPsth2qApQbJk9Src0R2jTfc8hvlxnzFNzPkm3EcDf+eA0cMLk5DwbMDqd4PddV+8KA4qCilLH3JOZSd/0FKcIIbZyLE6FaI4SyJH9ifjRjfUkaiNj0T/xI5ZoRv8uEKYGIASLDD8kzFpx3wwueTPk8EqbQhig/fVWQJH53IHkqwWRDIbQ6jUI8fkeLoxtMmNqCZZjDMBPIybmamfNMay0SqabYE= 22 | - secure: A12BEab6nt0TX4ZZgjiDoa1Si6gCrc66iMTMEJIjqCxe57Xs/KQB8PFux2GuqCrtba/DPjc7SVMrAYkBNFfNWMh8bZpd6qmSA1dn1JflKZ+JFSd7DqXA/az31v78sTTNKtn4jCRjt23RkO/K1u/qNCeQf5WtoTDjTcqRcnUIz922vlS/h0Pq1Pa4rpGQ1Y2fY/t4tIXytU81M/MHGwaG9tvktlv8Hlyf5FwztE2glLhUO/p0ltWbjlclA88/hEg8Q7fyebIwrWsFOAnsWCgPw2ii0Be0KdF7zhOoLQyBBOPDIPCOkjrVdcmWoha6eCox1Dq61qZQL/in9luYxV2yc9nUbuCIz1ZeZ5uL4f04Ucfd7ewuDP1LilIcSHzeOeoO2U5bcOEn2wnFwVx67BprH9t4eE6p/NlTSi1POqzFVdM0jiYItfitXehqoQwKXTtda/EDJbfkxFy8E0aOl1dyDmWLAh9mN1xGr0e1ZNdkwOS0fruSWNZCWCeraNdM63heddz9KY01cjeLMZxWAXcwJrHKNAYVNvXwTmWcbFp/e+nDP+ong2CkXqBaNurej2FqPxO+fLojH72OTejW9Lwlj7y5ckwedEAsP34Y71EGKiY1mqg+gMw+DvDqzMsUquN1+98bl9Hm4WyRdhL20Q6oZSEWqdrzREGNWx7NLnB11b4= 23 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hdl_localization) 3 | 4 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") 5 | add_definitions(-std=c++11) 6 | set(CMAKE_CXX_FLAGS "-std=c++11") 7 | else() 8 | # -mavx causes a lot of errors!! 9 | if("$ENV{ROS_DISTRO}" STRGREATER "melodic") 10 | add_definitions(-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 11 | set(CMAKE_CXX_FLAGS "-std=c++17 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 12 | else() 13 | add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 14 | set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 15 | endif() 16 | endif() 17 | 18 | # pcl 1.7 causes a segfault when it is built with debug mode 19 | set(CMAKE_BUILD_TYPE "RELEASE") 20 | 21 | find_package(catkin REQUIRED COMPONENTS 22 | nodelet 23 | tf2 24 | tf2_ros 25 | tf2_eigen 26 | tf2_geometry_msgs 27 | eigen_conversions 28 | pcl_ros 29 | roscpp 30 | rospy 31 | sensor_msgs 32 | geometry_msgs 33 | message_generation 34 | ndt_omp 35 | fast_gicp 36 | hdl_global_localization 37 | ) 38 | 39 | find_package(PCL 1.7 REQUIRED) 40 | include_directories(${PCL_INCLUDE_DIRS}) 41 | link_directories(${PCL_LIBRARY_DIRS}) 42 | add_definitions(${PCL_DEFINITIONS}) 43 | 44 | message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) 45 | message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) 46 | message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) 47 | 48 | find_package(OpenMP) 49 | if (OPENMP_FOUND) 50 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 51 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 52 | endif() 53 | 54 | ######################## 55 | ## message generation ## 56 | ######################## 57 | add_message_files(FILES 58 | ScanMatchingStatus.msg 59 | ) 60 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 61 | 62 | ################################### 63 | ## catkin specific configuration ## 64 | ################################### 65 | catkin_package( 66 | INCLUDE_DIRS include 67 | # LIBRARIES hdl_scan_matching_odometry 68 | # CATKIN_DEPENDS pcl_ros roscpp sensor_msgs 69 | # DEPENDS system_lib 70 | ) 71 | 72 | ########### 73 | ## Build ## 74 | ########### 75 | include_directories(include) 76 | include_directories( 77 | ${catkin_INCLUDE_DIRS} 78 | ) 79 | 80 | # nodelets 81 | add_library(hdl_localization_nodelet 82 | src/hdl_localization/pose_estimator.cpp 83 | apps/hdl_localization_nodelet.cpp 84 | ) 85 | target_link_libraries(hdl_localization_nodelet 86 | ${catkin_LIBRARIES} 87 | ${PCL_LIBRARIES} 88 | ) 89 | add_dependencies(hdl_localization_nodelet ${PROJECT_NAME}_gencpp) 90 | 91 | 92 | add_library(globalmap_server_nodelet apps/globalmap_server_nodelet.cpp) 93 | target_link_libraries(globalmap_server_nodelet 94 | ${catkin_LIBRARIES} 95 | ${PCL_LIBRARIES} 96 | ) 97 | 98 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, k.koide 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # New packages arrived 2 | 3 | We released a new open source 3D mapping framework [GLIM](https://github.com/koide3/glim). 4 | We also developed a map-based localization system [GLIL](https://koide3.github.io/glil_pubdoc/) as closed source. 5 | 6 | # hdl_localization 7 | ***hdl_localization*** is a ROS package for real-time 3D localization using a 3D LIDAR, such as velodyne HDL32e and VLP16. This package performs Unscented Kalman Filter-based pose estimation. It first estimates the sensor pose from IMU data implemented on the LIDAR, and then performs multi-threaded NDT scan matching between a globalmap point cloud and input point clouds to correct the estimated pose. IMU-based pose prediction is optional. If you disable it, the system uses the constant velocity model without IMU information. 8 | 9 | Video:
10 | [![hdl_localization](http://img.youtube.com/vi/1EyF9kxJOqA/0.jpg)](https://youtu.be/1EyF9kxJOqA) 11 | 12 | [![Build Status](https://travis-ci.org/koide3/hdl_global_localization.svg?branch=master)](https://travis-ci.org/koide3/hdl_global_localization) 13 | 14 | ## Requirements 15 | ***hdl_localization*** requires the following libraries: 16 | - PCL 17 | - OpenMP 18 | 19 | The following ros packages are required: 20 | - pcl_ros 21 | - [ndt_omp](https://github.com/koide3/ndt_omp) 22 | - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) 23 | - [hdl_global_localization](https://github.com/koide3/hdl_global_localization) 24 | 25 | ## Installation 26 | 27 | ```bash 28 | cd /your/catkin_ws/src 29 | git clone https://github.com/koide3/ndt_omp 30 | git clone https://github.com/SMRT-AIST/fast_gicp --recursive 31 | git clone https://github.com/koide3/hdl_localization 32 | git clone https://github.com/koide3/hdl_global_localization 33 | 34 | cd /your/catkin_ws 35 | catkin_make -DCMAKE_BUILD_TYPE=Release 36 | 37 | # if you want to enable CUDA-accelerated NDT 38 | # catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON 39 | ``` 40 | 41 | ### Support docker :whale: 42 | 43 | Using docker, you can conveniently satisfy the requirement environment. 44 | Please refer to the repository below and use the docker easily. 45 | 46 | - [Taeyoung96/hdl_localization_tutorial](https://github.com/Taeyoung96/hdl_localization_tutorial) 47 | 48 | ## Parameters 49 | All configurable parameters are listed in *launch/hdl_localization.launch* as ros params. 50 | The estimated pose can be reset using using "2D Pose Estimate" on rviz 51 | 52 | ## Topics 53 | - ***/odom*** (nav_msgs/Odometry) 54 | - Estimated sensor pose in the map frame 55 | - ***/aligned_points*** 56 | - Input point cloud aligned with the map 57 | - ***/status*** (hdl_localization/ScanMatchingStatus) 58 | - Scan matching result information (e.g., convergence, matching error, and inlier fraction) 59 | 60 | ## Services 61 | - ***/relocalize*** (std_srvs/Empty) 62 | - Reset the sensor pose with the global localization result 63 | - For details of the global localization method, see [hdl_global_localization](https://github.com/koide3/hdl_global_localization) 64 | 65 | ## Example 66 | 67 | Example bag file (recorded in an outdoor environment): [hdl_400.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz) (933MB) 68 | 69 | ```bash 70 | rosparam set use_sim_time true 71 | roslaunch hdl_localization hdl_localization.launch 72 | ``` 73 | 74 | ```bash 75 | roscd hdl_localization/rviz 76 | rviz -d hdl_localization.rviz 77 | ``` 78 | 79 | ```bash 80 | rosbag play --clock hdl_400.bag 81 | ``` 82 | 83 | ```bash 84 | # perform global localization 85 | rosservice call /relocalize 86 | ``` 87 | 88 | 89 | 90 | If it doesn't work well or the CPU usage is too high, change *ndt_neighbor_search_method* in *hdl_localization.launch* to "DIRECT1". It makes the scan matching significantly fast, but a bit unstable. 91 | 92 | ## Related packages 93 | 94 | - [interactive_slam](https://github.com/koide3/interactive_slam) 95 | - hdl_graph_slam 96 | - hdl_localization 97 | - hdl_global_localization 98 | - hdl_people_tracking 99 | 100 | 101 | 102 | Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019 [[link]](https://www.researchgate.net/publication/331283709_A_Portable_3D_LIDAR-based_System_for_Long-term_and_Wide-area_People_Behavior_Measurement). 103 | 104 | ## Contact 105 | Kenji Koide, k.koide@aist.go.jp 106 | 107 | Active Intelligent Systems Laboratory, Toyohashi University of Technology, Japan [\[URL\]](http://www.aisl.cs.tut.ac.jp) 108 | Human-Centered Mobility Research Center, National Institute of Advanced Industrial Science and Technology, Japan [\[URL\]](https://unit.aist.go.jp/rirc/en/team/smart_mobility.html) 109 | 110 | 111 | -------------------------------------------------------------------------------- /apps/globalmap_server_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace hdl_localization { 18 | 19 | class GlobalmapServerNodelet : public nodelet::Nodelet { 20 | public: 21 | using PointT = pcl::PointXYZI; 22 | 23 | GlobalmapServerNodelet() { 24 | } 25 | virtual ~GlobalmapServerNodelet() { 26 | } 27 | 28 | void onInit() override { 29 | nh = getNodeHandle(); 30 | mt_nh = getMTNodeHandle(); 31 | private_nh = getPrivateNodeHandle(); 32 | 33 | initialize_params(); 34 | 35 | // publish globalmap with "latched" publisher 36 | globalmap_pub = nh.advertise("/globalmap", 5, true); 37 | map_update_sub = nh.subscribe("/map_request/pcd", 10, &GlobalmapServerNodelet::map_update_callback, this); 38 | 39 | globalmap_pub_timer = nh.createWallTimer(ros::WallDuration(1.0), &GlobalmapServerNodelet::pub_once_cb, this, true, true); 40 | } 41 | 42 | private: 43 | void initialize_params() { 44 | // read globalmap from a pcd file 45 | std::string globalmap_pcd = private_nh.param("globalmap_pcd", ""); 46 | globalmap.reset(new pcl::PointCloud()); 47 | pcl::io::loadPCDFile(globalmap_pcd, *globalmap); 48 | globalmap->header.frame_id = "map"; 49 | 50 | std::ifstream utm_file(globalmap_pcd + ".utm"); 51 | if (utm_file.is_open() && private_nh.param("convert_utm_to_local", true)) { 52 | double utm_easting; 53 | double utm_northing; 54 | double altitude; 55 | utm_file >> utm_easting >> utm_northing >> altitude; 56 | for(auto& pt : globalmap->points) { 57 | pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude); 58 | } 59 | ROS_INFO_STREAM("Global map offset by UTM reference coordinates (x = " 60 | << utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")"); 61 | } 62 | 63 | // downsample globalmap 64 | double downsample_resolution = private_nh.param("downsample_resolution", 0.1); 65 | boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); 66 | voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 67 | voxelgrid->setInputCloud(globalmap); 68 | 69 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 70 | voxelgrid->filter(*filtered); 71 | 72 | globalmap = filtered; 73 | } 74 | 75 | void pub_once_cb(const ros::WallTimerEvent& event) { 76 | globalmap_pub.publish(globalmap); 77 | } 78 | 79 | void map_update_callback(const std_msgs::String &msg){ 80 | ROS_INFO_STREAM("Received map request, map path : "<< msg.data); 81 | std::string globalmap_pcd = msg.data; 82 | globalmap.reset(new pcl::PointCloud()); 83 | pcl::io::loadPCDFile(globalmap_pcd, *globalmap); 84 | globalmap->header.frame_id = "map"; 85 | 86 | // downsample globalmap 87 | double downsample_resolution = private_nh.param("downsample_resolution", 0.1); 88 | boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); 89 | voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 90 | voxelgrid->setInputCloud(globalmap); 91 | 92 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 93 | voxelgrid->filter(*filtered); 94 | 95 | globalmap = filtered; 96 | globalmap_pub.publish(globalmap); 97 | } 98 | 99 | private: 100 | // ROS 101 | ros::NodeHandle nh; 102 | ros::NodeHandle mt_nh; 103 | ros::NodeHandle private_nh; 104 | 105 | ros::Publisher globalmap_pub; 106 | ros::Subscriber map_update_sub; 107 | 108 | ros::WallTimer globalmap_pub_timer; 109 | pcl::PointCloud::Ptr globalmap; 110 | }; 111 | 112 | } 113 | 114 | 115 | PLUGINLIB_EXPORT_CLASS(hdl_localization::GlobalmapServerNodelet, nodelet::Nodelet) 116 | -------------------------------------------------------------------------------- /apps/hdl_localization_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace hdl_localization { 36 | 37 | class HdlLocalizationNodelet : public nodelet::Nodelet { 38 | public: 39 | using PointT = pcl::PointXYZI; 40 | 41 | HdlLocalizationNodelet() : tf_buffer(), tf_listener(tf_buffer) { 42 | } 43 | virtual ~HdlLocalizationNodelet() { 44 | } 45 | 46 | void onInit() override { 47 | nh = getNodeHandle(); 48 | mt_nh = getMTNodeHandle(); 49 | private_nh = getPrivateNodeHandle(); 50 | 51 | initialize_params(); 52 | 53 | robot_odom_frame_id = private_nh.param("robot_odom_frame_id", "robot_odom"); 54 | odom_child_frame_id = private_nh.param("odom_child_frame_id", "base_link"); 55 | 56 | use_imu = private_nh.param("use_imu", true); 57 | invert_acc = private_nh.param("invert_acc", false); 58 | invert_gyro = private_nh.param("invert_gyro", false); 59 | if (use_imu) { 60 | NODELET_INFO("enable imu-based prediction"); 61 | imu_sub = mt_nh.subscribe("/gpsimu_driver/imu_data", 256, &HdlLocalizationNodelet::imu_callback, this); 62 | } 63 | points_sub = mt_nh.subscribe("/velodyne_points", 5, &HdlLocalizationNodelet::points_callback, this); 64 | globalmap_sub = nh.subscribe("/globalmap", 1, &HdlLocalizationNodelet::globalmap_callback, this); 65 | initialpose_sub = nh.subscribe("/initialpose", 8, &HdlLocalizationNodelet::initialpose_callback, this); 66 | 67 | pose_pub = nh.advertise("/odom", 5, false); 68 | aligned_pub = nh.advertise("/aligned_points", 5, false); 69 | status_pub = nh.advertise("/status", 5, false); 70 | 71 | // global localization 72 | use_global_localization = private_nh.param("use_global_localization", true); 73 | if(use_global_localization) { 74 | NODELET_INFO_STREAM("wait for global localization services"); 75 | ros::service::waitForService("/hdl_global_localization/set_global_map"); 76 | ros::service::waitForService("/hdl_global_localization/query"); 77 | 78 | set_global_map_service = nh.serviceClient("/hdl_global_localization/set_global_map"); 79 | query_global_localization_service = nh.serviceClient("/hdl_global_localization/query"); 80 | 81 | relocalize_server = nh.advertiseService("/relocalize", &HdlLocalizationNodelet::relocalize, this); 82 | } 83 | } 84 | 85 | private: 86 | pcl::Registration::Ptr create_registration() const { 87 | std::string reg_method = private_nh.param("reg_method", "NDT_OMP"); 88 | std::string ndt_neighbor_search_method = private_nh.param("ndt_neighbor_search_method", "DIRECT7"); 89 | double ndt_neighbor_search_radius = private_nh.param("ndt_neighbor_search_radius", 2.0); 90 | double ndt_resolution = private_nh.param("ndt_resolution", 1.0); 91 | 92 | if(reg_method == "NDT_OMP") { 93 | NODELET_INFO("NDT_OMP is selected"); 94 | pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); 95 | ndt->setTransformationEpsilon(0.01); 96 | ndt->setResolution(ndt_resolution); 97 | if (ndt_neighbor_search_method == "DIRECT1") { 98 | NODELET_INFO("search_method DIRECT1 is selected"); 99 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); 100 | } else if (ndt_neighbor_search_method == "DIRECT7") { 101 | NODELET_INFO("search_method DIRECT7 is selected"); 102 | ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); 103 | } else { 104 | if (ndt_neighbor_search_method == "KDTREE") { 105 | NODELET_INFO("search_method KDTREE is selected"); 106 | } else { 107 | NODELET_WARN("invalid search method was given"); 108 | NODELET_WARN("default method is selected (KDTREE)"); 109 | } 110 | ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); 111 | } 112 | return ndt; 113 | } else if(reg_method.find("NDT_CUDA") != std::string::npos) { 114 | NODELET_INFO("NDT_CUDA is selected"); 115 | boost::shared_ptr> ndt(new fast_gicp::NDTCuda); 116 | ndt->setResolution(ndt_resolution); 117 | 118 | if(reg_method.find("D2D") != std::string::npos) { 119 | ndt->setDistanceMode(fast_gicp::NDTDistanceMode::D2D); 120 | } else if (reg_method.find("P2D") != std::string::npos) { 121 | ndt->setDistanceMode(fast_gicp::NDTDistanceMode::P2D); 122 | } 123 | 124 | if (ndt_neighbor_search_method == "DIRECT1") { 125 | NODELET_INFO("search_method DIRECT1 is selected"); 126 | ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT1); 127 | } else if (ndt_neighbor_search_method == "DIRECT7") { 128 | NODELET_INFO("search_method DIRECT7 is selected"); 129 | ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT7); 130 | } else if (ndt_neighbor_search_method == "DIRECT_RADIUS") { 131 | NODELET_INFO_STREAM("search_method DIRECT_RADIUS is selected : " << ndt_neighbor_search_radius); 132 | ndt->setNeighborSearchMethod(fast_gicp::NeighborSearchMethod::DIRECT_RADIUS, ndt_neighbor_search_radius); 133 | } else { 134 | NODELET_WARN("invalid search method was given"); 135 | } 136 | return ndt; 137 | } 138 | 139 | NODELET_ERROR_STREAM("unknown registration method:" << reg_method); 140 | return nullptr; 141 | } 142 | 143 | void initialize_params() { 144 | // intialize scan matching method 145 | double downsample_resolution = private_nh.param("downsample_resolution", 0.1); 146 | boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); 147 | voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 148 | downsample_filter = voxelgrid; 149 | 150 | NODELET_INFO("create registration method for localization"); 151 | registration = create_registration(); 152 | 153 | // global localization 154 | NODELET_INFO("create registration method for fallback during relocalization"); 155 | relocalizing = false; 156 | delta_estimater.reset(new DeltaEstimater(create_registration())); 157 | 158 | // initialize pose estimator 159 | if(private_nh.param("specify_init_pose", true)) { 160 | NODELET_INFO("initialize pose estimator with specified parameters!!"); 161 | pose_estimator.reset(new hdl_localization::PoseEstimator(registration, 162 | Eigen::Vector3f(private_nh.param("init_pos_x", 0.0), private_nh.param("init_pos_y", 0.0), private_nh.param("init_pos_z", 0.0)), 163 | Eigen::Quaternionf(private_nh.param("init_ori_w", 1.0), private_nh.param("init_ori_x", 0.0), private_nh.param("init_ori_y", 0.0), private_nh.param("init_ori_z", 0.0)), 164 | private_nh.param("cool_time_duration", 0.5) 165 | )); 166 | } 167 | } 168 | 169 | private: 170 | /** 171 | * @brief callback for imu data 172 | * @param imu_msg 173 | */ 174 | void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) { 175 | std::lock_guard lock(imu_data_mutex); 176 | imu_data.push_back(imu_msg); 177 | } 178 | 179 | /** 180 | * @brief callback for point cloud data 181 | * @param points_msg 182 | */ 183 | void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) { 184 | if(!globalmap) { 185 | NODELET_ERROR("globalmap has not been received!!"); 186 | return; 187 | } 188 | 189 | const auto& stamp = points_msg->header.stamp; 190 | pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud()); 191 | pcl::fromROSMsg(*points_msg, *pcl_cloud); 192 | 193 | if(pcl_cloud->empty()) { 194 | NODELET_ERROR("cloud is empty!!"); 195 | return; 196 | } 197 | 198 | // transform pointcloud into odom_child_frame_id 199 | std::string tfError; 200 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 201 | if(this->tf_buffer.canTransform(odom_child_frame_id, pcl_cloud->header.frame_id, stamp, ros::Duration(0.1), &tfError)) 202 | { 203 | if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_buffer)) { 204 | NODELET_ERROR("point cloud cannot be transformed into target frame!!"); 205 | return; 206 | } 207 | }else 208 | { 209 | NODELET_ERROR(tfError.c_str()); 210 | return; 211 | } 212 | 213 | auto filtered = downsample(cloud); 214 | last_scan = filtered; 215 | 216 | if(relocalizing) { 217 | delta_estimater->add_frame(filtered); 218 | } 219 | 220 | std::lock_guard estimator_lock(pose_estimator_mutex); 221 | if(!pose_estimator) { 222 | NODELET_ERROR("waiting for initial pose input!!"); 223 | return; 224 | } 225 | Eigen::Matrix4f before = pose_estimator->matrix(); 226 | 227 | // predict 228 | if(!use_imu) { 229 | pose_estimator->predict(stamp); 230 | } else { 231 | std::lock_guard lock(imu_data_mutex); 232 | auto imu_iter = imu_data.begin(); 233 | for(imu_iter; imu_iter != imu_data.end(); imu_iter++) { 234 | if(stamp < (*imu_iter)->header.stamp) { 235 | break; 236 | } 237 | const auto& acc = (*imu_iter)->linear_acceleration; 238 | const auto& gyro = (*imu_iter)->angular_velocity; 239 | double acc_sign = invert_acc ? -1.0 : 1.0; 240 | double gyro_sign = invert_gyro ? -1.0 : 1.0; 241 | pose_estimator->predict((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z)); 242 | } 243 | imu_data.erase(imu_data.begin(), imu_iter); 244 | } 245 | 246 | // odometry-based prediction 247 | ros::Time last_correction_time = pose_estimator->last_correction_time(); 248 | if(private_nh.param("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) { 249 | geometry_msgs::TransformStamped odom_delta; 250 | if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0.1))) { 251 | odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, stamp, robot_odom_frame_id, ros::Duration(0)); 252 | } else if(tf_buffer.canTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0))) { 253 | odom_delta = tf_buffer.lookupTransform(odom_child_frame_id, last_correction_time, odom_child_frame_id, ros::Time(0), robot_odom_frame_id, ros::Duration(0)); 254 | } 255 | 256 | if(odom_delta.header.stamp.isZero()) { 257 | NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id); 258 | } else { 259 | Eigen::Isometry3d delta = tf2::transformToEigen(odom_delta); 260 | pose_estimator->predict_odom(delta.cast().matrix()); 261 | } 262 | } 263 | 264 | // correct 265 | auto aligned = pose_estimator->correct(stamp, filtered); 266 | 267 | if(aligned_pub.getNumSubscribers()) { 268 | aligned->header.frame_id = "map"; 269 | aligned->header.stamp = cloud->header.stamp; 270 | aligned_pub.publish(aligned); 271 | } 272 | 273 | if(status_pub.getNumSubscribers()) { 274 | publish_scan_matching_status(points_msg->header, aligned); 275 | } 276 | 277 | publish_odometry(points_msg->header.stamp, pose_estimator->matrix()); 278 | } 279 | 280 | /** 281 | * @brief callback for globalmap input 282 | * @param points_msg 283 | */ 284 | void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) { 285 | NODELET_INFO("globalmap received!"); 286 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 287 | pcl::fromROSMsg(*points_msg, *cloud); 288 | globalmap = cloud; 289 | 290 | registration->setInputTarget(globalmap); 291 | 292 | if(use_global_localization) { 293 | NODELET_INFO("set globalmap for global localization!"); 294 | hdl_global_localization::SetGlobalMap srv; 295 | pcl::toROSMsg(*globalmap, srv.request.global_map); 296 | 297 | if(!set_global_map_service.call(srv)) { 298 | NODELET_INFO("failed to set global map"); 299 | } else { 300 | NODELET_INFO("done"); 301 | } 302 | } 303 | } 304 | 305 | /** 306 | * @brief perform global localization to relocalize the sensor position 307 | * @param 308 | */ 309 | bool relocalize(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res) { 310 | if(last_scan == nullptr) { 311 | NODELET_INFO_STREAM("no scan has been received"); 312 | return false; 313 | } 314 | 315 | relocalizing = true; 316 | delta_estimater->reset(); 317 | pcl::PointCloud::ConstPtr scan = last_scan; 318 | 319 | hdl_global_localization::QueryGlobalLocalization srv; 320 | pcl::toROSMsg(*scan, srv.request.cloud); 321 | srv.request.max_num_candidates = 1; 322 | 323 | if(!query_global_localization_service.call(srv) || srv.response.poses.empty()) { 324 | relocalizing = false; 325 | NODELET_INFO_STREAM("global localization failed"); 326 | return false; 327 | } 328 | 329 | const auto& result = srv.response.poses[0]; 330 | 331 | NODELET_INFO_STREAM("--- Global localization result ---"); 332 | NODELET_INFO_STREAM("Trans :" << result.position.x << " " << result.position.y << " " << result.position.z); 333 | NODELET_INFO_STREAM("Quat :" << result.orientation.x << " " << result.orientation.y << " " << result.orientation.z << " " << result.orientation.w); 334 | NODELET_INFO_STREAM("Error :" << srv.response.errors[0]); 335 | NODELET_INFO_STREAM("Inlier:" << srv.response.inlier_fractions[0]); 336 | 337 | Eigen::Isometry3f pose = Eigen::Isometry3f::Identity(); 338 | pose.linear() = Eigen::Quaternionf(result.orientation.w, result.orientation.x, result.orientation.y, result.orientation.z).toRotationMatrix(); 339 | pose.translation() = Eigen::Vector3f(result.position.x, result.position.y, result.position.z); 340 | pose = pose * delta_estimater->estimated_delta(); 341 | 342 | std::lock_guard lock(pose_estimator_mutex); 343 | pose_estimator.reset(new hdl_localization::PoseEstimator( 344 | registration, 345 | pose.translation(), 346 | Eigen::Quaternionf(pose.linear()), 347 | private_nh.param("cool_time_duration", 0.5))); 348 | 349 | relocalizing = false; 350 | 351 | return true; 352 | } 353 | 354 | /** 355 | * @brief callback for initial pose input ("2D Pose Estimate" on rviz) 356 | * @param pose_msg 357 | */ 358 | void initialpose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg) { 359 | NODELET_INFO("initial pose received!!"); 360 | std::lock_guard lock(pose_estimator_mutex); 361 | const auto& p = pose_msg->pose.pose.position; 362 | const auto& q = pose_msg->pose.pose.orientation; 363 | pose_estimator.reset( 364 | new hdl_localization::PoseEstimator( 365 | registration, 366 | Eigen::Vector3f(p.x, p.y, p.z), 367 | Eigen::Quaternionf(q.w, q.x, q.y, q.z), 368 | private_nh.param("cool_time_duration", 0.5)) 369 | ); 370 | } 371 | 372 | /** 373 | * @brief downsampling 374 | * @param cloud input cloud 375 | * @return downsampled cloud 376 | */ 377 | pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { 378 | if(!downsample_filter) { 379 | return cloud; 380 | } 381 | 382 | pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); 383 | downsample_filter->setInputCloud(cloud); 384 | downsample_filter->filter(*filtered); 385 | filtered->header = cloud->header; 386 | 387 | return filtered; 388 | } 389 | 390 | /** 391 | * @brief publish odometry 392 | * @param stamp timestamp 393 | * @param pose odometry pose to be published 394 | */ 395 | void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) { 396 | // broadcast the transform over tf 397 | if(tf_buffer.canTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0))) { 398 | geometry_msgs::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(pose.inverse().cast())); 399 | map_wrt_frame.header.stamp = stamp; 400 | map_wrt_frame.header.frame_id = odom_child_frame_id; 401 | map_wrt_frame.child_frame_id = "map"; 402 | 403 | geometry_msgs::TransformStamped frame_wrt_odom = tf_buffer.lookupTransform(robot_odom_frame_id, odom_child_frame_id, ros::Time(0), ros::Duration(0.1)); 404 | Eigen::Matrix4f frame2odom = tf2::transformToEigen(frame_wrt_odom).cast().matrix(); 405 | 406 | geometry_msgs::TransformStamped map_wrt_odom; 407 | tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom); 408 | 409 | tf2::Transform odom_wrt_map; 410 | tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map); 411 | odom_wrt_map = odom_wrt_map.inverse(); 412 | 413 | geometry_msgs::TransformStamped odom_trans; 414 | odom_trans.transform = tf2::toMsg(odom_wrt_map); 415 | odom_trans.header.stamp = stamp; 416 | odom_trans.header.frame_id = "map"; 417 | odom_trans.child_frame_id = robot_odom_frame_id; 418 | 419 | tf_broadcaster.sendTransform(odom_trans); 420 | } else { 421 | geometry_msgs::TransformStamped odom_trans = tf2::eigenToTransform(Eigen::Isometry3d(pose.cast())); 422 | odom_trans.header.stamp = stamp; 423 | odom_trans.header.frame_id = "map"; 424 | odom_trans.child_frame_id = odom_child_frame_id; 425 | tf_broadcaster.sendTransform(odom_trans); 426 | } 427 | 428 | // publish the transform 429 | nav_msgs::Odometry odom; 430 | odom.header.stamp = stamp; 431 | odom.header.frame_id = "map"; 432 | 433 | tf::poseEigenToMsg(Eigen::Isometry3d(pose.cast()), odom.pose.pose); 434 | odom.child_frame_id = odom_child_frame_id; 435 | odom.twist.twist.linear.x = 0.0; 436 | odom.twist.twist.linear.y = 0.0; 437 | odom.twist.twist.angular.z = 0.0; 438 | 439 | pose_pub.publish(odom); 440 | } 441 | 442 | /** 443 | * @brief publish scan matching status information 444 | */ 445 | void publish_scan_matching_status(const std_msgs::Header& header, pcl::PointCloud::ConstPtr aligned) { 446 | ScanMatchingStatus status; 447 | status.header = header; 448 | 449 | status.has_converged = registration->hasConverged(); 450 | status.matching_error = 0.0; 451 | 452 | const double max_correspondence_dist = private_nh.param("status_max_correspondence_dist", 0.5); 453 | const double max_valid_point_dist = private_nh.param("status_max_valid_point_dist", 25.0); 454 | 455 | int num_inliers = 0; 456 | int num_valid_points = 0; 457 | std::vector k_indices; 458 | std::vector k_sq_dists; 459 | for(int i = 0; i < aligned->size(); i++) { 460 | const auto& pt = aligned->at(i); 461 | if (pt.getVector3fMap().norm() > max_valid_point_dist) { 462 | continue; 463 | } 464 | num_valid_points++; 465 | 466 | registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); 467 | if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { 468 | status.matching_error += k_sq_dists[0]; 469 | num_inliers++; 470 | } 471 | } 472 | 473 | status.matching_error /= num_inliers; 474 | status.inlier_fraction = static_cast(num_inliers) / std::max(1, num_valid_points); 475 | status.relative_pose = tf2::eigenToTransform(Eigen::Isometry3d(registration->getFinalTransformation().cast())).transform; 476 | 477 | status.prediction_labels.reserve(2); 478 | status.prediction_errors.reserve(2); 479 | 480 | std::vector errors(6, 0.0); 481 | 482 | if(pose_estimator->wo_prediction_error()) { 483 | status.prediction_labels.push_back(std_msgs::String()); 484 | status.prediction_labels.back().data = "without_pred"; 485 | status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->wo_prediction_error().get().cast())).transform); 486 | } 487 | 488 | if(pose_estimator->imu_prediction_error()) { 489 | status.prediction_labels.push_back(std_msgs::String()); 490 | status.prediction_labels.back().data = use_imu ? "imu" : "motion_model"; 491 | status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->imu_prediction_error().get().cast())).transform); 492 | } 493 | 494 | if(pose_estimator->odom_prediction_error()) { 495 | status.prediction_labels.push_back(std_msgs::String()); 496 | status.prediction_labels.back().data = "odom"; 497 | status.prediction_errors.push_back(tf2::eigenToTransform(Eigen::Isometry3d(pose_estimator->odom_prediction_error().get().cast())).transform); 498 | } 499 | 500 | status_pub.publish(status); 501 | } 502 | 503 | private: 504 | // ROS 505 | ros::NodeHandle nh; 506 | ros::NodeHandle mt_nh; 507 | ros::NodeHandle private_nh; 508 | 509 | std::string robot_odom_frame_id; 510 | std::string odom_child_frame_id; 511 | 512 | bool use_imu; 513 | bool invert_acc; 514 | bool invert_gyro; 515 | ros::Subscriber imu_sub; 516 | ros::Subscriber points_sub; 517 | ros::Subscriber globalmap_sub; 518 | ros::Subscriber initialpose_sub; 519 | 520 | ros::Publisher pose_pub; 521 | ros::Publisher aligned_pub; 522 | ros::Publisher status_pub; 523 | 524 | tf2_ros::Buffer tf_buffer; 525 | tf2_ros::TransformListener tf_listener; 526 | tf2_ros::TransformBroadcaster tf_broadcaster; 527 | 528 | // imu input buffer 529 | std::mutex imu_data_mutex; 530 | std::vector imu_data; 531 | 532 | // globalmap and registration method 533 | pcl::PointCloud::Ptr globalmap; 534 | pcl::Filter::Ptr downsample_filter; 535 | pcl::Registration::Ptr registration; 536 | 537 | // pose estimator 538 | std::mutex pose_estimator_mutex; 539 | std::unique_ptr pose_estimator; 540 | 541 | // global localization 542 | bool use_global_localization; 543 | std::atomic_bool relocalizing; 544 | std::unique_ptr delta_estimater; 545 | 546 | pcl::PointCloud::ConstPtr last_scan; 547 | ros::ServiceServer relocalize_server; 548 | ros::ServiceClient set_global_map_service; 549 | ros::ServiceClient query_global_localization_service; 550 | }; 551 | } 552 | 553 | 554 | PLUGINLIB_EXPORT_CLASS(hdl_localization::HdlLocalizationNodelet, nodelet::Nodelet) 555 | -------------------------------------------------------------------------------- /data/figs/localization1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_localization/de2dc7769aa2412876d05ae6161408d3295b6a3c/data/figs/localization1.png -------------------------------------------------------------------------------- /data/figs/localization2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_localization/de2dc7769aa2412876d05ae6161408d3295b6a3c/data/figs/localization2.png -------------------------------------------------------------------------------- /data/figs/packages.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_localization/de2dc7769aa2412876d05ae6161408d3295b6a3c/data/figs/packages.png -------------------------------------------------------------------------------- /data/map.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/koide3/hdl_localization/de2dc7769aa2412876d05ae6161408d3295b6a3c/data/map.pcd -------------------------------------------------------------------------------- /docker/melodic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | libomp-dev libgtest-dev libboost-all-dev libopencv-dev \ 6 | ros-melodic-tf2 ros-melodic-tf2-ros ros-melodic-tf2-geometry-msgs \ 7 | ros-melodic-eigen-conversions ros-melodic-tf-conversions ros-melodic-pcl-ros \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 14 | 15 | RUN git clone https://github.com/koide3/ndt_omp -b melodic 16 | RUN git clone https://github.com/SMRT-AIST/fast_gicp --recursive 17 | RUN git clone https://github.com/koide3/hdl_global_localization 18 | 19 | COPY . /root/catkin_ws/src/hdl_localization/ 20 | WORKDIR /root/catkin_ws/src/hdl_localization 21 | 22 | WORKDIR /root/catkin_ws 23 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make' 24 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 25 | 26 | WORKDIR / 27 | 28 | ENTRYPOINT ["/ros_entrypoint.sh"] 29 | CMD ["bash"] 30 | -------------------------------------------------------------------------------- /docker/melodic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | libomp-dev libgtest-dev libboost-all-dev libopencv-dev clang lld \ 6 | ros-melodic-tf2 ros-melodic-tf2-ros ros-melodic-tf2-geometry-msgs \ 7 | ros-melodic-eigen-conversions ros-melodic-tf-conversions ros-melodic-pcl-ros \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 12 | 13 | RUN mkdir -p /root/catkin_ws/src 14 | WORKDIR /root/catkin_ws/src 15 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 16 | 17 | RUN git clone https://github.com/koide3/ndt_omp -b melodic 18 | RUN git clone https://github.com/SMRT-AIST/fast_gicp --recursive 19 | RUN git clone https://github.com/koide3/hdl_global_localization 20 | 21 | COPY . /root/catkin_ws/src/hdl_localization/ 22 | WORKDIR /root/catkin_ws/src/hdl_localization 23 | 24 | WORKDIR /root/catkin_ws 25 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make' 26 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 27 | 28 | WORKDIR / 29 | 30 | ENTRYPOINT ["/ros_entrypoint.sh"] 31 | CMD ["bash"] 32 | -------------------------------------------------------------------------------- /docker/noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | git libgtest-dev libboost-all-dev libopencv-dev \ 6 | ros-noetic-tf2 ros-noetic-tf2-ros ros-noetic-tf2-geometry-msgs \ 7 | ros-noetic-eigen-conversions ros-noetic-tf-conversions ros-noetic-pcl-ros \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 14 | 15 | RUN git clone https://github.com/koide3/ndt_omp 16 | RUN git clone https://github.com/SMRT-AIST/fast_gicp --recursive 17 | RUN git clone https://github.com/koide3/hdl_global_localization 18 | 19 | COPY . /root/catkin_ws/src/hdl_localization/ 20 | WORKDIR /root/catkin_ws/src/hdl_localization 21 | 22 | WORKDIR /root/catkin_ws 23 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo' 24 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 25 | 26 | WORKDIR / 27 | 28 | ENTRYPOINT ["/ros_entrypoint.sh"] 29 | CMD ["bash"] 30 | -------------------------------------------------------------------------------- /docker/noetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | git clang lld libomp-dev libgtest-dev libboost-all-dev libopencv-dev \ 6 | ros-noetic-tf2 ros-noetic-tf2-ros ros-noetic-tf2-geometry-msgs \ 7 | ros-noetic-eigen-conversions ros-noetic-tf-conversions ros-noetic-pcl-ros \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 12 | 13 | RUN mkdir -p /root/catkin_ws/src 14 | WORKDIR /root/catkin_ws/src 15 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 16 | 17 | RUN git clone https://github.com/koide3/ndt_omp 18 | RUN git clone https://github.com/SMRT-AIST/fast_gicp --recursive 19 | RUN git clone https://github.com/koide3/hdl_global_localization 20 | 21 | COPY . /root/catkin_ws/src/hdl_localization/ 22 | WORKDIR /root/catkin_ws/src/hdl_localization 23 | 24 | WORKDIR /root/catkin_ws 25 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make' 26 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 27 | 28 | WORKDIR / 29 | 30 | ENTRYPOINT ["/ros_entrypoint.sh"] 31 | CMD ["bash"] 32 | -------------------------------------------------------------------------------- /include/hdl_localization/delta_estimater.hpp: -------------------------------------------------------------------------------- 1 | #ifndef HDL_LOCALIZATION_DELTA_ESTIMATER_HPP 2 | #define HDL_LOCALIZATION_DELTA_ESTIMATER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace hdl_localization { 10 | 11 | class DeltaEstimater { 12 | public: 13 | using PointT = pcl::PointXYZI; 14 | 15 | DeltaEstimater(pcl::Registration::Ptr reg): delta(Eigen::Isometry3f::Identity()), reg(reg) {} 16 | ~DeltaEstimater() {} 17 | 18 | void reset() { 19 | std::lock_guard lock(mutex); 20 | delta.setIdentity(); 21 | last_frame.reset(); 22 | } 23 | 24 | void add_frame(pcl::PointCloud::ConstPtr frame) { 25 | std::unique_lock lock(mutex); 26 | if (last_frame == nullptr) { 27 | last_frame = frame; 28 | return; 29 | } 30 | 31 | reg->setInputTarget(last_frame); 32 | reg->setInputSource(frame); 33 | lock.unlock(); 34 | 35 | pcl::PointCloud aligned; 36 | reg->align(aligned); 37 | 38 | lock.lock(); 39 | last_frame = frame; 40 | delta = delta * Eigen::Isometry3f(reg->getFinalTransformation()); 41 | } 42 | 43 | Eigen::Isometry3f estimated_delta() const { 44 | std::lock_guard lock(mutex); 45 | return delta; 46 | } 47 | 48 | private: 49 | mutable std::mutex mutex; 50 | Eigen::Isometry3f delta; 51 | pcl::Registration::Ptr reg; 52 | 53 | pcl::PointCloud::ConstPtr last_frame; 54 | }; 55 | 56 | } // namespace hdl_localization 57 | 58 | 59 | #endif -------------------------------------------------------------------------------- /include/hdl_localization/odom_system.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ODOM_SYSTEM_HPP 2 | #define ODOM_SYSTEM_HPP 3 | 4 | #include 5 | 6 | namespace hdl_localization { 7 | 8 | /** 9 | * @brief This class models the sensor pose estimation based on robot odometry 10 | * @note state = [px, py, pz, qw, qx, qy, qz] 11 | * observation = [px, py, pz, qw, qx, qy, qz] 12 | * maybe better to use expmap 13 | */ 14 | class OdomSystem { 15 | public: 16 | typedef float T; 17 | typedef Eigen::Matrix Vector3t; 18 | typedef Eigen::Matrix Vector4t; 19 | typedef Eigen::Matrix Matrix4t; 20 | typedef Eigen::Matrix VectorXt; 21 | typedef Eigen::Quaternion Quaterniont; 22 | 23 | public: 24 | // system equation 25 | VectorXt f(const VectorXt& state, const VectorXt& control) const { 26 | Matrix4t pt = Matrix4t::Identity(); 27 | pt.block<3, 1>(0, 3) = Vector3t(state[0], state[1], state[2]); 28 | pt.block<3, 3>(0, 0) = Quaterniont(state[3], state[4], state[5], state[6]).normalized().toRotationMatrix(); 29 | 30 | Matrix4t delta = Matrix4t::Identity(); 31 | delta.block<3, 1>(0, 3) = Vector3t(control[0], control[1], control[2]); 32 | delta.block<3, 3>(0, 0) = Quaterniont(control[3], control[4], control[5], control[6]).normalized().toRotationMatrix(); 33 | 34 | Matrix4t pt_ = pt * delta; 35 | Quaterniont quat_(pt_.block<3, 3>(0, 0)); 36 | 37 | VectorXt next_state(7); 38 | next_state.head<3>() = pt_.block<3, 1>(0, 3); 39 | next_state.tail<4>() = Vector4t(quat_.w(), quat_.x(), quat_.y(), quat_.z()); 40 | 41 | return next_state; 42 | } 43 | 44 | // observation equation 45 | VectorXt h(const VectorXt& state) const { 46 | return state; 47 | } 48 | }; 49 | 50 | } // namespace hdl_localization 51 | 52 | #endif // POSE_SYSTEM_HPP 53 | -------------------------------------------------------------------------------- /include/hdl_localization/pose_estimator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSE_ESTIMATOR_HPP 2 | #define POSE_ESTIMATOR_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kkl { 13 | namespace alg { 14 | template class UnscentedKalmanFilterX; 15 | } 16 | } 17 | 18 | namespace hdl_localization { 19 | 20 | class PoseSystem; 21 | class OdomSystem; 22 | 23 | /** 24 | * @brief scan matching-based pose estimator 25 | */ 26 | class PoseEstimator { 27 | public: 28 | using PointT = pcl::PointXYZI; 29 | 30 | /** 31 | * @brief constructor 32 | * @param registration registration method 33 | * @param pos initial position 34 | * @param quat initial orientation 35 | * @param cool_time_duration during "cool time", prediction is not performed 36 | */ 37 | PoseEstimator(pcl::Registration::Ptr& registration, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration = 1.0); 38 | ~PoseEstimator(); 39 | 40 | /** 41 | * @brief predict 42 | * @param stamp timestamp 43 | */ 44 | void predict(const ros::Time& stamp); 45 | 46 | /** 47 | * @brief predict 48 | * @param stamp timestamp 49 | * @param acc acceleration 50 | * @param gyro angular velocity 51 | */ 52 | void predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro); 53 | 54 | /** 55 | * @brief update the state of the odomety-based pose estimation 56 | */ 57 | void predict_odom(const Eigen::Matrix4f& odom_delta); 58 | 59 | /** 60 | * @brief correct 61 | * @param cloud input cloud 62 | * @return cloud aligned to the globalmap 63 | */ 64 | pcl::PointCloud::Ptr correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud); 65 | 66 | /* getters */ 67 | ros::Time last_correction_time() const; 68 | 69 | Eigen::Vector3f pos() const; 70 | Eigen::Vector3f vel() const; 71 | Eigen::Quaternionf quat() const; 72 | Eigen::Matrix4f matrix() const; 73 | 74 | Eigen::Vector3f odom_pos() const; 75 | Eigen::Quaternionf odom_quat() const; 76 | Eigen::Matrix4f odom_matrix() const; 77 | 78 | const boost::optional& wo_prediction_error() const; 79 | const boost::optional& imu_prediction_error() const; 80 | const boost::optional& odom_prediction_error() const; 81 | 82 | private: 83 | ros::Time init_stamp; // when the estimator was initialized 84 | ros::Time prev_stamp; // when the estimator was updated last time 85 | ros::Time last_correction_stamp; // when the estimator performed the correction step 86 | double cool_time_duration; // 87 | 88 | Eigen::MatrixXf process_noise; 89 | std::unique_ptr> ukf; 90 | std::unique_ptr> odom_ukf; 91 | 92 | Eigen::Matrix4f last_observation; 93 | boost::optional wo_pred_error; 94 | boost::optional imu_pred_error; 95 | boost::optional odom_pred_error; 96 | 97 | pcl::Registration::Ptr registration; 98 | }; 99 | 100 | } // namespace hdl_localization 101 | 102 | #endif // POSE_ESTIMATOR_HPP 103 | -------------------------------------------------------------------------------- /include/hdl_localization/pose_system.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSE_SYSTEM_HPP 2 | #define POSE_SYSTEM_HPP 3 | 4 | #include 5 | 6 | namespace hdl_localization { 7 | 8 | /** 9 | * @brief Definition of system to be estimated by ukf 10 | * @note state = [px, py, pz, vx, vy, vz, qw, qx, qy, qz, acc_bias_x, acc_bias_y, acc_bias_z, gyro_bias_x, gyro_bias_y, gyro_bias_z] 11 | */ 12 | class PoseSystem { 13 | public: 14 | typedef float T; 15 | typedef Eigen::Matrix Vector3t; 16 | typedef Eigen::Matrix Matrix4t; 17 | typedef Eigen::Matrix VectorXt; 18 | typedef Eigen::Quaternion Quaterniont; 19 | public: 20 | PoseSystem() { 21 | dt = 0.01; 22 | } 23 | 24 | // system equation (without input) 25 | VectorXt f(const VectorXt& state) const { 26 | VectorXt next_state(16); 27 | 28 | Vector3t pt = state.middleRows(0, 3); 29 | Vector3t vt = state.middleRows(3, 3); 30 | Quaterniont qt(state[6], state[7], state[8], state[9]); 31 | qt.normalize(); 32 | 33 | Vector3t acc_bias = state.middleRows(10, 3); 34 | Vector3t gyro_bias = state.middleRows(13, 3); 35 | 36 | // position 37 | next_state.middleRows(0, 3) = pt + vt * dt; // 38 | 39 | // velocity 40 | next_state.middleRows(3, 3) = vt; 41 | 42 | // orientation 43 | Quaterniont qt_ = qt; 44 | 45 | next_state.middleRows(6, 4) << qt_.w(), qt_.x(), qt_.y(), qt_.z(); 46 | next_state.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration 47 | next_state.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity 48 | 49 | return next_state; 50 | } 51 | 52 | // system equation 53 | VectorXt f(const VectorXt& state, const VectorXt& control) const { 54 | VectorXt next_state(16); 55 | 56 | Vector3t pt = state.middleRows(0, 3); 57 | Vector3t vt = state.middleRows(3, 3); 58 | Quaterniont qt(state[6], state[7], state[8], state[9]); 59 | qt.normalize(); 60 | 61 | Vector3t acc_bias = state.middleRows(10, 3); 62 | Vector3t gyro_bias = state.middleRows(13, 3); 63 | 64 | Vector3t raw_acc = control.middleRows(0, 3); 65 | Vector3t raw_gyro = control.middleRows(3, 3); 66 | 67 | // position 68 | next_state.middleRows(0, 3) = pt + vt * dt; // 69 | 70 | // velocity 71 | Vector3t g(0.0f, 0.0f, 9.80665f); 72 | Vector3t acc_ = raw_acc - acc_bias; 73 | Vector3t acc = qt * acc_; 74 | next_state.middleRows(3, 3) = vt + (acc - g) * dt; 75 | // next_state.middleRows(3, 3) = vt; // + (acc - g) * dt; // acceleration didn't contribute to accuracy due to large noise 76 | 77 | // orientation 78 | Vector3t gyro = raw_gyro - gyro_bias; 79 | Quaterniont dq(1, gyro[0] * dt / 2, gyro[1] * dt / 2, gyro[2] * dt / 2); 80 | dq.normalize(); 81 | Quaterniont qt_ = (qt * dq).normalized(); 82 | next_state.middleRows(6, 4) << qt_.w(), qt_.x(), qt_.y(), qt_.z(); 83 | 84 | next_state.middleRows(10, 3) = state.middleRows(10, 3); // constant bias on acceleration 85 | next_state.middleRows(13, 3) = state.middleRows(13, 3); // constant bias on angular velocity 86 | 87 | return next_state; 88 | } 89 | 90 | // observation equation 91 | VectorXt h(const VectorXt& state) const { 92 | VectorXt observation(7); 93 | observation.middleRows(0, 3) = state.middleRows(0, 3); 94 | observation.middleRows(3, 4) = state.middleRows(6, 4).normalized(); 95 | 96 | return observation; 97 | } 98 | 99 | double dt; 100 | }; 101 | 102 | } 103 | 104 | #endif // POSE_SYSTEM_HPP 105 | -------------------------------------------------------------------------------- /include/kkl/alg/unscented_kalman_filter.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * UnscentedKalmanFilterX.hpp 3 | * @author koide 4 | * 16/02/01 5 | **/ 6 | #ifndef KKL_UNSCENTED_KALMAN_FILTER_X_HPP 7 | #define KKL_UNSCENTED_KALMAN_FILTER_X_HPP 8 | 9 | #include 10 | #include 11 | 12 | namespace kkl { 13 | namespace alg { 14 | 15 | /** 16 | * @brief Unscented Kalman Filter class 17 | * @param T scaler type 18 | * @param System system class to be estimated 19 | */ 20 | template 21 | class UnscentedKalmanFilterX { 22 | typedef Eigen::Matrix VectorXt; 23 | typedef Eigen::Matrix MatrixXt; 24 | public: 25 | /** 26 | * @brief constructor 27 | * @param system system to be estimated 28 | * @param state_dim state vector dimension 29 | * @param input_dim input vector dimension 30 | * @param measurement_dim measurement vector dimension 31 | * @param process_noise process noise covariance (state_dim x state_dim) 32 | * @param measurement_noise measurement noise covariance (measurement_dim x measuremend_dim) 33 | * @param mean initial mean 34 | * @param cov initial covariance 35 | */ 36 | UnscentedKalmanFilterX(const System& system, int state_dim, int input_dim, int measurement_dim, const MatrixXt& process_noise, const MatrixXt& measurement_noise, const VectorXt& mean, const MatrixXt& cov) 37 | : state_dim(state_dim), 38 | input_dim(input_dim), 39 | measurement_dim(measurement_dim), 40 | N(state_dim), 41 | M(input_dim), 42 | K(measurement_dim), 43 | S(2 * state_dim + 1), 44 | mean(mean), 45 | cov(cov), 46 | system(system), 47 | process_noise(process_noise), 48 | measurement_noise(measurement_noise), 49 | lambda(1), 50 | normal_dist(0.0, 1.0) 51 | { 52 | weights.resize(S, 1); 53 | sigma_points.resize(S, N); 54 | ext_weights.resize(2 * (N + K) + 1, 1); 55 | ext_sigma_points.resize(2 * (N + K) + 1, N + K); 56 | expected_measurements.resize(2 * (N + K) + 1, K); 57 | 58 | // initialize weights for unscented filter 59 | weights[0] = lambda / (N + lambda); 60 | for (int i = 1; i < 2 * N + 1; i++) { 61 | weights[i] = 1 / (2 * (N + lambda)); 62 | } 63 | 64 | // weights for extended state space which includes error variances 65 | ext_weights[0] = lambda / (N + K + lambda); 66 | for (int i = 1; i < 2 * (N + K) + 1; i++) { 67 | ext_weights[i] = 1 / (2 * (N + K + lambda)); 68 | } 69 | } 70 | 71 | /** 72 | * @brief predict 73 | * @param control input vector 74 | */ 75 | void predict() { 76 | // calculate sigma points 77 | ensurePositiveFinite(cov); 78 | computeSigmaPoints(mean, cov, sigma_points); 79 | for (int i = 0; i < S; i++) { 80 | sigma_points.row(i) = system.f(sigma_points.row(i)); 81 | } 82 | 83 | const auto& R = process_noise; 84 | 85 | // unscented transform 86 | VectorXt mean_pred(mean.size()); 87 | MatrixXt cov_pred(cov.rows(), cov.cols()); 88 | 89 | mean_pred.setZero(); 90 | cov_pred.setZero(); 91 | for (int i = 0; i < S; i++) { 92 | mean_pred += weights[i] * sigma_points.row(i); 93 | } 94 | for (int i = 0; i < S; i++) { 95 | VectorXt diff = sigma_points.row(i).transpose() - mean_pred; 96 | cov_pred += weights[i] * diff * diff.transpose(); 97 | } 98 | cov_pred += R; 99 | 100 | mean = mean_pred; 101 | cov = cov_pred; 102 | } 103 | 104 | /** 105 | * @brief predict 106 | * @param control input vector 107 | */ 108 | void predict(const VectorXt& control) { 109 | // calculate sigma points 110 | ensurePositiveFinite(cov); 111 | computeSigmaPoints(mean, cov, sigma_points); 112 | for (int i = 0; i < S; i++) { 113 | sigma_points.row(i) = system.f(sigma_points.row(i), control); 114 | } 115 | 116 | const auto& R = process_noise; 117 | 118 | // unscented transform 119 | VectorXt mean_pred(mean.size()); 120 | MatrixXt cov_pred(cov.rows(), cov.cols()); 121 | 122 | mean_pred.setZero(); 123 | cov_pred.setZero(); 124 | for (int i = 0; i < S; i++) { 125 | mean_pred += weights[i] * sigma_points.row(i); 126 | } 127 | for (int i = 0; i < S; i++) { 128 | VectorXt diff = sigma_points.row(i).transpose() - mean_pred; 129 | cov_pred += weights[i] * diff * diff.transpose(); 130 | } 131 | cov_pred += R; 132 | 133 | mean = mean_pred; 134 | cov = cov_pred; 135 | } 136 | 137 | /** 138 | * @brief correct 139 | * @param measurement measurement vector 140 | */ 141 | void correct(const VectorXt& measurement) { 142 | // create extended state space which includes error variances 143 | VectorXt ext_mean_pred = VectorXt::Zero(N + K, 1); 144 | MatrixXt ext_cov_pred = MatrixXt::Zero(N + K, N + K); 145 | ext_mean_pred.topLeftCorner(N, 1) = VectorXt(mean); 146 | ext_cov_pred.topLeftCorner(N, N) = MatrixXt(cov); 147 | ext_cov_pred.bottomRightCorner(K, K) = measurement_noise; 148 | 149 | ensurePositiveFinite(ext_cov_pred); 150 | computeSigmaPoints(ext_mean_pred, ext_cov_pred, ext_sigma_points); 151 | 152 | // unscented transform 153 | expected_measurements.setZero(); 154 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 155 | expected_measurements.row(i) = system.h(ext_sigma_points.row(i).transpose().topLeftCorner(N, 1)); 156 | expected_measurements.row(i) += VectorXt(ext_sigma_points.row(i).transpose().bottomRightCorner(K, 1)); 157 | } 158 | 159 | VectorXt expected_measurement_mean = VectorXt::Zero(K); 160 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 161 | expected_measurement_mean += ext_weights[i] * expected_measurements.row(i); 162 | } 163 | MatrixXt expected_measurement_cov = MatrixXt::Zero(K, K); 164 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 165 | VectorXt diff = expected_measurements.row(i).transpose() - expected_measurement_mean; 166 | expected_measurement_cov += ext_weights[i] * diff * diff.transpose(); 167 | } 168 | 169 | // calculated transformed covariance 170 | MatrixXt sigma = MatrixXt::Zero(N + K, K); 171 | for (int i = 0; i < ext_sigma_points.rows(); i++) { 172 | auto diffA = (ext_sigma_points.row(i).transpose() - ext_mean_pred); 173 | auto diffB = (expected_measurements.row(i).transpose() - expected_measurement_mean); 174 | sigma += ext_weights[i] * (diffA * diffB.transpose()); 175 | } 176 | 177 | kalman_gain = sigma * expected_measurement_cov.inverse(); 178 | const auto& K = kalman_gain; 179 | 180 | VectorXt ext_mean = ext_mean_pred + K * (measurement - expected_measurement_mean); 181 | MatrixXt ext_cov = ext_cov_pred - K * expected_measurement_cov * K.transpose(); 182 | 183 | mean = ext_mean.topLeftCorner(N, 1); 184 | cov = ext_cov.topLeftCorner(N, N); 185 | } 186 | 187 | /* getter */ 188 | const VectorXt& getMean() const { return mean; } 189 | const MatrixXt& getCov() const { return cov; } 190 | const MatrixXt& getSigmaPoints() const { return sigma_points; } 191 | 192 | System& getSystem() { return system; } 193 | const System& getSystem() const { return system; } 194 | const MatrixXt& getProcessNoiseCov() const { return process_noise; } 195 | const MatrixXt& getMeasurementNoiseCov() const { return measurement_noise; } 196 | 197 | const MatrixXt& getKalmanGain() const { return kalman_gain; } 198 | 199 | /* setter */ 200 | UnscentedKalmanFilterX& setMean(const VectorXt& m) { mean = m; return *this; } 201 | UnscentedKalmanFilterX& setCov(const MatrixXt& s) { cov = s; return *this; } 202 | 203 | UnscentedKalmanFilterX& setProcessNoiseCov(const MatrixXt& p) { process_noise = p; return *this; } 204 | UnscentedKalmanFilterX& setMeasurementNoiseCov(const MatrixXt& m) { measurement_noise = m; return *this; } 205 | 206 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 207 | private: 208 | const int state_dim; 209 | const int input_dim; 210 | const int measurement_dim; 211 | 212 | const int N; 213 | const int M; 214 | const int K; 215 | const int S; 216 | 217 | public: 218 | VectorXt mean; 219 | MatrixXt cov; 220 | 221 | System system; 222 | MatrixXt process_noise; // 223 | MatrixXt measurement_noise; // 224 | 225 | T lambda; 226 | VectorXt weights; 227 | 228 | MatrixXt sigma_points; 229 | 230 | VectorXt ext_weights; 231 | MatrixXt ext_sigma_points; 232 | MatrixXt expected_measurements; 233 | 234 | private: 235 | /** 236 | * @brief compute sigma points 237 | * @param mean mean 238 | * @param cov covariance 239 | * @param sigma_points calculated sigma points 240 | */ 241 | void computeSigmaPoints(const VectorXt& mean, const MatrixXt& cov, MatrixXt& sigma_points) { 242 | const int n = mean.size(); 243 | assert(cov.rows() == n && cov.cols() == n); 244 | 245 | Eigen::LLT llt; 246 | llt.compute((n + lambda) * cov); 247 | MatrixXt l = llt.matrixL(); 248 | 249 | sigma_points.row(0) = mean; 250 | for (int i = 0; i < n; i++) { 251 | sigma_points.row(1 + i * 2) = mean + l.col(i); 252 | sigma_points.row(1 + i * 2 + 1) = mean - l.col(i); 253 | } 254 | } 255 | 256 | /** 257 | * @brief make covariance matrix positive finite 258 | * @param cov covariance matrix 259 | */ 260 | void ensurePositiveFinite(MatrixXt& cov) { 261 | return; 262 | const double eps = 1e-9; 263 | 264 | Eigen::EigenSolver solver(cov); 265 | MatrixXt D = solver.pseudoEigenvalueMatrix(); 266 | MatrixXt V = solver.pseudoEigenvectors(); 267 | for (int i = 0; i < D.rows(); i++) { 268 | if (D(i, i) < eps) { 269 | D(i, i) = eps; 270 | } 271 | } 272 | 273 | cov = V * D * V.inverse(); 274 | } 275 | 276 | public: 277 | MatrixXt kalman_gain; 278 | 279 | std::mt19937 mt; 280 | std::normal_distribution normal_dist; 281 | }; 282 | 283 | } 284 | } 285 | 286 | 287 | #endif 288 | -------------------------------------------------------------------------------- /launch/hdl_localization.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 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /msg/ScanMatchingStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | bool has_converged 4 | float32 matching_error 5 | float32 inlier_fraction 6 | geometry_msgs/Transform relative_pose 7 | 8 | std_msgs/String[] prediction_labels 9 | geometry_msgs/Transform[] prediction_errors -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | hdl_localization 4 | 0.0.0 5 | The hdl_localization package 6 | 7 | koide 8 | 9 | BSD 10 | 11 | catkin 12 | nodelet 13 | tf2 14 | tf2_ros 15 | tf2_eigen 16 | tf2_geometry_msgs 17 | eigen_conversions 18 | pcl_ros 19 | roscpp 20 | rospy 21 | sensor_msgs 22 | geometry_msgs 23 | message_generation 24 | ndt_omp 25 | fast_gicp 26 | hdl_global_localization 27 | 28 | nodelet 29 | tf2 30 | tf2_ros 31 | tf2_eigen 32 | tf2_geometry_msgs 33 | eigen_conversions 34 | pcl_ros 35 | roscpp 36 | rospy 37 | sensor_msgs 38 | geometry_msgs 39 | message_generation 40 | ndt_omp 41 | fast_gicp 42 | hdl_global_localization 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /rviz/hdl_localization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 125 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud23 10 | - /MarkerArray1/Namespaces1 11 | Splitter Ratio: 0.5 12 | Tree Height: 1231 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: PointCloud2 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 10 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 50 55 | Reference Frame: 56 | Value: true 57 | - Class: rviz/TF 58 | Enabled: true 59 | Frame Timeout: 50 60 | Frames: 61 | All Enabled: true 62 | map: 63 | Value: true 64 | velodyne: 65 | Value: true 66 | Marker Alpha: 1 67 | Marker Scale: 4 68 | Name: TF 69 | Show Arrows: true 70 | Show Axes: true 71 | Show Names: true 72 | Tree: 73 | map: 74 | velodyne: 75 | {} 76 | Update Interval: 0 77 | Value: true 78 | - Alpha: 1 79 | Autocompute Intensity Bounds: true 80 | Autocompute Value Bounds: 81 | Max Value: 10 82 | Min Value: -10 83 | Value: true 84 | Axis: Z 85 | Channel Name: intensity 86 | Class: rviz/PointCloud2 87 | Color: 255; 255; 255 88 | Color Transformer: Intensity 89 | Decay Time: 0 90 | Enabled: true 91 | Invert Rainbow: false 92 | Max Color: 255; 255; 255 93 | Max Intensity: 108 94 | Min Color: 0; 0; 0 95 | Min Intensity: 0 96 | Name: PointCloud2 97 | Position Transformer: XYZ 98 | Queue Size: 10 99 | Selectable: true 100 | Size (Pixels): 3 101 | Size (m): 0.009999999776482582 102 | Style: Flat Squares 103 | Topic: /velodyne_points 104 | Unreliable: false 105 | Use Fixed Frame: true 106 | Use rainbow: true 107 | Value: true 108 | - Alpha: 1 109 | Autocompute Intensity Bounds: true 110 | Autocompute Value Bounds: 111 | Max Value: 10 112 | Min Value: -10 113 | Value: true 114 | Axis: Z 115 | Channel Name: z 116 | Class: rviz/PointCloud2 117 | Color: 255; 255; 255 118 | Color Transformer: Intensity 119 | Decay Time: 0 120 | Enabled: true 121 | Invert Rainbow: false 122 | Max Color: 255; 255; 255 123 | Max Intensity: 29.598661422729492 124 | Min Color: 0; 0; 0 125 | Min Intensity: -3.1513378620147705 126 | Name: PointCloud2 127 | Position Transformer: XYZ 128 | Queue Size: 10 129 | Selectable: true 130 | Size (Pixels): 3 131 | Size (m): 0.05000000074505806 132 | Style: Flat Squares 133 | Topic: /globalmap 134 | Unreliable: false 135 | Use Fixed Frame: true 136 | Use rainbow: true 137 | Value: true 138 | - Alpha: 1 139 | Autocompute Intensity Bounds: true 140 | Autocompute Value Bounds: 141 | Max Value: 10 142 | Min Value: -10 143 | Value: true 144 | Axis: Z 145 | Channel Name: intensity 146 | Class: rviz/PointCloud2 147 | Color: 255; 255; 255 148 | Color Transformer: Intensity 149 | Decay Time: 0 150 | Enabled: true 151 | Invert Rainbow: false 152 | Max Color: 255; 255; 255 153 | Max Intensity: 108 154 | Min Color: 255; 85; 0 155 | Min Intensity: 0 156 | Name: PointCloud2 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 3 161 | Size (m): 0.20000000298023224 162 | Style: Flat Squares 163 | Topic: /aligned_points 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: false 167 | Value: true 168 | - Angle Tolerance: 0.10000000149011612 169 | Class: rviz/Odometry 170 | Covariance: 171 | Orientation: 172 | Alpha: 0.5 173 | Color: 255; 255; 127 174 | Color Style: Unique 175 | Frame: Local 176 | Offset: 1 177 | Scale: 1 178 | Value: true 179 | Position: 180 | Alpha: 0.30000001192092896 181 | Color: 204; 51; 204 182 | Scale: 1 183 | Value: true 184 | Value: true 185 | Enabled: true 186 | Keep: 100 187 | Name: Odometry 188 | Position Tolerance: 0.10000000149011612 189 | Queue Size: 10 190 | Shape: 191 | Alpha: 1 192 | Axes Length: 1 193 | Axes Radius: 0.10000000149011612 194 | Color: 255; 25; 0 195 | Head Length: 0.30000001192092896 196 | Head Radius: 0.10000000149011612 197 | Shaft Length: 1 198 | Shaft Radius: 0.05000000074505806 199 | Value: Arrow 200 | Topic: /odom 201 | Unreliable: false 202 | Value: true 203 | - Alpha: 1 204 | Autocompute Intensity Bounds: true 205 | Autocompute Value Bounds: 206 | Max Value: 10 207 | Min Value: -10 208 | Value: true 209 | Axis: Z 210 | Channel Name: intensity 211 | Class: rviz/PointCloud2 212 | Color: 255; 0; 0 213 | Color Transformer: FlatColor 214 | Decay Time: 0 215 | Enabled: false 216 | Invert Rainbow: false 217 | Max Color: 255; 255; 255 218 | Max Intensity: 49.5 219 | Min Color: 0; 0; 0 220 | Min Intensity: 2 221 | Name: PointCloud2 222 | Position Transformer: XYZ 223 | Queue Size: 10 224 | Selectable: true 225 | Size (Pixels): 3 226 | Size (m): 0.20000000298023224 227 | Style: Flat Squares 228 | Topic: /hdl_people_detection_nodelet/human_points 229 | Unreliable: false 230 | Use Fixed Frame: true 231 | Use rainbow: false 232 | Value: false 233 | - Class: rviz/MarkerArray 234 | Enabled: false 235 | Marker Topic: /hdl_people_detection_nodelet/detection_markers 236 | Name: MarkerArray 237 | Namespaces: 238 | {} 239 | Queue Size: 100 240 | Value: false 241 | - Alpha: 0.5 242 | Autocompute Intensity Bounds: true 243 | Autocompute Value Bounds: 244 | Max Value: 10 245 | Min Value: -10 246 | Value: true 247 | Axis: Z 248 | Channel Name: intensity 249 | Class: rviz/PointCloud2 250 | Color: 59; 0; 255 251 | Color Transformer: FlatColor 252 | Decay Time: 0 253 | Enabled: false 254 | Invert Rainbow: false 255 | Max Color: 255; 255; 255 256 | Max Intensity: 0 257 | Min Color: 0; 0; 0 258 | Min Intensity: 0 259 | Name: PointCloud2 260 | Position Transformer: XYZ 261 | Queue Size: 10 262 | Selectable: true 263 | Size (Pixels): 3 264 | Size (m): 0.10000000149011612 265 | Style: Flat Squares 266 | Topic: /hdl_people_detection_nodelet/backsub_voxel_points 267 | Unreliable: false 268 | Use Fixed Frame: true 269 | Use rainbow: true 270 | Value: false 271 | - Class: rviz/MarkerArray 272 | Enabled: true 273 | Marker Topic: /hdl_people_tracking_nodelet/markers 274 | Name: MarkerArray 275 | Namespaces: 276 | {} 277 | Queue Size: 100 278 | Value: true 279 | - Alpha: 0.699999988079071 280 | Class: rviz/Map 281 | Color Scheme: map 282 | Draw Behind: false 283 | Enabled: true 284 | Name: Map 285 | Topic: /hdl_global_localization/bbs/gridmap 286 | Unreliable: false 287 | Use Timestamp: false 288 | Value: true 289 | - Alpha: 1 290 | Autocompute Intensity Bounds: true 291 | Autocompute Value Bounds: 292 | Max Value: 10 293 | Min Value: -10 294 | Value: true 295 | Axis: Z 296 | Channel Name: intensity 297 | Class: rviz/PointCloud2 298 | Color: 255; 255; 255 299 | Color Transformer: Intensity 300 | Decay Time: 0 301 | Enabled: true 302 | Invert Rainbow: false 303 | Max Color: 0; 255; 0 304 | Max Intensity: 4096 305 | Min Color: 0; 0; 0 306 | Min Intensity: 0 307 | Name: PointCloud2 308 | Position Transformer: XYZ 309 | Queue Size: 10 310 | Selectable: true 311 | Size (Pixels): 3 312 | Size (m): 0.10000000149011612 313 | Style: Flat Squares 314 | Topic: /hdl_global_localization/bbs/map_slice 315 | Unreliable: false 316 | Use Fixed Frame: true 317 | Use rainbow: true 318 | Value: true 319 | Enabled: true 320 | Global Options: 321 | Background Color: 48; 48; 48 322 | Default Light: true 323 | Fixed Frame: map 324 | Frame Rate: 30 325 | Name: root 326 | Tools: 327 | - Class: rviz/Interact 328 | Hide Inactive Objects: true 329 | - Class: rviz/MoveCamera 330 | - Class: rviz/Select 331 | - Class: rviz/FocusCamera 332 | - Class: rviz/Measure 333 | - Class: rviz/SetInitialPose 334 | Theta std deviation: 0.2617993950843811 335 | Topic: /initialpose 336 | X std deviation: 0.5 337 | Y std deviation: 0.5 338 | - Class: rviz/SetGoal 339 | Topic: /move_base_simple/goal 340 | - Class: rviz/PublishPoint 341 | Single click: true 342 | Topic: /clicked_point 343 | Value: true 344 | Views: 345 | Current: 346 | Class: rviz/Orbit 347 | Distance: 178.50033569335938 348 | Enable Stereo Rendering: 349 | Stereo Eye Separation: 0.05999999865889549 350 | Stereo Focal Distance: 1 351 | Swap Stereo Eyes: false 352 | Value: false 353 | Field of View: 0.7853981852531433 354 | Focal Point: 355 | X: -17.491586685180664 356 | Y: 11.979555130004883 357 | Z: -31.46576499938965 358 | Focal Shape Fixed Size: true 359 | Focal Shape Size: 0.05000000074505806 360 | Invert Z Axis: false 361 | Name: Current View 362 | Near Clip Distance: 0.009999999776482582 363 | Pitch: 1.1597964763641357 364 | Target Frame: 365 | Yaw: 6.019834995269775 366 | Saved: ~ 367 | Window Geometry: 368 | Displays: 369 | collapsed: false 370 | Height: 1660 371 | Hide Left Dock: false 372 | Hide Right Dock: false 373 | QMainWindow State: 000000ff00000000fd00000004000000000000020f000005a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007a01000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000052000005a30000010701000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011a000005a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000052000005a3000000e601000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000095900000056fc0100000002fb0000000800540069006d00650100000000000009590000039a01000003fb0000000800540069006d006501000000000000045000000000000000000000062e000005a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 374 | Selection: 375 | collapsed: false 376 | Time: 377 | collapsed: false 378 | Tool Properties: 379 | collapsed: false 380 | Views: 381 | collapsed: false 382 | Width: 2393 383 | X: 0 384 | Y: 0 385 | -------------------------------------------------------------------------------- /scripts/plot_status.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import rospy 3 | import numpy 4 | import scipy.spatial 5 | from matplotlib import pyplot 6 | from hdl_localization.msg import * 7 | 8 | 9 | class Plotter(object): 10 | def __init__(self): 11 | pyplot.ion() 12 | pyplot.show(block=False) 13 | 14 | self.status_buffer = [] 15 | self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback) 16 | self.status_sub = rospy.Subscriber('/status', ScanMatchingStatus, self.status_callback) 17 | 18 | def status_callback(self, status_msg): 19 | self.status_buffer.append(status_msg) 20 | 21 | if len(self.status_buffer) > 50: 22 | self.status_buffer = self.status_buffer[-50:] 23 | 24 | def timer_callback(self, event): 25 | if len(self.status_buffer) < 2: 26 | return 27 | 28 | errors = {} 29 | for status in self.status_buffer: 30 | for label, error in zip(status.prediction_labels, status.prediction_errors): 31 | if label.data not in errors: 32 | errors[label.data] = [] 33 | 34 | quat = [error.rotation.x, error.rotation.y, error.rotation.z, error.rotation.w] 35 | trans = [error.translation.x, error.translation.y, error.translation.z] 36 | 37 | t = status.header.stamp.secs + status.header.stamp.nsecs / 1e9 38 | t_error = numpy.linalg.norm(trans) 39 | r_error = numpy.linalg.norm(scipy.spatial.transform.Rotation.from_quat(quat).as_rotvec()) 40 | 41 | if len(errors[label.data]) and abs(errors[label.data][-1][0] - t) > 1.0: 42 | errors[label.data] = [] 43 | 44 | errors[label.data].append((t, t_error, r_error)) 45 | 46 | pyplot.clf() 47 | for label in errors: 48 | errs = numpy.float64(errors[label]) 49 | pyplot.subplot('211') 50 | pyplot.plot(errs[:, 0], errs[:, 1], label=label) 51 | 52 | pyplot.subplot('212') 53 | pyplot.plot(errs[:, 0], errs[:, 2], label=label) 54 | 55 | pyplot.subplot('211') 56 | pyplot.ylabel('trans error') 57 | pyplot.subplot('212') 58 | pyplot.ylabel('rot error') 59 | 60 | pyplot.legend(loc='upper center', bbox_to_anchor=(0.5, -0.05), ncol=len(errors)) 61 | pyplot.gcf().canvas.flush_events() 62 | # pyplot.pause(0.0001) 63 | 64 | 65 | def main(): 66 | rospy.init_node('status_plotter') 67 | node = Plotter() 68 | rospy.spin() 69 | 70 | if __name__ == '__main__': 71 | main() 72 | -------------------------------------------------------------------------------- /src/hdl_localization/pose_estimator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace hdl_localization { 9 | 10 | /** 11 | * @brief constructor 12 | * @param registration registration method 13 | * @param pos initial position 14 | * @param quat initial orientation 15 | * @param cool_time_duration during "cool time", prediction is not performed 16 | */ 17 | PoseEstimator::PoseEstimator(pcl::Registration::Ptr& registration, const Eigen::Vector3f& pos, const Eigen::Quaternionf& quat, double cool_time_duration) 18 | : registration(registration), cool_time_duration(cool_time_duration) { 19 | last_observation = Eigen::Matrix4f::Identity(); 20 | last_observation.block<3, 3>(0, 0) = quat.toRotationMatrix(); 21 | last_observation.block<3, 1>(0, 3) = pos; 22 | 23 | process_noise = Eigen::MatrixXf::Identity(16, 16); 24 | process_noise.middleRows(0, 3) *= 1.0; 25 | process_noise.middleRows(3, 3) *= 1.0; 26 | process_noise.middleRows(6, 4) *= 0.5; 27 | process_noise.middleRows(10, 3) *= 1e-6; 28 | process_noise.middleRows(13, 3) *= 1e-6; 29 | 30 | Eigen::MatrixXf measurement_noise = Eigen::MatrixXf::Identity(7, 7); 31 | measurement_noise.middleRows(0, 3) *= 0.01; 32 | measurement_noise.middleRows(3, 4) *= 0.001; 33 | 34 | Eigen::VectorXf mean(16); 35 | mean.middleRows(0, 3) = pos; 36 | mean.middleRows(3, 3).setZero(); 37 | mean.middleRows(6, 4) = Eigen::Vector4f(quat.w(), quat.x(), quat.y(), quat.z()).normalized(); 38 | mean.middleRows(10, 3).setZero(); 39 | mean.middleRows(13, 3).setZero(); 40 | 41 | Eigen::MatrixXf cov = Eigen::MatrixXf::Identity(16, 16) * 0.01; 42 | 43 | PoseSystem system; 44 | ukf.reset(new kkl::alg::UnscentedKalmanFilterX(system, 16, 6, 7, process_noise, measurement_noise, mean, cov)); 45 | } 46 | 47 | PoseEstimator::~PoseEstimator() {} 48 | 49 | /** 50 | * @brief predict 51 | * @param stamp timestamp 52 | * @param acc acceleration 53 | * @param gyro angular velocity 54 | */ 55 | void PoseEstimator::predict(const ros::Time& stamp) { 56 | if (init_stamp.is_zero()) { 57 | init_stamp = stamp; 58 | } 59 | 60 | if ((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) { 61 | prev_stamp = stamp; 62 | return; 63 | } 64 | 65 | double dt = (stamp - prev_stamp).toSec(); 66 | prev_stamp = stamp; 67 | 68 | ukf->setProcessNoiseCov(process_noise * dt); 69 | ukf->system.dt = dt; 70 | 71 | ukf->predict(); 72 | } 73 | 74 | /** 75 | * @brief predict 76 | * @param stamp timestamp 77 | * @param acc acceleration 78 | * @param gyro angular velocity 79 | */ 80 | void PoseEstimator::predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) { 81 | if (init_stamp.is_zero()) { 82 | init_stamp = stamp; 83 | } 84 | 85 | if ((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) { 86 | prev_stamp = stamp; 87 | return; 88 | } 89 | 90 | double dt = (stamp - prev_stamp).toSec(); 91 | prev_stamp = stamp; 92 | 93 | ukf->setProcessNoiseCov(process_noise * dt); 94 | ukf->system.dt = dt; 95 | 96 | Eigen::VectorXf control(6); 97 | control.head<3>() = acc; 98 | control.tail<3>() = gyro; 99 | 100 | ukf->predict(control); 101 | } 102 | 103 | /** 104 | * @brief update the state of the odomety-based pose estimation 105 | */ 106 | void PoseEstimator::predict_odom(const Eigen::Matrix4f& odom_delta) { 107 | if(!odom_ukf) { 108 | Eigen::MatrixXf odom_process_noise = Eigen::MatrixXf::Identity(7, 7); 109 | Eigen::MatrixXf odom_measurement_noise = Eigen::MatrixXf::Identity(7, 7) * 1e-3; 110 | 111 | Eigen::VectorXf odom_mean(7); 112 | odom_mean.block<3, 1>(0, 0) = Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]); 113 | odom_mean.block<4, 1>(3, 0) = Eigen::Vector4f(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]); 114 | Eigen::MatrixXf odom_cov = Eigen::MatrixXf::Identity(7, 7) * 1e-2; 115 | 116 | OdomSystem odom_system; 117 | odom_ukf.reset(new kkl::alg::UnscentedKalmanFilterX(odom_system, 7, 7, 7, odom_process_noise, odom_measurement_noise, odom_mean, odom_cov)); 118 | } 119 | 120 | // invert quaternion if the rotation axis is flipped 121 | Eigen::Quaternionf quat(odom_delta.block<3, 3>(0, 0)); 122 | if(odom_quat().coeffs().dot(quat.coeffs()) < 0.0) { 123 | quat.coeffs() *= -1.0f; 124 | } 125 | 126 | Eigen::VectorXf control(7); 127 | control.middleRows(0, 3) = odom_delta.block<3, 1>(0, 3); 128 | control.middleRows(3, 4) = Eigen::Vector4f(quat.w(), quat.x(), quat.y(), quat.z()); 129 | 130 | Eigen::MatrixXf process_noise = Eigen::MatrixXf::Identity(7, 7); 131 | process_noise.topLeftCorner(3, 3) = Eigen::Matrix3f::Identity() * odom_delta.block<3, 1>(0, 3).norm() + Eigen::Matrix3f::Identity() * 1e-3; 132 | process_noise.bottomRightCorner(4, 4) = Eigen::Matrix4f::Identity() * (1 - std::abs(quat.w())) + Eigen::Matrix4f::Identity() * 1e-3; 133 | 134 | odom_ukf->setProcessNoiseCov(process_noise); 135 | odom_ukf->predict(control); 136 | } 137 | 138 | /** 139 | * @brief correct 140 | * @param cloud input cloud 141 | * @return cloud aligned to the globalmap 142 | */ 143 | pcl::PointCloud::Ptr PoseEstimator::correct(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud) { 144 | if (init_stamp.is_zero()) { 145 | init_stamp = stamp; 146 | } 147 | 148 | last_correction_stamp = stamp; 149 | 150 | Eigen::Matrix4f no_guess = last_observation; 151 | Eigen::Matrix4f imu_guess; 152 | Eigen::Matrix4f odom_guess; 153 | Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity(); 154 | 155 | if(!odom_ukf) { 156 | init_guess = imu_guess = matrix(); 157 | } else { 158 | imu_guess = matrix(); 159 | odom_guess = odom_matrix(); 160 | 161 | Eigen::VectorXf imu_mean(7); 162 | Eigen::MatrixXf imu_cov = Eigen::MatrixXf::Identity(7, 7); 163 | imu_mean.block<3, 1>(0, 0) = ukf->mean.block<3, 1>(0, 0); 164 | imu_mean.block<4, 1>(3, 0) = ukf->mean.block<4, 1>(6, 0); 165 | 166 | imu_cov.block<3, 3>(0, 0) = ukf->cov.block<3, 3>(0, 0); 167 | imu_cov.block<3, 4>(0, 3) = ukf->cov.block<3, 4>(0, 6); 168 | imu_cov.block<4, 3>(3, 0) = ukf->cov.block<4, 3>(6, 0); 169 | imu_cov.block<4, 4>(3, 3) = ukf->cov.block<4, 4>(6, 6); 170 | 171 | Eigen::VectorXf odom_mean = odom_ukf->mean; 172 | Eigen::MatrixXf odom_cov = odom_ukf->cov; 173 | 174 | if (imu_mean.tail<4>().dot(odom_mean.tail<4>()) < 0.0) { 175 | odom_mean.tail<4>() *= -1.0; 176 | } 177 | 178 | Eigen::MatrixXf inv_imu_cov = imu_cov.inverse(); 179 | Eigen::MatrixXf inv_odom_cov = odom_cov.inverse(); 180 | 181 | Eigen::MatrixXf fused_cov = (inv_imu_cov + inv_odom_cov).inverse(); 182 | Eigen::VectorXf fused_mean = fused_cov * inv_imu_cov * imu_mean + fused_cov * inv_odom_cov * odom_mean; 183 | 184 | init_guess.block<3, 1>(0, 3) = Eigen::Vector3f(fused_mean[0], fused_mean[1], fused_mean[2]); 185 | init_guess.block<3, 3>(0, 0) = Eigen::Quaternionf(fused_mean[3], fused_mean[4], fused_mean[5], fused_mean[6]).normalized().toRotationMatrix(); 186 | } 187 | 188 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); 189 | registration->setInputSource(cloud); 190 | registration->align(*aligned, init_guess); 191 | 192 | Eigen::Matrix4f trans = registration->getFinalTransformation(); 193 | Eigen::Vector3f p = trans.block<3, 1>(0, 3); 194 | Eigen::Quaternionf q(trans.block<3, 3>(0, 0)); 195 | 196 | if(quat().coeffs().dot(q.coeffs()) < 0.0f) { 197 | q.coeffs() *= -1.0f; 198 | } 199 | 200 | Eigen::VectorXf observation(7); 201 | observation.middleRows(0, 3) = p; 202 | observation.middleRows(3, 4) = Eigen::Vector4f(q.w(), q.x(), q.y(), q.z()); 203 | last_observation = trans; 204 | 205 | wo_pred_error = no_guess.inverse() * registration->getFinalTransformation(); 206 | 207 | ukf->correct(observation); 208 | imu_pred_error = imu_guess.inverse() * registration->getFinalTransformation(); 209 | 210 | if(odom_ukf) { 211 | if (observation.tail<4>().dot(odom_ukf->mean.tail<4>()) < 0.0) { 212 | odom_ukf->mean.tail<4>() *= -1.0; 213 | } 214 | 215 | odom_ukf->correct(observation); 216 | odom_pred_error = odom_guess.inverse() * registration->getFinalTransformation(); 217 | } 218 | 219 | return aligned; 220 | } 221 | 222 | /* getters */ 223 | ros::Time PoseEstimator::last_correction_time() const { 224 | return last_correction_stamp; 225 | } 226 | 227 | Eigen::Vector3f PoseEstimator::pos() const { 228 | return Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]); 229 | } 230 | 231 | Eigen::Vector3f PoseEstimator::vel() const { 232 | return Eigen::Vector3f(ukf->mean[3], ukf->mean[4], ukf->mean[5]); 233 | } 234 | 235 | Eigen::Quaternionf PoseEstimator::quat() const { 236 | return Eigen::Quaternionf(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]).normalized(); 237 | } 238 | 239 | Eigen::Matrix4f PoseEstimator::matrix() const { 240 | Eigen::Matrix4f m = Eigen::Matrix4f::Identity(); 241 | m.block<3, 3>(0, 0) = quat().toRotationMatrix(); 242 | m.block<3, 1>(0, 3) = pos(); 243 | return m; 244 | } 245 | 246 | Eigen::Vector3f PoseEstimator::odom_pos() const { 247 | return Eigen::Vector3f(odom_ukf->mean[0], odom_ukf->mean[1], odom_ukf->mean[2]); 248 | } 249 | 250 | Eigen::Quaternionf PoseEstimator::odom_quat() const { 251 | return Eigen::Quaternionf(odom_ukf->mean[3], odom_ukf->mean[4], odom_ukf->mean[5], odom_ukf->mean[6]).normalized(); 252 | } 253 | 254 | Eigen::Matrix4f PoseEstimator::odom_matrix() const { 255 | Eigen::Matrix4f m = Eigen::Matrix4f::Identity(); 256 | m.block<3, 3>(0, 0) = odom_quat().toRotationMatrix(); 257 | m.block<3, 1>(0, 3) = odom_pos(); 258 | return m; 259 | } 260 | 261 | const boost::optional& PoseEstimator::wo_prediction_error() const { 262 | return wo_pred_error; 263 | } 264 | 265 | const boost::optional& PoseEstimator::imu_prediction_error() const { 266 | return imu_pred_error; 267 | } 268 | 269 | const boost::optional& PoseEstimator::odom_prediction_error() const { 270 | return odom_pred_error; 271 | } 272 | } --------------------------------------------------------------------------------