├── .gitignore ├── LICENSE ├── README.md ├── demo.mp4 ├── demo_pic.png ├── mad-icp.gif ├── mad-icp ├── CMakeLists.txt ├── cmake │ ├── FindCSparse.cmake │ ├── FindCholmod.cmake │ ├── FindGlog.cmake │ └── packages.cmake ├── config │ └── mapping_bag.yaml ├── launch │ ├── run_mad.launch │ └── rviz.rviz ├── log │ ├── 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 │ ├── 72 │ ├── 73 │ ├── 74 │ ├── 75 │ ├── 76 │ ├── 77 │ ├── 78 │ ├── 79 │ ├── 80 │ ├── 81 │ ├── 82 │ ├── 83 │ ├── 84 │ ├── 85 │ ├── 86 │ ├── 87 │ ├── 88 │ ├── 89 │ ├── 90 │ ├── 91 │ ├── 92 │ ├── 93 │ ├── 94 │ ├── 95 │ ├── 96 │ ├── 97 │ ├── 98 │ ├── 99 │ └── log_time.txt ├── package.xml └── src │ ├── apps │ ├── CMakeLists.txt │ ├── bin_runner.cpp │ ├── cpp_utils │ │ └── parse_cmd_line.h │ └── mad_icp.cpp │ ├── common │ ├── CMakeLists.txt │ ├── eigen_types.h │ ├── global_flags.cc │ ├── global_flags.h │ ├── io_utils.cc │ ├── io_utils.h │ ├── message_def.h │ ├── point_types.h │ ├── sys_utils.h │ ├── timer │ │ ├── timer.cc │ │ └── timer.h │ └── tool_color_printf.hpp │ ├── mad_icp │ ├── CMakeLists.txt │ ├── odometry │ │ ├── CMakeLists.txt │ │ ├── mad_icp.cpp │ │ ├── mad_icp.h │ │ ├── pipeline.cpp │ │ ├── pipeline.h │ │ ├── vel_estimator.cpp │ │ └── vel_estimator.h │ └── tools │ │ ├── CMakeLists.txt │ │ ├── constants.h │ │ ├── frame.h │ │ ├── lie_algebra.h │ │ ├── mad_tree.cpp │ │ ├── mad_tree.h │ │ └── utils.h │ └── preprocess │ ├── CMakeLists.txt │ └── cloud_convert2 │ ├── cloud_convert2.cc │ └── cloud_convert2.h └── paper_with_supplementary.pdf /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Robots Vision and Perception Group 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # MAD-ICP Ros Version 3 | 4 | This is a Ros Version Mad-ICP 5 | 6 |
7 | 8 | 9 |
10 | 11 | ## New Features 12 | 1. ROS support,with a simple viewer 13 | 2. multi-type lidar support(velodyne,ouster,robosense,and pandar) 14 | 15 | # ------------------------------------------------------------- 16 | # MAD-ICP 17 | 18 | ## It Is All About Matching Data -- Robust and Informed LiDAR Odometry 19 | 20 | A minimal, robust, accurate, and real-time LiDAR odometry. 21 |
22 | 23 |
24 | This version is mainly for reviewers. Soon, you can install this system via `pip` and have a proper viewer. Our preprint is available for more details and results. 25 | 26 | 27 | ## Building ## 28 | 29 | Building has been tested on Ubuntu 20.04 (with g++). 30 | 31 | The following external dependencies are required. 32 | 33 | | Dependency | Version(s) known to work | 34 | | ------------ | ------------------------ | 35 | | [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page) | 3.3 | 36 | | [OpenMP](https://www.openmp.org/) | | 37 | | [pybind11](https://pybind11.readthedocs.io/en/stable/) | | 38 | 39 | After obtaining all dependencies, the application can be built with CMake, for example, as follows: 40 | 41 | ```bash 42 | cd mad-icp 43 | mkdir build && cd build && cmake .. && make -j 44 | ``` 45 | 46 | ## Running 47 | 48 | A few other Python packages need to be installed for running, namely: `numpy,` `pyyaml`, and `rosbags.` You can find the specific versions in the `requirements.txt`. We suggest to create a virtual env and run `pip3 install -r requirements.txt`. 49 | 50 | We provide a Python launcher for both rosbags and bin formats (we are currently working on bin format luncher; for this reason, you do not find configurations for KITTI and Mulran). The configuration file is important for the sensor characteristics and extrinsic information (most of the time, ground truths are not in the LiDAR frame). The internal parameters are in `configurations/params.cfg`; all the experiments have been run with this same set. 51 | 52 | How to run (rosbag), make sure `estimate_path` and `data_path` point to a folder: 53 | ```bash 54 | cd apps 55 | python3 rosbag_runner.py --data_path /path_to_bag_folder/ --estimate_path /path_to_estimate_folder/ --dataset_config ../configurations/datasets/dataset_config_file --mad_icp_config ../configurations/params.cfg --num_cores 4 --num_keyframes 4 --realtime 56 | ``` 57 | 58 | Our runner directly saves the odometry estimate file in KITTI format (homogenous matrix row-major 12 components); in the near future, we will provide more available formats like TUM. 59 | Our pipeline is anytime realtime, therefore you can play with parameters `num_keyframes` and `num_cores`, if you have enough computation we suggest to increase these (we run demo with 16 and 16), if not you can leave it in the proposed way. 60 | -------------------------------------------------------------------------------- /demo.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/demo.mp4 -------------------------------------------------------------------------------- /demo_pic.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/demo_pic.png -------------------------------------------------------------------------------- /mad-icp.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp.gif -------------------------------------------------------------------------------- /mad-icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.2) 2 | project(mad_icp) 3 | 4 | MESSAGE(STATUS "[Release Mode]") 5 | set(CMAKE_BUILD_TYPE "Release") 6 | 7 | # MESSAGE(STATUS "[Debug Mode]") 8 | # SET(CMAKE_BUILD_TYPE "Debug") 9 | # SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -fpermissive -g2 -ggdb") 10 | 11 | # set(CMAKE_CXX_STANDARD 14) 12 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wuninitialized") 13 | 14 | # ADD_COMPILE_OPTIONS(-std=c++14 ) 15 | # ADD_COMPILE_OPTIONS(-std=c++14 ) 16 | # set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 17 | # set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 18 | # set(CMAKE_CXX_STANDARD 14) 19 | # set(CMAKE_CXX_STANDARD_REQUIRED ON) 20 | # set(CMAKE_CXX_EXTENSIONS OFF) 21 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 22 | 23 | set(CMAKE_CXX_STANDARD 17) 24 | # set(CMAKE_CXX_FLAGS -Werror) 25 | set(CMAKE_CXX_STANDARD_REQUIRED TRUE) 26 | 27 | find_package(catkin REQUIRED COMPONENTS 28 | geometry_msgs 29 | nav_msgs 30 | sensor_msgs 31 | roscpp 32 | rospy 33 | rosbag 34 | std_msgs 35 | tf 36 | eigen_conversions 37 | message_generation 38 | visualization_msgs 39 | ) 40 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 41 | 42 | 43 | catkin_package( 44 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs 45 | DEPENDS EIGEN3 PCL Ceres 46 | INCLUDE_DIRS include 47 | ) 48 | 49 | include(cmake/packages.cmake) 50 | include_directories( 51 | include 52 | ) 53 | 54 | include_directories(${PROJECT_SOURCE_DIR}) 55 | 56 | include_directories(${PROJECT_SOURCE_DIR}/src) 57 | 58 | add_subdirectory(${PROJECT_SOURCE_DIR}/src/common) 59 | 60 | add_subdirectory(${PROJECT_SOURCE_DIR}/src/preprocess) 61 | 62 | add_subdirectory(${PROJECT_SOURCE_DIR}/src/mad_icp) 63 | 64 | add_subdirectory(${PROJECT_SOURCE_DIR}/src/apps) -------------------------------------------------------------------------------- /mad-icp/cmake/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | find_path(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | PATH_SUFFIXES 14 | suitesparse 15 | ) 16 | 17 | find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse 18 | PATHS 19 | /usr/lib 20 | /usr/local/lib 21 | /opt/local/lib 22 | /sw/lib 23 | ) 24 | 25 | include(FindPackageHandleStandardArgs) 26 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 27 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 28 | -------------------------------------------------------------------------------- /mad-icp/cmake/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY 20 | NAMES cholmod libcholmod 21 | PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 22 | 23 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 24 | 25 | if(CHOLMOD_LIBRARIES) 26 | 27 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 28 | 29 | find_library(AMD_LIBRARY 30 | NAMES amd libamd 31 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 32 | if (AMD_LIBRARY) 33 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 34 | else () 35 | set(CHOLMOD_LIBRARIES FALSE) 36 | endif () 37 | 38 | endif(CHOLMOD_LIBRARIES) 39 | 40 | if(CHOLMOD_LIBRARIES) 41 | 42 | find_library(COLAMD_LIBRARY NAMES colamd libcolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 43 | if (COLAMD_LIBRARY) 44 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 45 | else () 46 | set(CHOLMOD_LIBRARIES FALSE) 47 | endif () 48 | 49 | endif(CHOLMOD_LIBRARIES) 50 | 51 | if(CHOLMOD_LIBRARIES) 52 | 53 | find_library(CAMD_LIBRARY NAMES camd libcamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 54 | if (CAMD_LIBRARY) 55 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 56 | else () 57 | set(CHOLMOD_LIBRARIES FALSE) 58 | endif () 59 | 60 | endif(CHOLMOD_LIBRARIES) 61 | 62 | if(CHOLMOD_LIBRARIES) 63 | 64 | find_library(CCOLAMD_LIBRARY NAMES ccolamd libccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 65 | if (CCOLAMD_LIBRARY) 66 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 67 | else () 68 | set(CHOLMOD_LIBRARIES FALSE) 69 | endif () 70 | 71 | endif(CHOLMOD_LIBRARIES) 72 | 73 | if(CHOLMOD_LIBRARIES) 74 | 75 | find_library(CHOLMOD_METIS_LIBRARY NAMES metis libmetis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 76 | if (CHOLMOD_METIS_LIBRARY) 77 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 78 | endif () 79 | 80 | endif(CHOLMOD_LIBRARIES) 81 | 82 | if(CHOLMOD_LIBRARIES) 83 | find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 84 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 85 | if (CHOLMOD_SUITESPARSECONFIG_LIBRARY) 86 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY}) 87 | endif () 88 | endif(CHOLMOD_LIBRARIES) 89 | 90 | include(FindPackageHandleStandardArgs) 91 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 92 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 93 | 94 | mark_as_advanced(CHOLMOD_LIBRARIES) 95 | -------------------------------------------------------------------------------- /mad-icp/cmake/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindGlog.cmake - Find Google glog logging library. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # GLOG_FOUND: TRUE iff glog is found. 37 | # GLOG_INCLUDE_DIRS: Include directories for glog. 38 | # GLOG_LIBRARIES: Libraries required to link glog. 39 | # 40 | # The following variables control the behaviour of this module: 41 | # 42 | # GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to 43 | # search for glog includes, e.g: /timbuktu/include. 44 | # GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to 45 | # search for glog libraries, e.g: /timbuktu/lib. 46 | # 47 | # The following variables are also defined by this module, but in line with 48 | # CMake recommended FindPackage() module style should NOT be referenced directly 49 | # by callers (use the plural variables detailed above instead). These variables 50 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 51 | # are NOT re-called (i.e. search for library is not repeated) if these variables 52 | # are set with valid values _in the CMake cache_. This means that if these 53 | # variables are set directly in the cache, either by the user in the CMake GUI, 54 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 55 | # explicitly defines a cache variable), then they will be used verbatim, 56 | # bypassing the HINTS variables and other hard-coded search locations. 57 | # 58 | # GLOG_INCLUDE_DIR: Include directory for glog, not including the 59 | # include directory of any dependencies. 60 | # GLOG_LIBRARY: glog library, not including the libraries of any 61 | # dependencies. 62 | 63 | # Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when 64 | # FindGlog was invoked. 65 | macro(GLOG_RESET_FIND_LIBRARY_PREFIX) 66 | if (MSVC) 67 | set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") 68 | endif (MSVC) 69 | endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) 70 | 71 | # Called if we failed to find glog or any of it's required dependencies, 72 | # unsets all public (designed to be used externally) variables and reports 73 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 74 | macro(GLOG_REPORT_NOT_FOUND REASON_MSG) 75 | unset(GLOG_FOUND) 76 | unset(GLOG_INCLUDE_DIRS) 77 | unset(GLOG_LIBRARIES) 78 | # Make results of search visible in the CMake GUI if glog has not 79 | # been found so that user does not have to toggle to advanced view. 80 | mark_as_advanced(CLEAR GLOG_INCLUDE_DIR 81 | GLOG_LIBRARY) 82 | 83 | glog_reset_find_library_prefix() 84 | 85 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 86 | # use the camelcase library name, not uppercase. 87 | if (Glog_FIND_QUIETLY) 88 | message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) 89 | elseif (Glog_FIND_REQUIRED) 90 | message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) 91 | else () 92 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 93 | # but continues configuration and allows generation. 94 | message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) 95 | endif () 96 | endmacro(GLOG_REPORT_NOT_FOUND) 97 | 98 | # Handle possible presence of lib prefix for libraries on MSVC, see 99 | # also GLOG_RESET_FIND_LIBRARY_PREFIX(). 100 | if (MSVC) 101 | # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES 102 | # s/t we can set it back before returning. 103 | set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") 104 | # The empty string in this list is important, it represents the case when 105 | # the libraries have no prefix (shared libraries / DLLs). 106 | set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") 107 | endif (MSVC) 108 | 109 | # Search user-installed locations first, so that we prefer user installs 110 | # to system installs where both exist. 111 | list(APPEND GLOG_CHECK_INCLUDE_DIRS 112 | /usr/local/include 113 | /usr/local/homebrew/include # Mac OS X 114 | /opt/local/var/macports/software # Mac OS X. 115 | /opt/local/include 116 | /usr/include) 117 | # Windows (for C:/Program Files prefix). 118 | list(APPEND GLOG_CHECK_PATH_SUFFIXES 119 | glog/include 120 | glog/Include 121 | Glog/include 122 | Glog/Include) 123 | 124 | list(APPEND GLOG_CHECK_LIBRARY_DIRS 125 | /usr/local/lib 126 | /usr/local/homebrew/lib # Mac OS X. 127 | /opt/local/lib 128 | /usr/lib) 129 | # Windows (for C:/Program Files prefix). 130 | list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES 131 | glog/lib 132 | glog/Lib 133 | Glog/lib 134 | Glog/Lib) 135 | 136 | # Search supplied hint directories first if supplied. 137 | find_path(GLOG_INCLUDE_DIR 138 | NAMES glog/logging.h 139 | PATHS ${GLOG_INCLUDE_DIR_HINTS} 140 | ${GLOG_CHECK_INCLUDE_DIRS} 141 | PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) 142 | if (NOT GLOG_INCLUDE_DIR OR 143 | NOT EXISTS ${GLOG_INCLUDE_DIR}) 144 | glog_report_not_found( 145 | "Could not find glog include directory, set GLOG_INCLUDE_DIR " 146 | "to directory containing glog/logging.h") 147 | endif (NOT GLOG_INCLUDE_DIR OR 148 | NOT EXISTS ${GLOG_INCLUDE_DIR}) 149 | 150 | find_library(GLOG_LIBRARY NAMES glog 151 | PATHS ${GLOG_LIBRARY_DIR_HINTS} 152 | ${GLOG_CHECK_LIBRARY_DIRS} 153 | PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) 154 | if (NOT GLOG_LIBRARY OR 155 | NOT EXISTS ${GLOG_LIBRARY}) 156 | glog_report_not_found( 157 | "Could not find glog library, set GLOG_LIBRARY " 158 | "to full path to libglog.") 159 | endif (NOT GLOG_LIBRARY OR 160 | NOT EXISTS ${GLOG_LIBRARY}) 161 | 162 | # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets 163 | # if called. 164 | set(GLOG_FOUND TRUE) 165 | 166 | # Glog does not seem to provide any record of the version in its 167 | # source tree, thus cannot extract version. 168 | 169 | # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and 170 | # thus FIND_[PATH/LIBRARY] are not called, but specified locations are 171 | # invalid, otherwise we would report the library as found. 172 | if (GLOG_INCLUDE_DIR AND 173 | NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) 174 | glog_report_not_found( 175 | "Caller defined GLOG_INCLUDE_DIR:" 176 | " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") 177 | endif (GLOG_INCLUDE_DIR AND 178 | NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) 179 | # TODO: This regex for glog library is pretty primitive, we use lowercase 180 | # for comparison to handle Windows using CamelCase library names, could 181 | # this check be better? 182 | string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) 183 | if (GLOG_LIBRARY AND 184 | NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") 185 | glog_report_not_found( 186 | "Caller defined GLOG_LIBRARY: " 187 | "${GLOG_LIBRARY} does not match glog.") 188 | endif (GLOG_LIBRARY AND 189 | NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") 190 | 191 | # Set standard CMake FindPackage variables if found. 192 | if (GLOG_FOUND) 193 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 194 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 195 | endif (GLOG_FOUND) 196 | 197 | glog_reset_find_library_prefix() 198 | 199 | # Handle REQUIRED / QUIET optional arguments. 200 | include(FindPackageHandleStandardArgs) 201 | find_package_handle_standard_args(Glog DEFAULT_MSG 202 | GLOG_INCLUDE_DIRS GLOG_LIBRARIES) 203 | 204 | # Only mark internal variables as advanced if we found glog, otherwise 205 | # leave them visible in the standard GUI for the user to set manually. 206 | if (GLOG_FOUND) 207 | mark_as_advanced(FORCE GLOG_INCLUDE_DIR 208 | GLOG_LIBRARY) 209 | endif (GLOG_FOUND) 210 | -------------------------------------------------------------------------------- /mad-icp/cmake/packages.cmake: -------------------------------------------------------------------------------- 1 | # 引入该目录下的.cmake文件 2 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 3 | 4 | include_directories(${CMAKE_SOURCE_DIR}/../devel/include) # 引用ros生成的msg header 5 | 6 | 7 | # find_package(OpenMP QUIET) 8 | find_package(OpenMP REQUIRED) 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 10 | # set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 11 | 12 | # eigen 3 13 | find_package(Eigen3 REQUIRED) 14 | include_directories(${EIGEN3_INCLUDE_DIRS}) 15 | 16 | # glog 17 | find_package(Glog REQUIRED) 18 | include_directories(${Glog_INCLUDE_DIRS}) 19 | 20 | # csparse 21 | find_package(CSparse REQUIRED) 22 | include_directories(${CSPARSE_INCLUDE_DIR}) 23 | 24 | # cholmod 25 | find_package(Cholmod REQUIRED) 26 | include_directories(${CHOLMOD_INCLUDE_DIRS}) 27 | 28 | # pcl 29 | find_package(PCL REQUIRED) 30 | include_directories(${PCL_INCLUDE_DIRS}) 31 | link_directories(${PCL_LIBRARY_DIRS}) 32 | 33 | # ros 34 | # for pointcloud2 35 | find_package(catkin REQUIRED COMPONENTS 36 | roscpp 37 | rospy 38 | std_msgs 39 | sensor_msgs 40 | pcl_ros 41 | pcl_conversions 42 | ) 43 | include_directories(${catkin_INCLUDE_DIRS}) 44 | 45 | # yaml-cpp 46 | find_package(yaml-cpp REQUIRED) 47 | include_directories(${yaml-cpp_INCLUDE_DIRS}) 48 | 49 | set(third_party_libs 50 | ${catkin_LIBRARIES} 51 | ${PCL_LIBRARIES} 52 | glog gflags 53 | ${yaml-cpp_LIBRARIES} 54 | yaml-cpp 55 | ) -------------------------------------------------------------------------------- /mad-icp/config/mapping_bag.yaml: -------------------------------------------------------------------------------- 1 | YAML: 1.0 2 | 3 | common: 4 | lid_topic: /os_cloud_node/points # 5 | gnorm: 1 # 9.805 # 部分数据，加速度模长为1->9.805 6 | 7 | preprocess: 8 | point_filter_num: 1 9 | lidar_type: 3 # 1-AVIA 2-velodyne 3-ouster 4-robosense 5-pandar 10 | blind: 0.1 11 | min_range: 0.7 12 | max_range: 150 13 | sensor_hz: 10 14 | deskew: False 15 | 16 | bag_path: /media/cc/2e27f3b2-979d-44a9-8362-43c922a6581c/lost+found/3D_OpenData/slamesh/scout_podium_ouster_2022-12-13-15-22-59.bag 17 | 18 | mapping: 19 | extrinsic_T: [0, 0, 0] 20 | extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] 21 | 22 | mad_icp: 23 | b_max: 0.2 # [m] max size of kd leaves 24 | b_min: 0.1 # [m] when a node is flatten than this param, propagate normal 25 | b_ratio: 0.02 # the increase factor of search radius needed in data association 26 | p_th: 0.8 # [%] ensuring an update when the curr point cloud is registered less than this param 27 | rho_ker: 0.1 # huber threshold in mad-icp 28 | n: 10 # the number of last poses to smooth velocity 29 | -------------------------------------------------------------------------------- /mad-icp/launch/run_mad.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /mad-icp/launch/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Odometry1/Odometry1 9 | - /Odometry1/Odometry1/Shape1 10 | Splitter Ratio: 0.520588219165802 11 | Tree Height: 903 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: full 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 1 39 | Cell Size: 5 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: false 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: false 56 | - Class: rviz/Axes 57 | Enabled: true 58 | Length: 1 59 | Name: Axes 60 | Radius: 0.10000000149011612 61 | Reference Frame: base_link 62 | Value: true 63 | - Class: rviz/Group 64 | Displays: 65 | - Angle Tolerance: 0.009999999776482582 66 | Class: rviz/Odometry 67 | Covariance: 68 | Orientation: 69 | Alpha: 0.5 70 | Color: 255; 255; 127 71 | Color Style: Unique 72 | Frame: Local 73 | Offset: 1 74 | Scale: 1 75 | Value: true 76 | Position: 77 | Alpha: 0.30000001192092896 78 | Color: 204; 51; 204 79 | Scale: 1 80 | Value: true 81 | Value: true 82 | Enabled: true 83 | Keep: 100 84 | Name: Odometry 85 | Position Tolerance: 0.009999999776482582 86 | Shape: 87 | Alpha: 1 88 | Axes Length: 0.20000000298023224 89 | Axes Radius: 0.009999999776482582 90 | Color: 255; 25; 0 91 | Head Length: 0.029999999329447746 92 | Head Radius: 0.10000000149011612 93 | Shaft Length: 0.10000000149011612 94 | Shaft Radius: 0.029999999329447746 95 | Value: Axes 96 | Topic: /odom 97 | Unreliable: false 98 | Value: true 99 | Enabled: true 100 | Name: Odometry 101 | - Class: rviz/Axes 102 | Enabled: true 103 | Length: 1 104 | Name: Axes 105 | Radius: 0.30000001192092896 106 | Reference Frame: map 107 | Value: true 108 | - Alpha: 0 109 | Buffer Length: 2 110 | Class: rviz/Path 111 | Color: 25; 255; 255 112 | Enabled: true 113 | Head Diameter: 0.30000001192092896 114 | Head Length: 0.20000000298023224 115 | Length: 0.30000001192092896 116 | Line Style: Lines 117 | Line Width: 0.20000000298023224 118 | Name: Path 119 | Offset: 120 | X: 0 121 | Y: 0 122 | Z: 0 123 | Pose Color: 25; 255; 0 124 | Pose Style: None 125 | Radius: 0.029999999329447746 126 | Shaft Diameter: 0.10000000149011612 127 | Shaft Length: 0.10000000149011612 128 | Topic: /odometry_path 129 | Unreliable: false 130 | Value: true 131 | - Alpha: 0.10000000149011612 132 | Autocompute Intensity Bounds: false 133 | Autocompute Value Bounds: 134 | Max Value: 19.64900016784668 135 | Min Value: -0.919323205947876 136 | Value: true 137 | Axis: Z 138 | Channel Name: intensity 139 | Class: rviz/PointCloud2 140 | Color: 255; 255; 255 141 | Color Transformer: Intensity 142 | Decay Time: 300 143 | Enabled: true 144 | Invert Rainbow: false 145 | Max Color: 255; 255; 255 146 | Max Intensity: 100 147 | Min Color: 0; 0; 0 148 | Min Intensity: 0 149 | Name: full 150 | Position Transformer: XYZ 151 | Queue Size: 10 152 | Selectable: true 153 | Size (Pixels): 1 154 | Size (m): 0.009999999776482582 155 | Style: Flat Squares 156 | Topic: /current_cloud 157 | Unreliable: false 158 | Use Fixed Frame: true 159 | Use rainbow: true 160 | Value: true 161 | - Alpha: 1 162 | Autocompute Intensity Bounds: true 163 | Autocompute Value Bounds: 164 | Max Value: 10 165 | Min Value: -10 166 | Value: true 167 | Axis: Z 168 | Channel Name: intensity 169 | Class: rviz/PointCloud2 170 | Color: 255; 255; 255 171 | Color Transformer: Intensity 172 | Decay Time: 0 173 | Enabled: true 174 | Invert Rainbow: false 175 | Max Color: 255; 255; 255 176 | Min Color: 0; 0; 0 177 | Name: current_cloud 178 | Position Transformer: XYZ 179 | Queue Size: 10 180 | Selectable: true 181 | Size (Pixels): 2 182 | Size (m): 0.009999999776482582 183 | Style: Points 184 | Topic: /current_cloud 185 | Unreliable: false 186 | Use Fixed Frame: true 187 | Use rainbow: true 188 | Value: true 189 | Enabled: true 190 | Global Options: 191 | Background Color: 0; 0; 0 192 | Default Light: true 193 | Fixed Frame: map 194 | Frame Rate: 30 195 | Name: root 196 | Tools: 197 | - Class: rviz/Interact 198 | Hide Inactive Objects: true 199 | - Class: rviz/MoveCamera 200 | - Class: rviz/Select 201 | - Class: rviz/FocusCamera 202 | - Class: rviz/Measure 203 | - Class: rviz/SetInitialPose 204 | Theta std deviation: 0.2617993950843811 205 | Topic: /initialpose 206 | X std deviation: 0.5 207 | Y std deviation: 0.5 208 | - Class: rviz/SetGoal 209 | Topic: /move_base_simple/goal 210 | - Class: rviz/PublishPoint 211 | Single click: true 212 | Topic: /clicked_point 213 | Value: true 214 | Views: 215 | Current: 216 | Class: rviz/Orbit 217 | Distance: 38.74952697753906 218 | Enable Stereo Rendering: 219 | Stereo Eye Separation: 0.05999999865889549 220 | Stereo Focal Distance: 1 221 | Swap Stereo Eyes: false 222 | Value: false 223 | Focal Point: 224 | X: 12.213801383972168 225 | Y: 0.5084913372993469 226 | Z: 1.8638496398925781 227 | Focal Shape Fixed Size: true 228 | Focal Shape Size: 0.05000000074505806 229 | Invert Z Axis: false 230 | Name: Current View 231 | Near Clip Distance: 0.009999999776482582 232 | Pitch: 1.5697963237762451 233 | Target Frame: aft_mapped 234 | Value: Orbit (rviz) 235 | Yaw: 1.386191487312317 236 | Saved: ~ 237 | Window Geometry: 238 | Displays: 239 | collapsed: false 240 | Height: 1142 241 | Hide Left Dock: false 242 | Hide Right Dock: true 243 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003c4fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002650000019c0000000000000000fb0000000a0049006d00610067006501000002730000027f0000000000000000fb0000000a0049006d00610067006501000003d2000000d3000000000000000000000001000001520000036efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000036e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000068c00000052fc0100000002fb0000000800540069006d006501000000000000068c000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000530000003c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 244 | Selection: 245 | collapsed: false 246 | Time: 247 | collapsed: false 248 | Tool Properties: 249 | collapsed: false 250 | Views: 251 | collapsed: true 252 | Width: 1676 253 | X: 87 254 | Y: 129 255 | -------------------------------------------------------------------------------- /mad-icp/log/1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/1 -------------------------------------------------------------------------------- /mad-icp/log/10: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/10 -------------------------------------------------------------------------------- /mad-icp/log/11: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/11 -------------------------------------------------------------------------------- /mad-icp/log/12: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/12 -------------------------------------------------------------------------------- /mad-icp/log/13: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/13 -------------------------------------------------------------------------------- /mad-icp/log/14: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/14 -------------------------------------------------------------------------------- /mad-icp/log/15: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/15 -------------------------------------------------------------------------------- /mad-icp/log/16: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/16 -------------------------------------------------------------------------------- /mad-icp/log/17: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/17 -------------------------------------------------------------------------------- /mad-icp/log/18: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/18 -------------------------------------------------------------------------------- /mad-icp/log/19: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/19 -------------------------------------------------------------------------------- /mad-icp/log/2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/2 -------------------------------------------------------------------------------- /mad-icp/log/20: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/20 -------------------------------------------------------------------------------- /mad-icp/log/21: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/21 -------------------------------------------------------------------------------- /mad-icp/log/22: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/22 -------------------------------------------------------------------------------- /mad-icp/log/23: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/23 -------------------------------------------------------------------------------- /mad-icp/log/24: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/24 -------------------------------------------------------------------------------- /mad-icp/log/25: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/25 -------------------------------------------------------------------------------- /mad-icp/log/26: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/26 -------------------------------------------------------------------------------- /mad-icp/log/27: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/27 -------------------------------------------------------------------------------- /mad-icp/log/28: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/28 -------------------------------------------------------------------------------- /mad-icp/log/29: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/29 -------------------------------------------------------------------------------- /mad-icp/log/3: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/3 -------------------------------------------------------------------------------- /mad-icp/log/30: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/30 -------------------------------------------------------------------------------- /mad-icp/log/31: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/31 -------------------------------------------------------------------------------- /mad-icp/log/32: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/32 -------------------------------------------------------------------------------- /mad-icp/log/33: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/33 -------------------------------------------------------------------------------- /mad-icp/log/34: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/34 -------------------------------------------------------------------------------- /mad-icp/log/35: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/35 -------------------------------------------------------------------------------- /mad-icp/log/36: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/36 -------------------------------------------------------------------------------- /mad-icp/log/37: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/37 -------------------------------------------------------------------------------- /mad-icp/log/38: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/38 -------------------------------------------------------------------------------- /mad-icp/log/39: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/39 -------------------------------------------------------------------------------- /mad-icp/log/4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/4 -------------------------------------------------------------------------------- /mad-icp/log/40: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/40 -------------------------------------------------------------------------------- /mad-icp/log/41: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/41 -------------------------------------------------------------------------------- /mad-icp/log/42: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/42 -------------------------------------------------------------------------------- /mad-icp/log/43: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/43 -------------------------------------------------------------------------------- /mad-icp/log/44: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/44 -------------------------------------------------------------------------------- /mad-icp/log/45: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/45 -------------------------------------------------------------------------------- /mad-icp/log/46: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/46 -------------------------------------------------------------------------------- /mad-icp/log/47: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/47 -------------------------------------------------------------------------------- /mad-icp/log/48: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/48 -------------------------------------------------------------------------------- /mad-icp/log/49: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/49 -------------------------------------------------------------------------------- /mad-icp/log/5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/5 -------------------------------------------------------------------------------- /mad-icp/log/50: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/50 -------------------------------------------------------------------------------- /mad-icp/log/51: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/51 -------------------------------------------------------------------------------- /mad-icp/log/52: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/52 -------------------------------------------------------------------------------- /mad-icp/log/53: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/53 -------------------------------------------------------------------------------- /mad-icp/log/54: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/54 -------------------------------------------------------------------------------- /mad-icp/log/55: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/55 -------------------------------------------------------------------------------- /mad-icp/log/56: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/56 -------------------------------------------------------------------------------- /mad-icp/log/57: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/57 -------------------------------------------------------------------------------- /mad-icp/log/58: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/58 -------------------------------------------------------------------------------- /mad-icp/log/59: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/59 -------------------------------------------------------------------------------- /mad-icp/log/6: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/6 -------------------------------------------------------------------------------- /mad-icp/log/60: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/60 -------------------------------------------------------------------------------- /mad-icp/log/61: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/61 -------------------------------------------------------------------------------- /mad-icp/log/62: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/62 -------------------------------------------------------------------------------- /mad-icp/log/63: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/63 -------------------------------------------------------------------------------- /mad-icp/log/64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/64 -------------------------------------------------------------------------------- /mad-icp/log/65: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/65 -------------------------------------------------------------------------------- /mad-icp/log/66: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/66 -------------------------------------------------------------------------------- /mad-icp/log/67: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/67 -------------------------------------------------------------------------------- /mad-icp/log/68: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/68 -------------------------------------------------------------------------------- /mad-icp/log/69: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/69 -------------------------------------------------------------------------------- /mad-icp/log/7: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/7 -------------------------------------------------------------------------------- /mad-icp/log/70: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/70 -------------------------------------------------------------------------------- /mad-icp/log/71: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/71 -------------------------------------------------------------------------------- /mad-icp/log/72: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/72 -------------------------------------------------------------------------------- /mad-icp/log/73: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/73 -------------------------------------------------------------------------------- /mad-icp/log/74: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/74 -------------------------------------------------------------------------------- /mad-icp/log/75: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/75 -------------------------------------------------------------------------------- /mad-icp/log/76: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/76 -------------------------------------------------------------------------------- /mad-icp/log/77: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/77 -------------------------------------------------------------------------------- /mad-icp/log/78: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/78 -------------------------------------------------------------------------------- /mad-icp/log/79: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/79 -------------------------------------------------------------------------------- /mad-icp/log/8: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/8 -------------------------------------------------------------------------------- /mad-icp/log/80: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/80 -------------------------------------------------------------------------------- /mad-icp/log/81: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/81 -------------------------------------------------------------------------------- /mad-icp/log/82: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/82 -------------------------------------------------------------------------------- /mad-icp/log/83: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/83 -------------------------------------------------------------------------------- /mad-icp/log/84: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/84 -------------------------------------------------------------------------------- /mad-icp/log/85: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/85 -------------------------------------------------------------------------------- /mad-icp/log/86: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/86 -------------------------------------------------------------------------------- /mad-icp/log/87: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/87 -------------------------------------------------------------------------------- /mad-icp/log/88: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/88 -------------------------------------------------------------------------------- /mad-icp/log/89: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/89 -------------------------------------------------------------------------------- /mad-icp/log/9: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/9 -------------------------------------------------------------------------------- /mad-icp/log/90: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/90 -------------------------------------------------------------------------------- /mad-icp/log/91: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/91 -------------------------------------------------------------------------------- /mad-icp/log/92: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/92 -------------------------------------------------------------------------------- /mad-icp/log/93: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/93 -------------------------------------------------------------------------------- /mad-icp/log/94: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/94 -------------------------------------------------------------------------------- /mad-icp/log/95: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/95 -------------------------------------------------------------------------------- /mad-icp/log/96: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/96 -------------------------------------------------------------------------------- /mad-icp/log/97: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/97 -------------------------------------------------------------------------------- /mad-icp/log/98: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/98 -------------------------------------------------------------------------------- /mad-icp/log/99: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/mad-icp/log/99 -------------------------------------------------------------------------------- /mad-icp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mad_icp 4 | 0.0.0 5 | 6 | 7 | This work is created based on some optimization on ALOAM by HKUST and LOAM by CMU 8 | 9 | 10 | chengwei 11 | 12 | BSD 13 | https:* 14 | chengwei 15 | 16 | catkin 17 | geometry_msgs 18 | nav_msgs 19 | roscpp 20 | rospy 21 | std_msgs 22 | rosbag 23 | sensor_msgs 24 | tf 25 | eigen_conversions 26 | message_generation 27 | message_runtime 28 | visualization_msgs 29 | visualization_msgs 30 | message_generation 31 | message_runtime 32 | geometry_msgs 33 | nav_msgs 34 | sensor_msgs 35 | roscpp 36 | rospy 37 | std_msgs 38 | rosbag 39 | tf 40 | eigen_conversions 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /mad-icp/src/apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(${PROJECT_NAME} mad_icp.cpp ) 2 | target_link_libraries(${PROJECT_NAME} 3 | ${PROJECT_NAME}.common 4 | ${PROJECT_NAME}.pre 5 | tools 6 | odometry 7 | ${third_party_libs} 8 | ) 9 | 10 | add_executable(bin_runner bin_runner.cpp) 11 | target_link_libraries(bin_runner 12 | odometry 13 | yaml-cpp 14 | ) 15 | -------------------------------------------------------------------------------- /mad-icp/src/apps/bin_runner.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "cpp_utils/parse_cmd_line.h" 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | std::string data_path; 45 | std::string estimate_path; 46 | std::string dataset_config; 47 | std::string mad_icp_config = "../../../configurations/params.cfg"; 48 | int num_cores = 4; 49 | int num_keyframes = 4; 50 | bool realtime = false; 51 | bool kitti = false; 52 | 53 | // for kitti scan correction (not documented) 54 | constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0; 55 | 56 | // parsing cmd line stuff 57 | void printHelp(); 58 | void parseCmdLine(int argc, char *argv[]); 59 | Eigen::Matrix4d parseMatrix(const std::vector> &vec); 60 | 61 | // write homogenous transformation in base frame 62 | void writeTransformedPose(std::ofstream &estimate_file_buf, 63 | const Eigen::Matrix4d &lidar_to_world, 64 | const Eigen::Matrix4d &lidar_to_base); 65 | 66 | int main(int argc, char *argv[]) 67 | { 68 | parseCmdLine(argc, argv); 69 | // srand(time(NULL)); 70 | YAML::Node yaml_dataset_config, yaml_mad_icp_config; 71 | yaml_dataset_config = YAML::LoadFile(dataset_config); 72 | yaml_mad_icp_config = YAML::LoadFile(mad_icp_config); 73 | 74 | // parse data specific configurations 75 | const double min_range = yaml_dataset_config["min_range"].as(); 76 | const double max_range = yaml_dataset_config["max_range"].as(); 77 | const double sensor_hz = yaml_dataset_config["sensor_hz"].as(); 78 | const bool deskew = yaml_dataset_config["deskew"].as(); 79 | // parsing lidar in base homogenous transformation 80 | const auto lidar_to_base_vec = yaml_dataset_config["lidar_to_base"].as>>(); 81 | const Eigen::Matrix4d lidar_to_base = parseMatrix(lidar_to_base_vec); 82 | 83 | // parse mad-icp configuration 84 | const double b_max = yaml_mad_icp_config["b_max"].as(); 85 | const double b_min = yaml_mad_icp_config["b_min"].as(); 86 | const double b_ratio = yaml_mad_icp_config["b_ratio"].as(); 87 | const double p_th = yaml_mad_icp_config["p_th"].as(); 88 | const double rho_ker = yaml_mad_icp_config["rho_ker"].as(); 89 | const int n = yaml_mad_icp_config["n"].as(); 90 | 91 | // check some params for machine 92 | if (realtime) 93 | if (num_keyframes > num_cores) 94 | { 95 | throw std::runtime_error("if realtime num_keyframes needs to be <= num_cores"); 96 | } 97 | 98 | // parsing bin filenames in order 99 | std::vector files_in_directory; 100 | std::copy(std::filesystem::directory_iterator(data_path), 101 | std::filesystem::directory_iterator(), 102 | std::back_inserter(files_in_directory)); 103 | std::sort(files_in_directory.begin(), files_in_directory.end()); 104 | 105 | struct timeval t_start, t_end, t_delta; 106 | 107 | std::unique_ptr pipeline = 108 | std::make_unique(sensor_hz, deskew, b_max, rho_ker, p_th, b_min, b_ratio, num_keyframes, num_cores, realtime); 109 | 110 | double time = 0.; 111 | const double time_incr = 1. / sensor_hz; 112 | 113 | std::ofstream os; 114 | std::string estimate_file = estimate_path + "/estimate.txt"; 115 | os.open(estimate_file); 116 | os << std::fixed << std::setprecision(12); 117 | 118 | ContainerType cloud; 119 | Eigen::Matrix4d lidar_to_world; 120 | 121 | for (const std::string &filename : files_in_directory) 122 | { 123 | std::cout << "Loading frame # " << pipeline->currentID() << std::endl; 124 | 125 | // load bin point cloud - start 126 | // allocate 4 MB buffer (only ~130*4*4 KB are needed) 127 | gettimeofday(&t_start, nullptr); 128 | int32_t num = 1000000; 129 | float *data = (float *)malloc(num * sizeof(float)); 130 | 131 | // pointers 132 | float *px = data + 0; 133 | float *py = data + 1; 134 | float *pz = data + 2; 135 | float *pr = data + 3; 136 | 137 | cloud.clear(); 138 | cloud.reserve(num); 139 | 140 | // load bin point cloud 141 | FILE *stream; 142 | stream = fopen(filename.c_str(), "rb"); 143 | num = fread(data, sizeof(float), num, stream) / 4; 144 | for (int32_t i = 0; i < num; i++) 145 | { 146 | Eigen::Vector3f point(*px, *py, *pz); 147 | px += 4; 148 | py += 4; 149 | pz += 4; 150 | pr += 4; 151 | 152 | if (point.norm() < min_range || point.norm() > max_range || std::isnan(point.x()) || std::isnan(point.y()) || 153 | std::isnan(point.z())) 154 | continue; 155 | 156 | // apply kitti magic correction (not documented) 157 | const Eigen::Vector3d pointd = point.cast(); 158 | if (kitti) 159 | { 160 | const Eigen::Vector3d rotation_vector = pointd.cross(Eigen::Vector3d(0., 0., 1.)); 161 | const Eigen::Vector3d corrected_point = Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotation_vector.normalized()) * pointd; 162 | cloud.push_back(corrected_point); 163 | } 164 | else 165 | { 166 | cloud.push_back(pointd); 167 | } 168 | } 169 | 170 | cloud.shrink_to_fit(); 171 | fclose(stream); 172 | free(data); 173 | gettimeofday(&t_end, nullptr); 174 | timersub(&t_end, &t_start, &t_delta); 175 | std::cout << std::fixed << std::setprecision(4) 176 | << "Time for reading points [ms]: " << double(t_delta.tv_sec) * 1000. + 1e-3 * t_delta.tv_usec << std::endl; 177 | // load bin point cloud - end 178 | 179 | gettimeofday(&t_start, nullptr); 180 | pipeline->compute(time, cloud); 181 | gettimeofday(&t_end, nullptr); 182 | timersub(&t_end, &t_start, &t_delta); 183 | std::cout << std::fixed << std::setprecision(4) 184 | << "Time for odometry estimation [ms]: " << double(t_delta.tv_sec) * 1000. + 1e-3 * t_delta.tv_usec << std::endl; 185 | 186 | lidar_to_world = pipeline->currentPose(); 187 | 188 | time += time_incr; 189 | 190 | // dump trajectory to txt file, no visualization in C++ 191 | // use Python for viz 192 | writeTransformedPose(os, lidar_to_world, lidar_to_base); 193 | std::cout << std::endl; 194 | } 195 | 196 | os.close(); 197 | return 0; 198 | } 199 | 200 | void printHelp() 201 | { 202 | // clang-format off 203 | std::cout << "mad-icp runner for bin (KITTI, Mulran format)\n"; 204 | std::cout << "bin_runner -data_path -estimate_path -dataset_config \n"; 205 | std::cout << " -data_path [string] path containing multiple bin (folder path)\n"; 206 | std::cout << " -estimate_path [string] trajectory estimate output path (folder path)\n"; 207 | std::cout << " -dataset_config [string] dataset configuration file\n"; 208 | std::cout << " -mad_icp_config [string] parameters for mad icp\n"; 209 | std::cout << " -num_cores [int] how many threads to use for icp (suggest maximum num)\n"; 210 | std::cout << " -num_keyframes [int] max number of kf kept in the local map\n"; 211 | std::cout << " -realtime [flag] if true anytime realtime\n"; 212 | std::cout << " -kitti [flag] if kitti, applies kitti correction\n"; 213 | std::cout << " -h display this help and exit\n"; 214 | // clang-format on 215 | } 216 | 217 | void parseCmdLine(int argc, char *argv[]) 218 | { 219 | InputParser input(argc, argv); 220 | // mandatory args 221 | if (!input.cmdOptionExists("-data_path") || !input.cmdOptionExists("-estimate_path") || 222 | !input.cmdOptionExists("-dataset_config")) 223 | { 224 | printHelp(); 225 | exit(0); 226 | } 227 | 228 | data_path = input.getCmdOption("-data_path"); 229 | estimate_path = input.getCmdOption("-estimate_path"); 230 | dataset_config = input.getCmdOption("-dataset_config"); 231 | 232 | // default ok! 233 | if (input.cmdOptionExists("-mad_icp_config")) 234 | mad_icp_config = input.getCmdOption("-mad_icp_config"); 235 | if (input.cmdOptionExists("-num_cores")) 236 | num_cores = input.getInt("-num_cores"); 237 | if (input.cmdOptionExists("-num_keyframes")) 238 | num_keyframes = input.getInt("-num_keyframes"); 239 | if (input.cmdOptionExists("-realtime")) 240 | realtime = true; 241 | if (input.cmdOptionExists("-kitti")) 242 | kitti = true; 243 | 244 | // help message 245 | if (input.cmdOptionExists("-h")) 246 | { 247 | printHelp(); 248 | exit(0); 249 | } 250 | } 251 | 252 | Eigen::Matrix4d parseMatrix(const std::vector> &vec) 253 | { 254 | // this need to be done to respect config file 255 | std::vector mat_vec; 256 | for (int r = 0; r < 4; ++r) 257 | { 258 | for (int c = 0; c < 4; ++c) 259 | { 260 | mat_vec.push_back(vec[r][c]); 261 | } 262 | } 263 | return Eigen::Map>(mat_vec.data()); 264 | } 265 | 266 | void writeTransformedPose(std::ofstream &estimate_file_buf, 267 | const Eigen::Matrix4d &lidar_to_world, 268 | const Eigen::Matrix4d &lidar_to_base) 269 | { 270 | const Eigen::Matrix4d base_to_world = lidar_to_base * lidar_to_world * lidar_to_base.inverse(); 271 | for (int row = 0; row < 3; ++row) 272 | { 273 | for (int col = 0; col < 4; ++col) 274 | { 275 | estimate_file_buf << base_to_world(row, col) << " "; 276 | } 277 | } 278 | estimate_file_buf << std::endl; 279 | } -------------------------------------------------------------------------------- /mad-icp/src/apps/cpp_utils/parse_cmd_line.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | class InputParser { 7 | public: 8 | InputParser(int& argc, char** argv) { 9 | for (int i = 1; i < argc; ++i) 10 | this->tokens_.push_back(std::string(argv[i])); 11 | } 12 | 13 | inline const std::string& getCmdOption(const std::string& option) const { 14 | std::vector::const_iterator itr; 15 | itr = std::find(this->tokens_.begin(), this->tokens_.end(), option); 16 | if (itr != this->tokens_.end() && ++itr != this->tokens_.end()) { 17 | return *itr; 18 | } 19 | static const std::string empty_string(""); 20 | return empty_string; 21 | } 22 | 23 | inline int getInt(const std::string& option) const { 24 | std::vector::const_iterator itr; 25 | itr = std::find(this->tokens_.begin(), this->tokens_.end(), option); 26 | if (itr != this->tokens_.end() && ++itr != this->tokens_.end()) { 27 | return std::stoi(*itr); 28 | } 29 | return 0; 30 | } 31 | 32 | inline float getFloat(const std::string& option) const { 33 | std::vector::const_iterator itr; 34 | itr = std::find(this->tokens_.begin(), this->tokens_.end(), option); 35 | if (itr != this->tokens_.end() && ++itr != this->tokens_.end()) { 36 | return std::stof(*itr); 37 | } 38 | return 0.f; 39 | } 40 | 41 | bool cmdOptionExists(const std::string& option) const { 42 | return std::find(this->tokens_.begin(), this->tokens_.end(), option) != this->tokens_.end(); 43 | } 44 | 45 | private: 46 | std::vector tokens_; 47 | }; 48 | -------------------------------------------------------------------------------- /mad-icp/src/apps/mad_icp.cpp: -------------------------------------------------------------------------------- 1 | // c++ lib 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | // ros lib 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include "preprocess/cloud_convert2/cloud_convert2.h" 28 | 29 | // mad_icp 30 | #include 31 | #include "mad_icp/odometry/pipeline.h" 32 | #include "mad_icp/tools/mad_tree.h" 33 | 34 | #include "common/io_utils.h" 35 | 36 | nav_msgs::Path laserOdoPath; 37 | 38 | DEFINE_string(config_directory, "/home/cc/catkin_context/src/mad_icp/config", "配置文件目录"); 39 | #define DEBUG_FILE_DIR(name) (std::string(std::string(ROOT_DIR) + "log/" + name)) 40 | 41 | mad_icp::CloudConvert *convert; 42 | std::unique_ptr lio = nullptr; 43 | std::string config_file; 44 | 45 | std::deque>> data_buffer_; 46 | 47 | ros::Publisher pubLaserOdometry, pubLaserOdometryPath; 48 | ros::Publisher pub_scan; 49 | 50 | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) 51 | { 52 | 53 | std::vector cloud_out; 54 | std::vector ts_out; 55 | zjloc::common::Timer::Evaluate([&]() 56 | { convert->Process(msg, cloud_out); }, 57 | "laser convert"); 58 | cloud_out.shrink_to_fit(); 59 | double cloud_stamp = msg->header.stamp.toSec(); 60 | data_buffer_.push_back(std::make_pair(cloud_stamp, cloud_out)); 61 | } 62 | 63 | void process() 64 | { 65 | if (!data_buffer_.empty()) 66 | { 67 | auto data = data_buffer_.front(); 68 | double stamp = data.first; 69 | std::cout << "Loading frame # " << lio->currentID() << std::endl; 70 | zjloc::common::Timer::Evaluate([&]() 71 | { lio->compute(stamp, data.second); }, 72 | "process measure"); 73 | 74 | Eigen::Matrix lidar_to_world = lio->currentPose(); 75 | data_buffer_.pop_front(); 76 | 77 | { 78 | 79 | zjloc::CloudPtr trans_cloud(new zjloc::PointCloudType()); 80 | Eigen::Isometry3d T = Eigen::Isometry3d(lidar_to_world); 81 | { 82 | zjloc::PointType point; 83 | for (auto pt : data.second) 84 | { 85 | Eigen::Vector3d pt_w = T * pt; 86 | point.x = pt_w[0], point.y = pt_w[1], point.z = pt_w[2]; 87 | trans_cloud->push_back(point); 88 | } 89 | sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2()); 90 | pcl::toROSMsg(*trans_cloud, *cloud_ptr_output); 91 | 92 | cloud_ptr_output->header.stamp = ros::Time().fromSec(stamp); 93 | cloud_ptr_output->header.frame_id = "map"; 94 | pub_scan.publish(*cloud_ptr_output); 95 | } 96 | 97 | static tf::TransformBroadcaster br; 98 | tf::Transform transform; 99 | Eigen::Quaterniond q_current(T.rotation()); 100 | Eigen::Vector3d pose = T.translation(); 101 | transform.setOrigin(tf::Vector3(pose[0], pose[1], pose[2])); 102 | tf::Quaternion q(q_current.x(), q_current.y(), q_current.z(), q_current.w()); 103 | transform.setRotation(q); 104 | 105 | br.sendTransform(tf::StampedTransform(transform, ros::Time().fromSec(stamp), "map", "base_link")); 106 | 107 | // publish odometry 108 | nav_msgs::Odometry laserOdometry; 109 | laserOdometry.header.frame_id = "map"; 110 | laserOdometry.child_frame_id = "base_link"; 111 | laserOdometry.header.stamp = ros::Time().fromSec(stamp); 112 | 113 | laserOdometry.pose.pose.orientation.x = q_current.x(); 114 | laserOdometry.pose.pose.orientation.y = q_current.y(); 115 | laserOdometry.pose.pose.orientation.z = q_current.z(); 116 | laserOdometry.pose.pose.orientation.w = q_current.w(); 117 | laserOdometry.pose.pose.position.x = pose[0]; 118 | laserOdometry.pose.pose.position.y = pose[1]; 119 | laserOdometry.pose.pose.position.z = pose[2]; 120 | pubLaserOdometry.publish(laserOdometry); 121 | 122 | // publish path 123 | geometry_msgs::PoseStamped laserPose; 124 | laserPose.header = laserOdometry.header; 125 | laserPose.pose = laserOdometry.pose.pose; 126 | laserOdoPath.header.stamp = laserOdometry.header.stamp; 127 | laserOdoPath.poses.push_back(laserPose); 128 | laserOdoPath.header.frame_id = "/map"; 129 | pubLaserOdometryPath.publish(laserOdoPath); 130 | } 131 | } 132 | } 133 | 134 | int main(int argc, char **argv) 135 | { 136 | google::InitGoogleLogging(argv[0]); 137 | FLAGS_stderrthreshold = google::INFO; 138 | FLAGS_colorlogtostderr = true; 139 | google::ParseCommandLineFlags(&argc, &argv, true); 140 | 141 | ros::init(argc, argv, "main"); 142 | ros::NodeHandle nh; 143 | 144 | std::cout << ANSI_COLOR_GREEN << "config_file:" << FLAGS_config_directory << ANSI_COLOR_RESET << std::endl; 145 | config_file = std::string(FLAGS_config_directory) + "/mapping_bag.yaml"; 146 | std::cout << ANSI_COLOR_GREEN << "config_file:" << config_file << ANSI_COLOR_RESET << std::endl; 147 | 148 | auto yaml = YAML::LoadFile(config_file); 149 | std::string laser_topic = yaml["common"]["lid_topic"].as(); 150 | std::string bag_path_ = yaml["bag_path"].as(); 151 | 152 | double sensor_hz = yaml["preprocess"]["sensor_hz"].as(); 153 | bool deskew = yaml["preprocess"]["deskew"].as(); 154 | double b_max = yaml["mad_icp"]["b_max"].as(); 155 | double rho_ker = yaml["mad_icp"]["rho_ker"].as(); 156 | double p_th = yaml["mad_icp"]["p_th"].as(); 157 | double b_min = yaml["mad_icp"]["b_min"].as(); 158 | double b_ratio = yaml["mad_icp"]["b_ratio"].as(); 159 | int num_keyframes = 4; 160 | int num_cores = 4; 161 | double realtime = true; 162 | 163 | convert = new mad_icp::CloudConvert; 164 | convert->LoadFromYAML(config_file); 165 | std::cout << ANSI_COLOR_GREEN_BOLD << "init successful" << ANSI_COLOR_RESET << std::endl; 166 | 167 | lio = std::make_unique(sensor_hz, deskew, b_max, rho_ker, p_th, 168 | b_min, b_ratio, num_keyframes, num_cores, realtime); 169 | 170 | ros::Subscriber subLaserCloud = nh.subscribe(laser_topic, 100, standard_pcl_cbk); 171 | 172 | pubLaserOdometry = nh.advertise("/odom", 100); 173 | pubLaserOdometryPath = nh.advertise("/odometry_path", 5); 174 | 175 | pub_scan = nh.advertise("current_cloud", 10); 176 | 177 | ros::Rate loop_rate(50); 178 | while (ros::ok()) 179 | { 180 | process(); 181 | ros::spinOnce(); 182 | loop_rate.sleep(); 183 | } 184 | 185 | ros::spin(); 186 | 187 | zjloc::common::Timer::PrintAll(); 188 | zjloc::common::Timer::DumpIntoFile(DEBUG_FILE_DIR("log_time.txt")); 189 | 190 | std::cout << ANSI_COLOR_GREEN_BOLD << " Finish done. " << ANSI_COLOR_RESET << std::endl; 191 | 192 | sleep(3); 193 | return 0; 194 | } -------------------------------------------------------------------------------- /mad-icp/src/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_library(${PROJECT_NAME}.common 3 | timer/timer.cc 4 | io_utils.cc 5 | global_flags.cc 6 | ) 7 | 8 | target_link_libraries(${PROJECT_NAME}.common 9 | ${third_party_libs} 10 | ) 11 | 12 | -------------------------------------------------------------------------------- /mad-icp/src/common/eigen_types.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPPING_EIGEN_TYPES_H 2 | #define MAPPING_EIGEN_TYPES_H 3 | 4 | // 引入Eigen头文件与常用类型 5 | #include 6 | #include 7 | #include 8 | 9 | using Vec2i = Eigen::Vector2i; 10 | using Vec3i = Eigen::Vector3i; 11 | using Vec3b = Eigen::Matrix; 12 | 13 | using Vec2d = Eigen::Vector2d; 14 | using Vec2f = Eigen::Vector2f; 15 | using Vec3d = Eigen::Vector3d; 16 | using Vec3f = Eigen::Vector3f; 17 | using Vec4d = Eigen::Vector4d; 18 | using Vec4f = Eigen::Vector4f; 19 | using Vec5d = Eigen::Matrix; 20 | using Vec5f = Eigen::Matrix; 21 | using Vec6d = Eigen::Matrix; 22 | using Vec6f = Eigen::Matrix; 23 | using Vec9d = Eigen::Matrix; 24 | using Vec15d = Eigen::Matrix; 25 | using Vec18d = Eigen::Matrix; 26 | 27 | using Mat1d = Eigen::Matrix; 28 | using Mat2d = Eigen::Matrix; 29 | using Mat23d = Eigen::Matrix; 30 | using Mat32d = Eigen::Matrix; 31 | using Mat3d = Eigen::Matrix3d; 32 | using Mat3f = Eigen::Matrix3f; 33 | using Mat4d = Eigen::Matrix4d; 34 | using Mat4f = Eigen::Matrix4f; 35 | using Mat5d = Eigen::Matrix; 36 | using Mat5f = Eigen::Matrix; 37 | using Mat6d = Eigen::Matrix; 38 | using Mat6f = Eigen::Matrix; 39 | using Mat9d = Eigen::Matrix; 40 | using Mat96d = Eigen::Matrix; 41 | using Mat12d = Eigen::Matrix; 42 | using Mat15d = Eigen::Matrix; 43 | using Mat18d = Eigen::Matrix; 44 | 45 | using VecXd = Eigen::Matrix; 46 | using MatXd = Eigen::Matrix; 47 | using MatX18d = Eigen::Matrix; 48 | 49 | using Quatd = Eigen::Quaterniond; 50 | using Quatf = Eigen::Quaternionf; 51 | 52 | const Mat3d Eye3d = Mat3d::Identity(); 53 | const Mat3f Eye3f = Mat3f::Identity(); 54 | const Vec3d Zero3d(0, 0, 0); 55 | const Vec3f Zero3f(0, 0, 0); 56 | 57 | using IdType = unsigned long; 58 | 59 | #endif // MAPPING_EIGEN_TYPES_H 60 | -------------------------------------------------------------------------------- /mad-icp/src/common/global_flags.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiang on 2022/7/28. 3 | // 4 | 5 | #include "common/global_flags.h" 6 | 7 | namespace zjloc::global 8 | { 9 | bool FLAG_EXIT = false; 10 | } -------------------------------------------------------------------------------- /mad-icp/src/common/global_flags.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiang on 2022/7/28. 3 | // 4 | 5 | #ifndef SLAM_IN_AUTO_DRIVING_GLOBAL_FLAGS_H 6 | #define SLAM_IN_AUTO_DRIVING_GLOBAL_FLAGS_H 7 | 8 | namespace zjloc::global 9 | { 10 | extern bool FLAG_EXIT; // 退出程序标志 11 | } 12 | 13 | #endif // SLAM_IN_AUTO_DRIVING_GLOBAL_FLAGS_H 14 | -------------------------------------------------------------------------------- /mad-icp/src/common/io_utils.cc: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiang on 2021/7/20. 3 | // 4 | #include "common/io_utils.h" 5 | 6 | #include 7 | 8 | namespace zjloc 9 | { 10 | 11 | void RosbagIO::Go() 12 | { 13 | rosbag::Bag bag(bag_file_); 14 | LOG(INFO) << "running in " << bag_file_ << ", reg process func: " << process_func_.size(); 15 | 16 | if (!bag.isOpen()) 17 | { 18 | LOG(ERROR) << "cannot open " << bag_file_; 19 | return; 20 | } 21 | 22 | // auto view = rosbag::View(bag); 23 | rosbag::View view(bag); 24 | 25 | auto start_real_time = std::chrono::high_resolution_clock::now(); 26 | auto start_sim_time = view.getBeginTime(); 27 | 28 | // auto prev_real_time = start_real_time; 29 | // auto prev_sim_time = start_sim_time; 30 | 31 | for (const rosbag::MessageInstance &m : view) 32 | { 33 | auto iter = process_func_.find(m.getTopic()); 34 | if (iter != process_func_.end()) 35 | { 36 | iter->second(m); 37 | } 38 | 39 | if (global::FLAG_EXIT) 40 | { 41 | break; 42 | } 43 | 44 | // auto real_time = std::chrono::high_resolution_clock::now(); 45 | // if (real_time - prev_real_time > std::chrono::seconds(5)) 46 | // { 47 | // auto sim_time = m.getTime(); 48 | // auto delta_real = std::chrono::duration_cast(real_time - prev_real_time).count() * 0.001; 49 | // auto delta_sim = (sim_time - prev_sim_time).toSec(); 50 | // printf("Processing the rosbag at %.1fX speed.", delta_sim / delta_real); 51 | // prev_sim_time = sim_time; 52 | // prev_real_time = real_time; 53 | // } 54 | } 55 | 56 | bag.close(); 57 | 58 | auto real_time = std::chrono::high_resolution_clock::now(); 59 | auto delta_real = std::chrono::duration_cast(real_time - start_real_time).count() * 0.001; 60 | auto delta_sim = (view.getEndTime() - start_sim_time).toSec(); 61 | printf("Entire rosbag processed at %.1fX speed", delta_sim / delta_real); 62 | 63 | LOG(INFO) << "bag " << bag_file_ << " finished."; 64 | } 65 | 66 | } // namespace zjloc -------------------------------------------------------------------------------- /mad-icp/src/common/io_utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiang on 2021/7/20. 3 | // 4 | 5 | #ifndef SLAM_IN_AUTO_DRIVING_IO_UTILS_H 6 | #define SLAM_IN_AUTO_DRIVING_IO_UTILS_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include "common/message_def.h" 17 | 18 | #include 19 | 20 | namespace zjloc 21 | { 22 | 23 | /** 24 | * ROSBAG IO 25 | * 指定一个包名，添加一些回调函数，就可以顺序遍历这个包 26 | */ 27 | class RosbagIO 28 | { 29 | public: 30 | explicit RosbagIO(std::string bag_file) : bag_file_(std::move(bag_file)) {} 31 | 32 | using MessageProcessFunction = std::function; 33 | 34 | /// 一些方便直接使用的topics, messages 35 | using PointCloud2Handle = std::function; 36 | 37 | // 遍历文件内容，调用回调函数 38 | void Go(); 39 | 40 | /// 通用处理函数 41 | RosbagIO &AddHandle(const std::string &topic_name, MessageProcessFunction func) 42 | { 43 | process_func_.emplace(topic_name, func); 44 | return *this; 45 | } 46 | 47 | /// point cloud2 的处理 48 | RosbagIO &AddPointCloud2Handle(const std::string &topic_name, PointCloud2Handle f) 49 | { 50 | return AddHandle(topic_name, [f](const rosbag::MessageInstance &m) -> bool 51 | { 52 | auto msg = m.instantiate(); 53 | if (msg == nullptr) { 54 | return false; 55 | } 56 | return f(msg); }); 57 | } 58 | 59 | /// 清除现有的处理函数 60 | void CleanProcessFunc() { process_func_.clear(); } 61 | 62 | private: 63 | std::map process_func_; 64 | std::string bag_file_; 65 | }; 66 | 67 | } // namespace zjloc 68 | 69 | #endif // SLAM_IN_AUTO_DRIVING_IO_UTILS_H 70 | -------------------------------------------------------------------------------- /mad-icp/src/common/message_def.h: -------------------------------------------------------------------------------- 1 | #ifndef SLAM_IN_AUTO_DRIVING_MESSAGE_DEF_H 2 | #define SLAM_IN_AUTO_DRIVING_MESSAGE_DEF_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #endif // SLAM_IN_AUTO_DRIVING_MESSAGE_DEF_H 9 | -------------------------------------------------------------------------------- /mad-icp/src/common/point_types.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiang on 2021/8/18. 3 | // 4 | 5 | #ifndef SLAM_IN_AUTO_DRIVING_POINT_TYPES_H 6 | #define SLAM_IN_AUTO_DRIVING_POINT_TYPES_H 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "common/eigen_types.h" 13 | 14 | namespace zjloc 15 | { 16 | 17 | // 定义系统中用到的点和点云类型 18 | using PointType = pcl::PointXYZI; 19 | using PointCloudType = pcl::PointCloud; 20 | using CloudPtr = PointCloudType::Ptr; 21 | using PointVec = std::vector>; 22 | using IndexVec = std::vector; 23 | 24 | // 点云到Eigen的常用的转换函数 25 | inline Vec3f ToVec3f(const PointType &pt) { return pt.getVector3fMap(); } 26 | inline Vec3d ToVec3d(const PointType &pt) { return pt.getVector3fMap().cast(); } 27 | 28 | // 模板类型转换函数 29 | template 30 | inline Eigen::Matrix ToEigen(const PointType &pt); 31 | 32 | template <> 33 | inline Eigen::Matrix ToEigen(const PointType &pt) 34 | { 35 | return Vec2f(pt.x, pt.y); 36 | } 37 | 38 | template <> 39 | inline Eigen::Matrix ToEigen(const PointType &pt) 40 | { 41 | return Vec3f(pt.x, pt.y, pt.z); 42 | } 43 | 44 | template 45 | inline PointType ToPointType(const Eigen::Matrix &pt) 46 | { 47 | PointType p; 48 | p.x = pt.x(); 49 | p.y = pt.y(); 50 | p.z = pt.z(); 51 | return p; 52 | } 53 | 54 | /// 带ring, range等其他信息的全量信息点云 55 | struct FullPointType 56 | { 57 | PCL_ADD_POINT4D; 58 | float intensity; 59 | float range = 0; 60 | float radius = 0; 61 | uint8_t ring = 0; 62 | uint8_t angle = 0; 63 | double time = 0; 64 | float height = 0; 65 | 66 | inline FullPointType() {} 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | }; 69 | 70 | /// 全量点云的定义 71 | using FullPointCloudType = pcl::PointCloud; 72 | using FullCloudPtr = FullPointCloudType::Ptr; 73 | 74 | inline Vec3f ToVec3f(const FullPointType &pt) { return pt.getVector3fMap(); } 75 | inline Vec3d ToVec3d(const FullPointType &pt) { return pt.getVector3fMap().cast(); } 76 | 77 | } // namespace zjloc 78 | 79 | POINT_CLOUD_REGISTER_POINT_STRUCT(zjloc::FullPointType, 80 | (float, x, x)(float, y, y)(float, z, z)(float, range, range)(float, radius, radius)( 81 | std::uint8_t, intensity, intensity)(std::uint16_t, angle, angle)( 82 | std::uint8_t, ring, ring)(double, time, time)(float, height, height)) 83 | 84 | namespace velodyne_ros 85 | { 86 | struct EIGEN_ALIGN16 Point 87 | { 88 | PCL_ADD_POINT4D; 89 | float intensity; 90 | float time; 91 | std::uint16_t ring; 92 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 93 | }; 94 | } // namespace velodyne_ros 95 | 96 | // clang-format off 97 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 98 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) 99 | (float, time, time)(std::uint16_t, ring, ring)) 100 | // clang-format on 101 | 102 | namespace robosense_ros 103 | { 104 | struct EIGEN_ALIGN16 Point 105 | { 106 | PCL_ADD_POINT4D; 107 | uint8_t intensity; 108 | uint16_t ring = 0; 109 | double timestamp = 0; 110 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 111 | }; 112 | } 113 | 114 | POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point, 115 | (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp)) 116 | 117 | namespace ouster_ros 118 | { 119 | struct EIGEN_ALIGN16 Point 120 | { 121 | PCL_ADD_POINT4D; 122 | float intensity; 123 | uint32_t t; 124 | uint16_t reflectivity; 125 | uint8_t ring; 126 | uint16_t ambient; 127 | uint32_t range; 128 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 129 | }; 130 | } // namespace ouster_ros 131 | 132 | // clang-format off 133 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 134 | (float, x, x) 135 | (float, y, y) 136 | (float, z, z) 137 | (float, intensity, intensity) 138 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 139 | (std::uint32_t, t, t) 140 | (std::uint16_t, reflectivity, reflectivity) 141 | (std::uint8_t, ring, ring) 142 | (std::uint16_t, ambient, ambient) 143 | (std::uint32_t, range, range) 144 | ) 145 | // clang-format on 146 | 147 | namespace pandar_ros 148 | { 149 | struct EIGEN_ALIGN16 Point 150 | { 151 | PCL_ADD_POINT4D; 152 | float intensity; 153 | double timestamp; 154 | uint16_t ring; 155 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 156 | }; 157 | } 158 | POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_ros::Point, 159 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( 160 | double, timestamp, timestamp)(uint16_t, ring, ring)) 161 | #endif // SLAM_IN_AUTO_DRIVING_POINT_TYPES_H 162 | -------------------------------------------------------------------------------- /mad-icp/src/common/sys_utils.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by xiang on 2021/7/21. 3 | // 4 | 5 | #ifndef SLAM_IN_AUTO_DRIVING_SYS_UTILS_H 6 | #define SLAM_IN_AUTO_DRIVING_SYS_UTILS_H 7 | 8 | #include 9 | #include 10 | 11 | namespace zjloc 12 | { 13 | 14 | // 一些系统相关函数，统计代码时间之类的功能 15 | 16 | /** 17 | * 统计代码运行时间 18 | * @tparam FuncT 19 | * @param func 被调用函数 20 | * @param func_name 函数名 21 | * @param times 调用次数 22 | */ 23 | template 24 | void evaluate_and_call(FuncT &&func, const std::string &func_name = "", int times = 10) 25 | { 26 | double total_time = 0; 27 | for (int i = 0; i < times; ++i) 28 | { 29 | auto t1 = std::chrono::high_resolution_clock::now(); 30 | func(); 31 | auto t2 = std::chrono::high_resolution_clock::now(); 32 | total_time += std::chrono::duration_cast>(t2 - t1).count() * 1000; 33 | } 34 | 35 | LOG(INFO) << "方法 " << func_name << " 平均调用时间/次数: " << total_time / times << "/" << times << " 毫秒."; 36 | } 37 | 38 | } // namespace zjloc 39 | 40 | #endif // SLAM_IN_AUTO_DRIVING_SYS_UTILS_H 41 | -------------------------------------------------------------------------------- /mad-icp/src/common/timer/timer.cc: -------------------------------------------------------------------------------- 1 | #include "timer.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace zjloc::common 8 | { 9 | 10 | std::map Timer::records_; 11 | 12 | void Timer::PrintAll() 13 | { 14 | std::cout << ANSI_COLOR_GREEN_BOLD << ">>> ===== Printing run time =====" << std::endl; 15 | for (const auto &r : records_) 16 | { 17 | std::cout << ANSI_COLOR_GREEN << "> [ " << r.first << " ] average time usage: " 18 | << std::accumulate(r.second.time_usage_in_ms_.begin(), r.second.time_usage_in_ms_.end(), 0.0) / 19 | double(r.second.time_usage_in_ms_.size()) 20 | << " ms , called times: " << r.second.time_usage_in_ms_.size() << std::endl; 21 | } 22 | std::cout << ">>> ===== Printing run time end =====" << ANSI_COLOR_RESET << std::endl; 23 | } 24 | 25 | void Timer::DumpIntoFile(const std::string &file_name) 26 | { 27 | std::ofstream ofs(file_name, std::ios::out); 28 | if (!ofs.is_open()) 29 | { 30 | std::cout << ANSI_COLOR_RED_BOLD << "Failed to open file: " << file_name << std::endl; 31 | return; 32 | } 33 | else 34 | { 35 | std::cout << ANSI_COLOR_GREEN_BOLD << "Dump Time Records into file: " << file_name << ANSI_COLOR_RESET << std::endl; 36 | } 37 | 38 | size_t max_length = 0; 39 | for (const auto &iter : records_) 40 | { 41 | ofs << iter.first << ", "; 42 | if (iter.second.time_usage_in_ms_.size() > max_length) 43 | { 44 | max_length = iter.second.time_usage_in_ms_.size(); 45 | } 46 | } 47 | ofs << std::endl; 48 | 49 | for (size_t i = 0; i < max_length; ++i) 50 | { 51 | for (const auto &iter : records_) 52 | { 53 | if (i < iter.second.time_usage_in_ms_.size()) 54 | { 55 | ofs << iter.second.time_usage_in_ms_[i] << ","; 56 | } 57 | else 58 | { 59 | ofs << ","; 60 | } 61 | } 62 | ofs << std::endl; 63 | } 64 | ofs.close(); 65 | } 66 | 67 | double Timer::GetMeanTime(const std::string &func_name) 68 | { 69 | if (records_.find(func_name) == records_.end()) 70 | { 71 | return 0.0; 72 | } 73 | 74 | auto r = records_[func_name]; 75 | return std::accumulate(r.time_usage_in_ms_.begin(), r.time_usage_in_ms_.end(), 0.0) / 76 | double(r.time_usage_in_ms_.size()); 77 | } 78 | 79 | } // namespace zjloc::utils -------------------------------------------------------------------------------- /mad-icp/src/common/timer/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef FUSION_TIMER_H 2 | #define FUSION_TIMER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "common/tool_color_printf.hpp" 13 | 14 | namespace zjloc::common 15 | { 16 | 17 | /// 统计时间工具 18 | /// NOTE Timer套在gtest当中貌似有问题 19 | class Timer 20 | { 21 | public: 22 | struct TimerRecord 23 | { 24 | TimerRecord() = default; 25 | TimerRecord(const std::string &name, double time_usage) 26 | { 27 | func_name_ = name; 28 | time_usage_in_ms_.emplace_back(time_usage); 29 | } 30 | std::string func_name_; 31 | std::vector time_usage_in_ms_; 32 | }; 33 | 34 | /** 35 | * 评价并记录函数用时 36 | * @tparam F 37 | * @param func 38 | * @param func_name 39 | */ 40 | template 41 | static void Evaluate(F &&func, const std::string &func_name) 42 | { 43 | auto t1 = std::chrono::steady_clock::now(); 44 | std::forward(func)(); 45 | auto t2 = std::chrono::steady_clock::now(); 46 | auto time_used = std::chrono::duration_cast>(t2 - t1).count() * 1000; 47 | 48 | if (records_.find(func_name) != records_.end()) 49 | { 50 | records_[func_name].time_usage_in_ms_.emplace_back(time_used); 51 | } 52 | else 53 | { 54 | records_.insert({func_name, TimerRecord(func_name, time_used)}); 55 | } 56 | } 57 | 58 | /// 打印记录的所有耗时 59 | static void PrintAll(); 60 | 61 | /// 写入文件，方便作图分析 62 | static void DumpIntoFile(const std::string &file_name); 63 | 64 | /// 获取某个函数的平均执行时间 65 | static double GetMeanTime(const std::string &func_name); 66 | 67 | /// 清理记录 68 | static void Clear() { records_.clear(); } 69 | 70 | private: 71 | static std::map records_; 72 | }; 73 | 74 | class TicToc 75 | { 76 | public: 77 | TicToc() 78 | { 79 | tic(); 80 | } 81 | 82 | TicToc(bool _disp) 83 | { 84 | disp_ = _disp; 85 | tic(); 86 | } 87 | 88 | void tic() 89 | { 90 | start = std::chrono::system_clock::now(); 91 | } 92 | 93 | void toc(std::string _about_task) 94 | { 95 | end = std::chrono::system_clock::now(); 96 | std::chrono::duration elapsed_seconds = end - start; 97 | double elapsed_ms = elapsed_seconds.count() * 1000; 98 | 99 | if (disp_) 100 | { 101 | std::cout.precision(3); // 10 for sec, 3 for ms 102 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 103 | } 104 | } 105 | double toc() 106 | { 107 | end = std::chrono::system_clock::now(); 108 | std::chrono::duration elapsed_seconds = end - start; 109 | double elapsed_ms = elapsed_seconds.count() * 1000; 110 | return elapsed_ms; 111 | } 112 | 113 | private: 114 | std::chrono::time_point start, end; 115 | bool disp_ = false; 116 | }; 117 | 118 | } // namespace zjloc::utils 119 | 120 | #endif // FUSION_TIMER_H 121 | -------------------------------------------------------------------------------- /mad-icp/src/common/tool_color_printf.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __TOOLS_COLOR_PRINT_HPP__ 3 | #define __TOOLS_COLOR_PRINT_HPP__ 4 | #include 5 | #include 6 | /* 7 | * Make screen print colorful :) 8 | * Related link: 9 | * [1] https://en.wikipedia.org/wiki/ANSI_escape_code 10 | * [2] https://github.com/caffedrine/CPP_Utils/blob/597fe5200be87fa1db2d2aa5d8a07c3bc32a66cd/Utils/include/AnsiColors.h 11 | * 12 | */ 13 | 14 | // const std::string _tools_color_printf_version = "V1.0"; 15 | // const std::string _tools_color_printf_info = "[Init]: Add macros, add scope_color()"; 16 | const std::string _tools_color_printf_version = "V1.2"; 17 | const std::string _tools_color_printf_info = "[Enh]: Add delete lines, ANSI_SCREEN_FLUSH"; 18 | using std::cout; 19 | using std::endl; 20 | // clang-format off 21 | #ifdef EMPTY_ANSI_COLORS 22 | #define ANSI_COLOR_RED "" 23 | #define ANSI_COLOR_RED_BOLD "" 24 | #define ANSI_COLOR_GREEN "" 25 | #define ANSI_COLOR_GREEN_BOLD "" 26 | #define ANSI_COLOR_YELLOW "" 27 | #define ANSI_COLOR_YELLOW_BOLD "" 28 | #define ANSI_COLOR_BLUE "" 29 | #define ANSI_COLOR_BLUE_BOLD "" 30 | #define ANSI_COLOR_MAGENTA "" 31 | #define ANSI_COLOR_MAGENTA_BOLD "" 32 | #else 33 | #define ANSI_COLOR_RED "\x1b[0;31m" 34 | #define ANSI_COLOR_RED_BOLD "\x1b[1;31m" 35 | #define ANSI_COLOR_RED_BG "\x1b[0;41m" 36 | 37 | #define ANSI_COLOR_GREEN "\x1b[0;32m" 38 | #define ANSI_COLOR_GREEN_BOLD "\x1b[1;32m" 39 | #define ANSI_COLOR_GREEN_BG "\x1b[0;42m" 40 | 41 | #define ANSI_COLOR_YELLOW "\x1b[0;33m" 42 | #define ANSI_COLOR_YELLOW_BOLD "\x1b[1;33m" 43 | #define ANSI_COLOR_YELLOW_BG "\x1b[0;43m" 44 | 45 | #define ANSI_COLOR_BLUE "\x1b[0;34m" 46 | #define ANSI_COLOR_BLUE_BOLD "\x1b[1;34m" 47 | #define ANSI_COLOR_BLUE_BG "\x1b[0;44m" 48 | 49 | #define ANSI_COLOR_MAGENTA "\x1b[0;35m" 50 | #define ANSI_COLOR_MAGENTA_BOLD "\x1b[1;35m" 51 | #define ANSI_COLOR_MAGENTA_BG "\x1b[0;45m" 52 | 53 | #define ANSI_COLOR_CYAN "\x1b[0;36m" 54 | #define ANSI_COLOR_CYAN_BOLD "\x1b[1;36m" 55 | #define ANSI_COLOR_CYAN_BG "\x1b[0;46m" 56 | 57 | #define ANSI_COLOR_WHITE "\x1b[0;37m" 58 | #define ANSI_COLOR_WHITE_BOLD "\x1b[1;37m" 59 | #define ANSI_COLOR_WHITE_BG "\x1b[0;47m" 60 | 61 | #define ANSI_COLOR_BLACK "\x1b[0;30m" 62 | #define ANSI_COLOR_BLACK_BOLD "\x1b[1;30m" 63 | #define ANSI_COLOR_BLACK_BG "\x1b[0;40m" 64 | 65 | #define ANSI_COLOR_RESET "\x1b[0m" 66 | 67 | #define ANSI_DELETE_LAST_LINE "\033[A\33[2K\r" 68 | #define ANSI_DELETE_CURRENT_LINE "\33[2K\r" 69 | #define ANSI_SCREEN_FLUSH std::fflush(stdout); 70 | 71 | #define SET_PRINT_COLOR( a ) cout << a ; 72 | 73 | #endif 74 | // clang-format on 75 | 76 | struct _Scope_color 77 | { 78 | _Scope_color(const char *color) 79 | { 80 | std::cout << color; 81 | } 82 | 83 | ~_Scope_color() 84 | { 85 | std::cout << ANSI_COLOR_RESET; 86 | } 87 | }; 88 | 89 | #define scope_color(a) _Scope_color _scope(a); 90 | 91 | // inline int demo_test_color_printf() 92 | // { 93 | // int i, j, n; 94 | 95 | // for ( i = 0; i < 11; i++ ) 96 | // { 97 | // for ( j = 0; j < 10; j++ ) 98 | // { 99 | // n = 10 * i + j; 100 | // if ( n > 108 ) 101 | // break; 102 | // printf( "\033[%dm %3d\033[m", n, n ); 103 | // } 104 | // printf( "\n" ); 105 | // } 106 | // return ( 0 ); 107 | // }; 108 | #endif -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(tools) 2 | add_subdirectory(odometry) 3 | 4 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(odometry SHARED 2 | vel_estimator.cpp 3 | pipeline.cpp 4 | mad_icp.cpp 5 | ) 6 | 7 | target_link_libraries(odometry 8 | tools 9 | ) 10 | 11 | target_compile_features(odometry PUBLIC) 12 | 13 | # pybind11_add_module(pypeline pybind/pypeline.cpp) 14 | # target_link_libraries(pypeline PUBLIC 15 | # odometry 16 | # tools 17 | # ) 18 | 19 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/mad_icp.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "mad_icp/odometry/mad_icp.h" 30 | 31 | MADicp::MADicp(double min_ball, double rho_ker, double b_ratio, int num_threads) 32 | : min_ball_(min_ball), rho_ker_(sqrt(rho_ker)), b_ratio_(b_ratio), num_threads_(num_threads) 33 | { 34 | X_.setIdentity(); 35 | H_adder_.setZero(); 36 | b_adder_.setZero(); 37 | 38 | H_adders_ = std::vector(num_threads); 39 | b_adders_ = std::vector(num_threads); 40 | } 41 | 42 | void MADicp::resetAdders() 43 | { 44 | H_adder_.setZero(); 45 | b_adder_.setZero(); 46 | 47 | for (size_t i = 0; i < num_threads_; ++i) 48 | { 49 | H_adders_[i].setZero(); 50 | b_adders_[i].setZero(); 51 | } 52 | } 53 | 54 | void MADicp::setMoving(const LeafList &moving_leaves) 55 | { 56 | moving_leaves_ = moving_leaves; 57 | } 58 | 59 | void MADicp::init(const Eigen::Isometry3d &moving_in_fixed) 60 | { 61 | X_ = moving_in_fixed; 62 | } 63 | 64 | void MADicp::errorAndJacobian(double &e, 65 | JacobianMatrixType &J, 66 | const MADtree &fixed, 67 | const MADtree &moving, 68 | const Eigen::Vector3d &moving_transformed) const 69 | { 70 | const auto &fixed_point = fixed.mean_; 71 | const auto &fixed_normal = fixed.eigenvectors_.col(0); 72 | const auto &moving_point = moving.mean_; 73 | const Eigen::Matrix3d &R = X_.linear(); 74 | 75 | e = (moving_transformed - fixed_point).dot(fixed_normal); 76 | J.block<1, 3>(0, 0) = fixed_normal.transpose() * R; 77 | J.block<1, 3>(0, 3) = -J.block<1, 3>(0, 0) * skew(moving_point); 78 | } 79 | 80 | void MADicp::update(const MADtree *fixed_tree) 81 | { 82 | const int thread_id = omp_get_thread_num(); 83 | 84 | for (auto &moving : moving_leaves_) 85 | { 86 | const Eigen::Vector3d ml = X_ * moving->mean_; 87 | const auto f = fixed_tree->bestMatchingLeafFast(ml); 88 | 89 | const double src_ball = min_ball_ + b_ratio_ * moving->mean_.norm(); 90 | if ((ml - f->mean_).norm() > src_ball) 91 | continue; 92 | 93 | moving->matched_ = true; 94 | 95 | JacobianMatrixType J; 96 | double e; 97 | 98 | errorAndJacobian(e, J, *f, *moving, ml); 99 | 100 | double scale = 1.; 101 | const double chi = abs(e); 102 | if (chi > rho_ker_) 103 | { 104 | scale = rho_ker_ / chi; 105 | } 106 | const double w = 1. - f->bbox_(0) / min_ball_; 107 | scale *= w * w; 108 | 109 | H_adders_[thread_id] += scale * J.transpose() * J; 110 | b_adders_[thread_id] += scale * J.transpose() * e; 111 | } 112 | } 113 | 114 | void MADicp::updateState() 115 | { 116 | for (size_t i = 0; i < num_threads_; ++i) 117 | { 118 | H_adder_ += H_adders_[i]; 119 | b_adder_ += b_adders_[i]; 120 | } 121 | 122 | Vector6d dx = H_adder_.ldlt().solve(-b_adder_); 123 | Eigen::Isometry3d dX = Eigen::Isometry3d::Identity(); 124 | 125 | dX.linear() = expMapSO3(dx.tail(3)); 126 | dX.translation() = dx.head(3); 127 | X_ = X_ * dX; 128 | } -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/mad_icp.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | class MADicp 42 | { 43 | public: 44 | using JacobianMatrixType = Eigen::Matrix; 45 | 46 | // MADicp(); 47 | 48 | MADicp(double min_ball, double rho_ker, double b_ratio, int num_threads); 49 | 50 | void resetAdders(); 51 | 52 | void setMoving(const LeafList &moving_leaves); 53 | 54 | void init(const Eigen::Isometry3d &moving_in_fixed = Eigen::Isometry3d::Identity()); 55 | 56 | void oneRound(); 57 | 58 | void update(const MADtree *fixed_tree); 59 | 60 | void updateState(); 61 | 62 | void errorAndJacobian(double &e, 63 | JacobianMatrixType &J, 64 | const MADtree &fixed, 65 | const MADtree &moving, 66 | const Eigen::Vector3d &moving_transformed) const; 67 | 68 | Eigen::Isometry3d X_; 69 | 70 | Matrix6d H_adder_; 71 | Vector6d b_adder_; 72 | 73 | LeafList moving_leaves_; 74 | 75 | std::vector H_adders_; 76 | std::vector b_adders_; 77 | 78 | double rho_ker_; 79 | double min_ball_; 80 | double b_ratio_; 81 | int num_threads_; 82 | }; 83 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/pipeline.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "mad_icp/odometry/pipeline.h" 30 | 31 | #include 32 | #include 33 | 34 | Pipeline::Pipeline(double sensor_hz, 35 | bool deskew, 36 | double b_max, 37 | double rho_ker, 38 | double p_th, 39 | double b_min, 40 | double b_ratio, 41 | int num_keyframes, 42 | int num_threads, 43 | bool realtime) : sensor_hz_(sensor_hz), 44 | deskew_(deskew), 45 | b_max_(b_max), 46 | p_th_(p_th), 47 | b_min_(b_min), 48 | num_keyframes_(num_keyframes), 49 | num_threads_(num_threads), 50 | realtime_(realtime), 51 | icp_(b_max, rho_ker, b_ratio, num_threads), 52 | vel_estimator_(sensor_hz) 53 | { 54 | current_tree_ = nullptr; 55 | frame_to_map_.setIdentity(); 56 | keyframe_to_map_.setIdentity(); 57 | current_velocity_.setZero(); 58 | seq_ = 0; 59 | seq_keyframe_ = 0; 60 | is_initialized_ = false; 61 | is_map_updated_ = false; 62 | 63 | max_parallel_levels_ = static_cast(std::log2(num_threads)); 64 | omp_set_num_threads(num_threads); 65 | } 66 | 67 | Pipeline::~Pipeline() 68 | { 69 | while (!frames_.empty()) 70 | { 71 | delete frames_.front()->tree_; 72 | frames_.pop_front(); 73 | } 74 | while (!keyframes_.empty()) 75 | { 76 | delete keyframes_.front()->tree_; 77 | keyframes_.pop_front(); 78 | } 79 | } 80 | 81 | void Pipeline::deskew(const ContainerTypePtr &curr_cloud, const Eigen::Isometry3d &T_prev, const Eigen::Isometry3d &T_now) 82 | { 83 | const double ts = 1. / sensor_hz_; 84 | 85 | Vector6d naive_vel; 86 | Eigen::Isometry3d T_now_to_prev = T_prev.inverse() * T_now; 87 | naive_vel.head(3) = T_now_to_prev.translation(); 88 | naive_vel.tail(3) = logMapSO3(T_now_to_prev.linear()); 89 | naive_vel = naive_vel / ts; 90 | 91 | using AzimuthPair = std::pair; 92 | std::vector sorted(curr_cloud->size()); 93 | 94 | for (size_t i = 0; i < sorted.size(); ++i) 95 | { 96 | const Eigen::Vector3d &point = curr_cloud->at(i); 97 | const double azimuth = atan2(point.y(), point.x()); 98 | sorted[i] = std::make_pair(azimuth, point); 99 | } 100 | 101 | std::sort( 102 | sorted.begin(), sorted.end(), [](AzimuthPair first, AzimuthPair second) -> bool 103 | { return first.first < second.first; }); 104 | 105 | const double resolution = 2 * M_PI / double(CHUNKS); 106 | const double delta = ts / double(CHUNKS - 1); 107 | double t = -ts; 108 | 109 | Vector6d delta_s = naive_vel * t; 110 | Eigen::Isometry3d meas_pose_to_robot = Eigen::Isometry3d::Identity(); 111 | meas_pose_to_robot.linear() = expMapSO3(delta_s.tail(3)); 112 | meas_pose_to_robot.translation() = delta_s.head(3); 113 | 114 | double angle = M_PI - resolution; 115 | for (int i = sorted.size() - 1; i >= 0; --i) 116 | { 117 | if (sorted[i].first < angle) 118 | { 119 | angle -= resolution; 120 | t += delta; 121 | 122 | delta_s = naive_vel * t; 123 | meas_pose_to_robot = Eigen::Isometry3d::Identity(); 124 | meas_pose_to_robot.linear() = expMapSO3(delta_s.tail(3)); 125 | meas_pose_to_robot.translation() = delta_s.head(3); 126 | } 127 | 128 | (*curr_cloud)[i] = meas_pose_to_robot * sorted[i].second; 129 | } 130 | } 131 | 132 | void Pipeline::compute(const double &curr_stamp, ContainerType curr_cloud_mem) 133 | { 134 | ContainerType *curr_cloud = &curr_cloud_mem; 135 | is_map_updated_ = false; 136 | 137 | if (!is_initialized_) 138 | { 139 | initialize(curr_stamp, curr_cloud); 140 | return; 141 | } 142 | 143 | struct timeval preprocessing_start, preprocessing_end, preprocessing_delta; 144 | gettimeofday(&preprocessing_start, nullptr); 145 | 146 | if (deskew_ && trajectory_.size() > 1) 147 | deskew(curr_cloud, trajectory_[trajectory_.size() - 2], trajectory_[trajectory_.size() - 1]); 148 | 149 | current_tree_ = 150 | new MADtree(curr_cloud, curr_cloud->begin(), curr_cloud->end(), b_max_, b_min_, 0, max_parallel_levels_, nullptr, nullptr); 151 | 152 | current_leaves_.clear(); 153 | current_tree_->getLeafs(std::back_insert_iterator(current_leaves_)); 154 | 155 | Vector6d dx = current_velocity_ * 1. / sensor_hz_; 156 | Eigen::Isometry3d dX; 157 | const Eigen::Matrix3d dR = expMapSO3(dx.tail(3)); 158 | dX.setIdentity(); 159 | dX.linear() = dR; 160 | dX.translation() = dx.head(3); 161 | Eigen::Isometry3d prediction = frame_to_map_ * dX; 162 | 163 | icp_.setMoving(current_leaves_); 164 | icp_.init(prediction); 165 | 166 | float icp_time = 0; 167 | float total_icp_time = 0; 168 | 169 | gettimeofday(&preprocessing_end, nullptr); 170 | timersub(&preprocessing_end, &preprocessing_start, &preprocessing_delta); 171 | const float preprocessing_time = float(preprocessing_delta.tv_sec) * 1000. + 1e-3 * preprocessing_delta.tv_usec; 172 | // std::cout << "Preprocessing time in ms: " << preprocessing_time << std::endl; 173 | 174 | struct timeval icp_start, icp_end, icp_delta; 175 | 176 | for (size_t icp_iteration = 0; icp_iteration < MAX_ICP_ITS; ++icp_iteration) 177 | { 178 | const float remaining_time = 90.0 - (preprocessing_time + total_icp_time + icp_time); 179 | if (realtime_ && remaining_time < 0) 180 | break; 181 | 182 | gettimeofday(&icp_start, nullptr); 183 | if (icp_iteration == MAX_ICP_ITS - 1) 184 | { 185 | for (MADtree *l : current_leaves_) 186 | { 187 | l->matched_ = false; 188 | } 189 | } 190 | 191 | icp_.resetAdders(); 192 | 193 | #pragma omp parallel for 194 | for (const Frame *frame : keyframes_) 195 | { 196 | icp_.update(frame->tree_); 197 | } 198 | 199 | #pragma omp barrier 200 | 201 | icp_.updateState(); 202 | 203 | gettimeofday(&icp_end, nullptr); 204 | timersub(&icp_end, &icp_start, &icp_delta); 205 | icp_time = float(icp_delta.tv_sec) * 1000. + 1e-3 * icp_delta.tv_usec; 206 | // std::cout << "icp_time: " << icp_time << std::endl << std::endl; 207 | total_icp_time += icp_time; 208 | } 209 | 210 | frame_to_map_ = icp_.X_; 211 | 212 | int matched_leaves = 0; 213 | for (MADtree *l : current_leaves_) 214 | { 215 | if (l->matched_) 216 | { 217 | matched_leaves++; 218 | } 219 | } 220 | 221 | double inliers_ratio = double(matched_leaves) / double(current_leaves_.size()); 222 | // std::cout << "Inliers ratio: " << inliers_ratio << std::endl; 223 | 224 | trajectory_.push_back(frame_to_map_); 225 | 226 | std::vector odom_window; 227 | for (int i = std::max(0, int(trajectory_.size()) - SMOOTHING_T); i < trajectory_.size(); ++i) 228 | { 229 | odom_window.push_back(trajectory_[i]); 230 | } 231 | 232 | vel_estimator_.init(current_velocity_); 233 | vel_estimator_.setOdometry(odom_window); 234 | 235 | vel_estimator_.oneRound(); 236 | current_velocity_ = vel_estimator_.X_; 237 | 238 | Frame *current_frame(new Frame); 239 | current_frame->frame_ = seq_; 240 | current_frame->frame_to_map_ = frame_to_map_; 241 | current_frame->stamp_ = curr_stamp; 242 | current_frame->weight_ = icp_.H_adder_.inverse().determinant(); 243 | current_tree_->applyTransform(frame_to_map_.linear(), frame_to_map_.translation()); 244 | current_frame->tree_ = current_tree_; 245 | current_frame->leaves_ = current_leaves_; 246 | 247 | frames_.push_back(current_frame); 248 | if (frames_.size() > FRAME_WINDOW) 249 | { 250 | delete frames_.front()->tree_; 251 | frames_.pop_front(); 252 | } 253 | 254 | if (inliers_ratio < p_th_) 255 | { 256 | double best_weight = std::numeric_limits::max(); 257 | int new_seq = 0; 258 | Frame *best_frame = nullptr; 259 | for (Frame *frame : frames_) 260 | { 261 | if (frame->weight_ < best_weight) 262 | { 263 | best_weight = frame->weight_; 264 | new_seq = frame->frame_; 265 | best_frame = frame; 266 | } 267 | } 268 | 269 | while (!frames_.empty() && frames_.front()->frame_ <= new_seq) 270 | { 271 | if (frames_.front()->frame_ < new_seq) 272 | { 273 | delete frames_.front()->tree_; 274 | } 275 | frames_.pop_front(); 276 | } 277 | 278 | keyframes_.push_back(best_frame); 279 | if (keyframes_.size() > num_keyframes_) 280 | { 281 | delete keyframes_.front()->tree_; 282 | keyframes_.pop_front(); 283 | } 284 | 285 | is_map_updated_ = true; 286 | seq_keyframe_ = new_seq; 287 | keyframe_to_map_ = best_frame->frame_to_map_; 288 | } 289 | 290 | seq_++; 291 | } 292 | 293 | void Pipeline::initialize(const double &curr_stamp, const ContainerTypePtr curr_cloud) 294 | { 295 | Frame *current_frame(new Frame); 296 | current_frame->frame_ = seq_; 297 | current_frame->frame_to_map_ = frame_to_map_; 298 | current_frame->stamp_ = curr_stamp; 299 | current_frame->tree_ = 300 | new MADtree(curr_cloud, curr_cloud->begin(), curr_cloud->end(), b_max_, b_min_, 0, max_parallel_levels_, nullptr, nullptr); 301 | 302 | current_frame->tree_->getLeafs(std::back_insert_iterator(current_frame->leaves_)); 303 | 304 | keyframes_.push_back(current_frame); 305 | 306 | trajectory_.push_back(Eigen::Isometry3d::Identity()); 307 | 308 | is_initialized_ = true; 309 | is_map_updated_ = true; 310 | seq_++; 311 | } 312 | 313 | const bool Pipeline::isMapUpdated() 314 | { 315 | return is_map_updated_; 316 | } 317 | 318 | const ContainerType Pipeline::currentLeaves() 319 | { 320 | ContainerType leaves; 321 | std::back_insert_iterator leaves_it(leaves); 322 | for (MADtree *leaf : current_leaves_) 323 | { 324 | ++leaves_it = leaf->mean_; 325 | } 326 | return leaves; 327 | } 328 | 329 | const ContainerType Pipeline::modelLeaves() 330 | { 331 | ContainerType leaves; 332 | std::back_insert_iterator leaves_it(leaves); 333 | for (auto frame : keyframes_) 334 | { 335 | for (MADtree *leaf : frame->leaves_) 336 | { 337 | ++leaves_it = leaf->mean_; 338 | } 339 | } 340 | return leaves; 341 | } -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/pipeline.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include "mad_icp/odometry/mad_icp.h" 32 | #include "mad_icp/odometry/vel_estimator.h" 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | class Pipeline 46 | { 47 | public: 48 | Pipeline(double sensor_hz, 49 | bool deskew, 50 | double b_max, 51 | double rho_ker, 52 | double p_th, 53 | double b_min, 54 | double b_ratio, 55 | int num_keyframes, 56 | int num_threads, 57 | bool realtime); 58 | ~Pipeline(); 59 | 60 | // clang-format off 61 | const Eigen::Matrix currentPose() const { return frame_to_map_.matrix(); } 62 | const std::vector& trajectory() const { return trajectory_; } 63 | const Eigen::Matrix keyframePose() const { return keyframe_to_map_.matrix(); } 64 | const bool isInitialized() const { return is_initialized_; } 65 | const size_t currentID() const { return seq_; } 66 | const size_t keyframeID() const { return seq_keyframe_; } 67 | // clang-format on 68 | 69 | const bool isMapUpdated(); 70 | const ContainerType currentLeaves(); 71 | const ContainerType modelLeaves(); 72 | void compute(const double &curr_stamp, ContainerType curr_cloud_mem); 73 | 74 | protected: 75 | void initialize(const double &curr_stamp, const ContainerTypePtr curr_cloud); 76 | void deskew(const ContainerTypePtr &curr_cloud, const Eigen::Isometry3d &T_prev, const Eigen::Isometry3d &T_now); 77 | 78 | MADicp icp_; 79 | VelEstimator vel_estimator_; 80 | 81 | Eigen::Isometry3d frame_to_map_; 82 | Eigen::Isometry3d keyframe_to_map_; 83 | 84 | Vector6d current_velocity_; 85 | 86 | std::deque keyframes_; 87 | std::deque frames_; 88 | std::vector trajectory_; 89 | 90 | MADtree *current_tree_; 91 | LeafList model_leaves_, current_leaves_; 92 | 93 | // start some params 94 | bool deskew_, realtime_; 95 | int num_keyframes_, num_threads_, max_parallel_levels_; 96 | double sensor_hz_, b_max_, p_th_, b_min_; 97 | // end some params 98 | 99 | size_t seq_; 100 | size_t seq_keyframe_; 101 | bool is_initialized_; 102 | bool is_map_updated_; 103 | }; 104 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/vel_estimator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "mad_icp/odometry/vel_estimator.h" 30 | #include 31 | VelEstimator::VelEstimator(double sensor_hz) 32 | { 33 | X_.setZero(); 34 | ts_ = 1. / sensor_hz; 35 | } 36 | 37 | void VelEstimator::init(const Vector6d &velocity) 38 | { 39 | X_ = velocity; 40 | } 41 | 42 | void VelEstimator::setOdometry(const std::vector &odometry) 43 | { 44 | odometry_ = odometry; 45 | } 46 | 47 | void VelEstimator::errorAndJacobian(Vector6d &e, 48 | Matrix6d &J, 49 | const Eigen::Isometry3d &T_now, 50 | const Eigen::Isometry3d &T_prev, 51 | const double delta_t) 52 | { 53 | Eigen::Isometry3d T_now_to_prev = T_prev.inverse() * T_now; 54 | e.block<3, 1>(0, 0) = delta_t * X_.block<3, 1>(0, 0) - T_now_to_prev.translation(); 55 | 56 | Eigen::Vector3d angles; 57 | angles(0) = atan2(-T_now_to_prev.linear()(1, 2), T_now_to_prev.linear()(2, 2)); 58 | angles(1) = asin(T_now_to_prev.linear()(0, 2)); 59 | angles(2) = atan2(-T_now_to_prev.linear()(0, 1), T_now_to_prev.linear()(0, 0)); 60 | e.block<3, 1>(3, 0) = delta_t * X_.block<3, 1>(3, 0) - angles; 61 | 62 | J = Matrix6d::Identity() * delta_t; 63 | } 64 | 65 | void VelEstimator::update(const Eigen::Isometry3d &T_now, 66 | const Eigen::Isometry3d &T_prev, 67 | const double delta_t, 68 | const double weight) 69 | { 70 | Vector6d e; 71 | Matrix6d J; 72 | errorAndJacobian(e, J, T_now, T_prev, delta_t); 73 | 74 | double scale = 1.; 75 | const double chi2 = e.squaredNorm(); 76 | const double chi = sqrt(chi2); 77 | if (chi > E_THRESHOLD_VEL) 78 | { 79 | scale = E_THRESHOLD_VEL / chi; 80 | } 81 | 82 | H_adder_ += scale * weight * J.transpose() * J; 83 | b_adder_ += scale * weight * J.transpose() * e; 84 | } 85 | 86 | void VelEstimator::oneRound() 87 | { 88 | H_adder_.setZero(); 89 | b_adder_.setZero(); 90 | 91 | const Eigen::Isometry3d T_now = odometry_.back(); 92 | 93 | for (size_t i = 0; i < odometry_.size() - 1; ++i) 94 | { 95 | const Eigen::Isometry3d T_prev = odometry_[i]; 96 | const double delta_t = (odometry_.size() - 1 - i) * ts_; 97 | const double weight = 1.f - double(odometry_.size() - 2 - i) / double(odometry_.size() - 1); 98 | 99 | update(T_now, T_prev, delta_t, weight); 100 | } 101 | 102 | Vector6d dx = H_adder_.ldlt().solve(-b_adder_); 103 | X_ += dx; 104 | } 105 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/odometry/vel_estimator.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | struct VelEstimator 40 | { 41 | VelEstimator(double sensor_hz); 42 | 43 | void setOdometry(const std::vector &odometry); 44 | 45 | void init(const Vector6d &velocity = Vector6d::Zero()); 46 | void oneRound(); 47 | 48 | void errorAndJacobian(Vector6d &e, 49 | Matrix6d &J, 50 | const Eigen::Isometry3d &T_now, 51 | const Eigen::Isometry3d &T_prev, 52 | const double delta_t); 53 | void update(const Eigen::Isometry3d &T_now, const Eigen::Isometry3d &T_prev, const double delta_t, const double scale); 54 | 55 | Matrix6d H_adder_; 56 | Vector6d X_, b_adder_; 57 | std::vector odometry_; 58 | double ts_; 59 | }; -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(tools SHARED 2 | mad_tree.cpp 3 | ) 4 | 5 | target_link_libraries(tools) 6 | target_compile_features(tools PUBLIC) -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/constants.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | static constexpr int CHUNKS = 1024; 32 | static constexpr int SMOOTHING_T = 10; 33 | static constexpr double E_THRESHOLD_VEL = 0.3162; 34 | static constexpr int MAX_ICP_ITS = 15; 35 | static constexpr int FRAME_WINDOW = 10; 36 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/frame.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include "mad_icp/tools/mad_tree.h" 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | struct Frame 38 | { 39 | Frame() 40 | { 41 | frame_to_map_.setIdentity(); 42 | tree_ = nullptr; 43 | stamp_ = 0.; 44 | weight_ = 0.; 45 | frame_ = 0; 46 | }; 47 | 48 | Eigen::Isometry3d frame_to_map_; 49 | MADtree *tree_; 50 | LeafList leaves_; 51 | double stamp_; 52 | double weight_; 53 | int frame_; 54 | }; -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/lie_algebra.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | inline Eigen::Matrix3d skew(const Eigen::Vector3d &v_) 34 | { 35 | Eigen::Matrix3d S; 36 | S << 0., -v_[2], v_[1], v_[2], 0., -v_[0], -v_[1], v_[0], 0.; 37 | return S; 38 | } 39 | 40 | inline Eigen::Matrix3d expMapSO3(const Eigen::Vector3d &omega_) 41 | { 42 | Eigen::Matrix3d R; 43 | const double theta_square = omega_.dot(omega_); 44 | const double theta = sqrt(theta_square); 45 | const Eigen::Matrix3d W = skew(omega_); 46 | const Eigen::Matrix3d K = W / theta; 47 | if (theta_square < double(1e-8)) 48 | { 49 | R = Eigen::Matrix3d::Identity() + W; 50 | } 51 | else 52 | { 53 | const double one_minus_cos = double(2) * sin(theta / double(2)) * sin(theta / double(2)); 54 | R = Eigen::Matrix3d::Identity() + sin(theta) * K + one_minus_cos * K * K; 55 | } 56 | return R; 57 | } 58 | 59 | inline Eigen::Vector3d logMapSO3(const Eigen::Matrix3d &R) 60 | { 61 | const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); 62 | const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); 63 | const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); 64 | 65 | // get trace(R) 66 | const double tr = R.trace(); 67 | const double pi(M_PI); 68 | const double two(2); 69 | 70 | Eigen::Vector3d omega; 71 | // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. 72 | // we do something special 73 | if (tr + double(1.0) < double(1e-10)) 74 | { 75 | if (abs(R33 + double(1.0)) > double(1e-5)) 76 | { 77 | omega = (pi / sqrt(two + two * R33)) * Eigen::Vector3d(R13, R23, double(1.0) + R33); 78 | } 79 | else if (abs(R22 + double(1.0)) > double(1e-5)) 80 | { 81 | omega = (pi / sqrt(two + two * R22)) * Eigen::Vector3d(R12, double(1.0) + R22, R32); 82 | } 83 | else 84 | { 85 | omega = (pi / sqrt(two + two * R11)) * Eigen::Vector3d(double(1.0) + R11, R21, R31); 86 | } 87 | } 88 | else 89 | { 90 | double magnitude; 91 | const double tr_3 = tr - double(3.0); // always negative 92 | if (tr_3 < double(-1e-7)) 93 | { 94 | double theta = acos((tr - double(1.0)) / two); 95 | magnitude = theta / (two * sin(theta)); 96 | } 97 | else 98 | { 99 | // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) 100 | // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) 101 | magnitude = double(0.5) - tr_3 * tr_3 / double(12.0); 102 | } 103 | omega = magnitude * Eigen::Vector3d(R32 - R23, R13 - R31, R21 - R12); 104 | } 105 | return omega; 106 | } -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/mad_tree.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "mad_icp/tools/mad_tree.h" 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | MADtree::MADtree(const ContainerTypePtr vec, 36 | const IteratorType begin, 37 | const IteratorType end, 38 | const double b_max, 39 | const double b_min, 40 | const int level, 41 | const int max_parallel_level, 42 | MADtree *parent, 43 | MADtree *plane_predecessor) 44 | { 45 | build(vec, begin, end, b_max, b_min, level, max_parallel_level, parent, plane_predecessor); 46 | } 47 | 48 | void MADtree::build(const ContainerTypePtr vec, 49 | const IteratorType begin, 50 | const IteratorType end, 51 | const double b_max, 52 | const double b_min, 53 | const int level, 54 | const int max_parallel_level, 55 | MADtree *parent, 56 | MADtree *plane_predecessor) 57 | { 58 | parent_ = parent; 59 | Eigen::Matrix3d cov; 60 | computeMeanAndCovariance(mean_, cov, begin, end); 61 | Eigen::SelfAdjointEigenSolver es; 62 | es.computeDirect(cov); 63 | eigenvectors_ = es.eigenvectors(); 64 | num_points_ = computeBoundingBox(bbox_, mean_, eigenvectors_.transpose(), begin, end); 65 | 66 | if (bbox_(2) < b_max) 67 | { 68 | if (plane_predecessor) 69 | { 70 | eigenvectors_.col(0) = plane_predecessor->eigenvectors_.col(0); 71 | } 72 | else 73 | { 74 | if (num_points_ < 3) 75 | { 76 | MADtree *node = this; 77 | while (node->parent_ && node->num_points_ < 3) 78 | node = node->parent_; 79 | eigenvectors_.col(0) = node->eigenvectors_.col(0); 80 | } 81 | } 82 | 83 | Eigen::Vector3d &nearest_point = *begin; 84 | double shortest_dist = std::numeric_limits::max(); 85 | for (IteratorType it = begin; it != end; ++it) 86 | { 87 | const Eigen::Vector3d &v = *it; 88 | const double dist = (v - mean_).norm(); 89 | if (dist < shortest_dist) 90 | { 91 | nearest_point = v; 92 | shortest_dist = dist; 93 | } 94 | } 95 | mean_ = nearest_point; 96 | 97 | return; 98 | } 99 | if (!plane_predecessor) 100 | { 101 | if (bbox_(0) < b_min) 102 | plane_predecessor = this; 103 | } 104 | 105 | const Eigen::Vector3d &_split_plane_normal = eigenvectors_.col(2); 106 | IteratorType middle = 107 | split(begin, end, [&](const Eigen::Vector3d &p) -> bool 108 | { return (p - mean_).dot(_split_plane_normal) < double(0); }); 109 | 110 | if (level >= max_parallel_level) 111 | { 112 | left_ = 113 | new MADtree(vec, begin, middle, b_max, b_min, level + 1, max_parallel_level, this, plane_predecessor); 114 | 115 | right_ = 116 | new MADtree(vec, middle, end, b_max, b_min, level + 1, max_parallel_level, this, plane_predecessor); 117 | } 118 | else 119 | { 120 | std::future l = std::async(MADtree::makeSubtree, 121 | vec, 122 | begin, 123 | middle, 124 | b_max, 125 | b_min, 126 | level + 1, 127 | max_parallel_level, 128 | this, 129 | plane_predecessor); 130 | 131 | std::future r = std::async(MADtree::makeSubtree, 132 | vec, 133 | middle, 134 | end, 135 | b_max, 136 | b_min, 137 | level + 1, 138 | max_parallel_level, 139 | this, 140 | plane_predecessor); 141 | left_ = l.get(); 142 | right_ = r.get(); 143 | } 144 | } 145 | 146 | MADtree *MADtree::makeSubtree(const ContainerTypePtr vec, 147 | const IteratorType begin, 148 | const IteratorType end, 149 | const double b_max, 150 | const double b_min, 151 | const int level, 152 | const int max_parallel_level, 153 | MADtree *parent, 154 | MADtree *plane_predecessor) 155 | { 156 | return new MADtree(vec, begin, end, b_max, b_min, level, max_parallel_level, parent, plane_predecessor); 157 | } 158 | 159 | const MADtree *MADtree::bestMatchingLeafFast(const Eigen::Vector3d &query) const 160 | { 161 | const MADtree *node = this; 162 | while (node->left_ || node->right_) 163 | { 164 | const Eigen::Vector3d &_split_plane_normal = node->eigenvectors_.col(2); 165 | node = ((query - node->mean_).dot(_split_plane_normal) < double(0)) ? node->left_ : node->right_; 166 | } 167 | 168 | return node; 169 | } 170 | 171 | void MADtree::getLeafs(std::back_insert_iterator> it) 172 | { 173 | if (!left_ && !right_) 174 | { 175 | ++it = this; 176 | return; 177 | } 178 | if (left_) 179 | left_->getLeafs(it); 180 | if (right_) 181 | right_->getLeafs(it); 182 | } 183 | 184 | void MADtree::applyTransform(const Eigen::Matrix3d &r, const Eigen::Vector3d &t) 185 | { 186 | mean_ = r * mean_ + t; 187 | eigenvectors_ = r * eigenvectors_; 188 | if (left_) 189 | left_->applyTransform(r, t); 190 | if (right_) 191 | right_->applyTransform(r, t); 192 | } 193 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/mad_tree.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include "mad_icp/tools/utils.h" 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | class MADtree; 40 | 41 | // some using 42 | using ContainerType = std::vector; 43 | using ContainerTypePtr = ContainerType *; 44 | using IteratorType = typename ContainerType::iterator; 45 | using LeafList = std::vector; 46 | 47 | struct MADtree 48 | { 49 | MADtree(const ContainerTypePtr vec, 50 | const IteratorType begin, 51 | const IteratorType end, 52 | const double b_max, 53 | const double b_min, 54 | const int level, 55 | const int max_parallel_level, 56 | MADtree *parent, 57 | MADtree *plane_predecessor); 58 | 59 | inline ~MADtree() 60 | { 61 | if (left_) 62 | delete left_; 63 | if (right_) 64 | delete right_; 65 | } 66 | 67 | void applyTransform(const Eigen::Matrix3d &r, const Eigen::Vector3d &t); 68 | 69 | const MADtree *bestMatchingLeafFast(const Eigen::Vector3d &query) const; 70 | 71 | void build(const ContainerTypePtr vec, 72 | const IteratorType begin, 73 | const IteratorType end, 74 | const double b_max, 75 | const double b_min, 76 | const int level, 77 | const int max_parallel_level, 78 | MADtree *parent, 79 | MADtree *plane_predecessor); 80 | 81 | static MADtree *makeSubtree(const ContainerTypePtr vec, 82 | const IteratorType begin, 83 | const IteratorType end, 84 | const double b_max, 85 | const double b_min, 86 | const int level, 87 | const int max_parallel_level, 88 | MADtree *parent, 89 | MADtree *plane_predecessor); 90 | 91 | void getLeafs(std::back_insert_iterator> it); 92 | 93 | int num_points_; 94 | bool matched_; 95 | MADtree *left_ = nullptr; 96 | MADtree *right_ = nullptr; 97 | MADtree *parent_ = nullptr; 98 | Eigen::Vector3d mean_; 99 | Eigen::Vector3d bbox_; 100 | Eigen::Matrix3d eigenvectors_; 101 | 102 | protected: 103 | MADtree(){}; 104 | }; 105 | -------------------------------------------------------------------------------- /mad-icp/src/mad_icp/tools/utils.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 R(obots) V(ision) and P(erception) group 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // 1. Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // 9 | // 2. Redistributions in binary form must reproduce the above copyright notice, 10 | // this list of conditions and the following disclaimer in the documentation 11 | // and/or other materials provided with the distribution. 12 | // 13 | // 3. Neither the name of the copyright holder nor the names of its contributors 14 | // may be used to endorse or promote products derived from this software 15 | // without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | using Matrix6d = Eigen::Matrix; 35 | using Vector6d = Eigen::Matrix; 36 | 37 | template 38 | IteratorType split(const IteratorType &begin, const IteratorType &end, const PredicateType &predicate) 39 | { 40 | IteratorType lower = begin; 41 | std::reverse_iterator upper = std::make_reverse_iterator(end); 42 | while (lower != upper.base()) 43 | { 44 | Eigen::Vector3d &v_lower = *lower; 45 | Eigen::Vector3d &v_upper = *upper; 46 | if (predicate(v_lower)) 47 | { 48 | ++lower; 49 | } 50 | else 51 | { 52 | std::swap(v_lower, v_upper); 53 | ++upper; 54 | } 55 | } 56 | return upper.base(); 57 | } 58 | 59 | template 60 | int computeMeanAndCovariance(Eigen::Vector3d &mean, Eigen::Matrix3d &cov, const IteratorType &begin, const IteratorType &end) 61 | { 62 | // mean computed as 1/(end-start) Sum_k={start..end} x_k 63 | // cov computed as (1/(end-start) Sum_k={start..end} x_k* x_k^T ) - mean*mean.transpose(); 64 | mean.setZero(); 65 | cov.setZero(); 66 | int k = 0; 67 | for (IteratorType it = begin; it != end; ++it) 68 | { 69 | const Eigen::Vector3d &v = *it; 70 | mean += v; 71 | cov += v * v.transpose(); 72 | ++k; 73 | } 74 | mean *= (1. / k); 75 | cov *= (1. / k); 76 | cov -= mean * mean.transpose(); 77 | cov *= double(k) / double(k - 1); 78 | 79 | return k; 80 | } 81 | 82 | template 83 | int computeBoundingBox(Eigen::Vector3d &b_max, 84 | const Eigen::Vector3d ¢er, 85 | const Eigen::Matrix3d &R, 86 | const IteratorType &begin, 87 | const IteratorType &end) 88 | { 89 | b_max.setZero(); 90 | int k = 0; 91 | 92 | Eigen::Vector3d bbox_neg = Eigen::Vector3d::Zero(); 93 | Eigen::Vector3d bbox_pos = Eigen::Vector3d::Zero(); 94 | for (IteratorType it = begin; it != end; ++it) 95 | { 96 | const Eigen::Vector3d v = R * (*it - center); 97 | for (size_t i = 0; i < 3; ++i) 98 | { 99 | bbox_neg(i) = std::min(bbox_neg(i), v(i)); 100 | bbox_pos(i) = std::max(bbox_pos(i), v(i)); 101 | } 102 | ++k; 103 | } 104 | 105 | b_max = bbox_pos - bbox_neg; 106 | return k; 107 | } -------------------------------------------------------------------------------- /mad-icp/src/preprocess/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_library(${PROJECT_NAME}.pre 3 | cloud_convert2/cloud_convert2.cc 4 | ) 5 | 6 | target_link_libraries(${PROJECT_NAME}.pre 7 | ${third_party_libs} 8 | ) -------------------------------------------------------------------------------- /mad-icp/src/preprocess/cloud_convert2/cloud_convert2.cc: -------------------------------------------------------------------------------- 1 | #include "cloud_convert2.h" 2 | 3 | #include 4 | #include 5 | // #include 6 | 7 | namespace mad_icp 8 | { 9 | 10 | // void CloudConvert::Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, 11 | // std::vector &cloud_out, std::vector &ts_out) 12 | // { 13 | // AviaHandler(msg); 14 | // cloud_out = cloud_vec; 15 | // ts_out = ts_vec; 16 | // } 17 | 18 | void CloudConvert::Process(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector &cloud_out) 19 | { 20 | switch (param_.lidar_type) 21 | { 22 | case LidarType::OUST64: 23 | Oust64Handler(msg); 24 | break; 25 | 26 | case LidarType::VELO32: 27 | VelodyneHandler(msg); 28 | break; 29 | 30 | case LidarType::ROBOSENSE16: 31 | RobosenseHandler(msg); 32 | break; 33 | 34 | // case LidarType::PANDAR: 35 | // PandarHandler(msg); 36 | // break; 37 | 38 | default: 39 | LOG(ERROR) << "Error LiDAR Type: " << int(lidar_type_); 40 | break; 41 | } 42 | 43 | cloud_out = cloud_vec; 44 | // ts_out = ts_vec; 45 | } 46 | 47 | // void CloudConvert::AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg) 48 | // { 49 | // std::vector().swap(cloud_vec); 50 | // std::vector().swap(ts_vec); 51 | 52 | // scan->timestamp = msg->header.stamp.toNSec(); 53 | // scan->size = msg->point_num; 54 | // scan->points.resize(scan->size); 55 | // static double tm_scale = 1e-9; 56 | // for (size_t j = 0; j < scan->size; ++j) 57 | // { 58 | // scan->points[j].x = msg->points[j].x; 59 | // scan->points[j].y = msg->points[j].y; 60 | // scan->points[j].z = msg->points[j].z; 61 | // scan->points[j].intensity = msg->points[j].reflectivity; 62 | // scan->points[j].ts = 63 | // (double)(scan->timestamp + msg->points[j].offset_time) * tm_scale; 64 | // } 65 | // } 66 | 67 | void CloudConvert::Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg) 68 | { 69 | if (cloud_vec.size() > 0) 70 | { 71 | cloud_vec.clear(); 72 | ts_vec.clear(); 73 | } 74 | 75 | pcl::PointCloud pl_orig; 76 | pcl::fromROSMsg(*msg, pl_orig); 77 | int plsize = pl_orig.size(); 78 | 79 | cloud_vec.reserve(plsize); 80 | ts_vec.reserve(plsize); // FIXME: 存储为相对时间，在(0,1)之间 81 | 82 | // static int idx = 0; 83 | // static std::stringstream os; 84 | // os.str(""); 85 | // os << "/home/cc/catkin_context/src/mad_icp/log/" << idx; 86 | // std::ofstream bin_file(os.str(), std::ios::out | std::ios::binary | std::ios::app); 87 | 88 | static double tm_scale = 1e9; 89 | 90 | double headertime = msg->header.stamp.toSec(); 91 | timespan_ = pl_orig.points.back().t / tm_scale; 92 | // std::cout << "span:" << timespan_ << ",0: " << pl_orig.points[0].t / tm_scale 93 | // << " , 100: " << pl_orig.points[100].t / tm_scale 94 | // << std::endl; 95 | 96 | for (int i = 0; i < pl_orig.points.size(); i++) 97 | { 98 | if (!(std::isfinite(pl_orig.points[i].x) && 99 | std::isfinite(pl_orig.points[i].y) && 100 | std::isfinite(pl_orig.points[i].z))) 101 | continue; 102 | 103 | if (i % param_.point_filter_num != 0) 104 | continue; 105 | 106 | double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + 107 | pl_orig.points[i].z * pl_orig.points[i].z; 108 | if (range > 150 * 150 || range < param_.blind * param_.blind) 109 | continue; 110 | 111 | // bin_file.write((char *)&pl_orig.points[i].x, 3 * sizeof(float)); 112 | // bin_file.write((char *)&pl_orig.points[i].intensity, sizeof(float)); 113 | 114 | cloud_vec.push_back(Eigen::Vector3d(pl_orig.points[i].x, pl_orig.points[i].y, pl_orig.points[i].z)); 115 | ts_vec.push_back(pl_orig.points[i].t / timespan_); 116 | } 117 | // bin_file.close(); 118 | // idx++; 119 | } 120 | 121 | void CloudConvert::VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg) 122 | { 123 | if (cloud_vec.size() > 0) 124 | { 125 | cloud_vec.clear(); 126 | ts_vec.clear(); 127 | } 128 | 129 | pcl::PointCloud pl_orig; 130 | pcl::fromROSMsg(*msg, pl_orig); 131 | int plsize = pl_orig.points.size(); 132 | 133 | cloud_vec.reserve(plsize); 134 | ts_vec.reserve(plsize); 135 | 136 | double headertime = msg->header.stamp.toSec(); // 秒 137 | 138 | static double tm_scale = 1; // 1e6 - nclt kaist or 1 139 | 140 | timespan_ = pl_orig.points.back().time / tm_scale; 141 | 142 | for (int i = 0; i < plsize; i++) 143 | { 144 | if (!(std::isfinite(pl_orig.points[i].x) && 145 | std::isfinite(pl_orig.points[i].y) && 146 | std::isfinite(pl_orig.points[i].z))) 147 | continue; 148 | 149 | double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + 150 | pl_orig.points[i].z * pl_orig.points[i].z; 151 | if (range > 150 * 150 || range < param_.blind * param_.blind) 152 | continue; 153 | 154 | cloud_vec.push_back(Eigen::Vector3d(pl_orig.points[i].x, pl_orig.points[i].y, pl_orig.points[i].z)); 155 | ts_vec.push_back(pl_orig.points[i].time / timespan_); 156 | } 157 | } 158 | 159 | void CloudConvert::RobosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg) 160 | { 161 | if (cloud_vec.size() > 0) 162 | { 163 | cloud_vec.clear(); 164 | ts_vec.clear(); 165 | } 166 | 167 | pcl::PointCloud pl_orig; 168 | pcl::fromROSMsg(*msg, pl_orig); 169 | int plsize = pl_orig.size(); 170 | 171 | cloud_vec.reserve(plsize); 172 | ts_vec.reserve(plsize); 173 | 174 | double headertime = msg->header.stamp.toSec(); 175 | 176 | timespan_ = pl_orig.points.back().timestamp - pl_orig.points[0].timestamp; 177 | 178 | int point_num = 0; 179 | for (int i = 0; i < plsize; i++) 180 | { 181 | if (!(std::isfinite(pl_orig.points[i].x) && 182 | std::isfinite(pl_orig.points[i].y) && 183 | std::isfinite(pl_orig.points[i].z))) 184 | continue; 185 | 186 | double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + 187 | pl_orig.points[i].z * pl_orig.points[i].z; 188 | if (range > 150 * 150 || range < param_.blind * param_.blind) 189 | continue; 190 | 191 | cloud_vec.push_back(Eigen::Vector3d(pl_orig.points[i].x, pl_orig.points[i].y, pl_orig.points[i].z)); 192 | ts_vec.push_back((pl_orig.points[i].timestamp - pl_orig.points[0].timestamp) / timespan_); 193 | } 194 | } 195 | 196 | void CloudConvert::PandarHandler(const sensor_msgs::PointCloud2::ConstPtr &msg) 197 | { 198 | if (cloud_vec.size() > 0) 199 | { 200 | cloud_vec.clear(); 201 | ts_vec.clear(); 202 | } 203 | 204 | pcl::PointCloud pl_orig; 205 | pcl::fromROSMsg(*msg, pl_orig); 206 | int plsize = pl_orig.points.size(); 207 | 208 | cloud_vec.reserve(plsize); 209 | ts_vec.reserve(plsize); 210 | 211 | double headertime = msg->header.stamp.toSec(); 212 | 213 | timespan_ = pl_orig.points.back().timestamp - pl_orig.points[0].timestamp; 214 | 215 | for (int i = 0; i < plsize; i++) 216 | { 217 | if (!(std::isfinite(pl_orig.points[i].x) && 218 | std::isfinite(pl_orig.points[i].y) && 219 | std::isfinite(pl_orig.points[i].z))) 220 | continue; 221 | 222 | if (i % param_.point_filter_num != 0) 223 | continue; 224 | 225 | double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + 226 | pl_orig.points[i].z * pl_orig.points[i].z; 227 | if (range > 150 * 150 || range < blind * blind) 228 | continue; 229 | 230 | cloud_vec.push_back(Eigen::Vector3d(pl_orig.points[i].x, pl_orig.points[i].y, pl_orig.points[i].z)); 231 | ts_vec.push_back((pl_orig.points[i].timestamp - pl_orig.points[0].timestamp) / timespan_); 232 | } 233 | } 234 | 235 | void CloudConvert::initFromConfig(const CVTParam ¶m) 236 | { 237 | param_.point_filter_num = param.point_filter_num; 238 | param_.blind = param.blind; 239 | param_.lidar_type = param.lidar_type; 240 | lidar_type_ = param.lidar_type; 241 | if (param_.lidar_type == LidarType::AVIA) 242 | LOG(INFO) << "Using AVIA Lidar"; 243 | else if (param_.lidar_type == LidarType::VELO32) 244 | LOG(INFO) << "Using Velodyne 32 Lidar"; 245 | else if (param_.lidar_type == LidarType::OUST64) 246 | LOG(INFO) << "Using OUST 64 Lidar"; 247 | else if (param_.lidar_type == LidarType::ROBOSENSE16) 248 | LOG(INFO) << "Using Robosense 16 LIdar"; 249 | else if (param_.lidar_type == LidarType::PANDAR) 250 | LOG(INFO) << "Using Pandar LIdar"; 251 | else 252 | LOG(WARNING) << "unknown lidar_type"; 253 | } 254 | 255 | void CloudConvert::LoadFromYAML(const std::string &yaml_file) 256 | { 257 | auto yaml = YAML::LoadFile(yaml_file); 258 | int lidar_type = yaml["preprocess"]["lidar_type"].as(); 259 | 260 | param_.point_filter_num = yaml["preprocess"]["point_filter_num"].as(); 261 | param_.blind = yaml["preprocess"]["blind"].as(); 262 | 263 | if (lidar_type == 1) 264 | { 265 | lidar_type_ = LidarType::AVIA; 266 | LOG(INFO) << "Using AVIA Lidar"; 267 | } 268 | else if (lidar_type == 2) 269 | { 270 | lidar_type_ = LidarType::VELO32; 271 | LOG(INFO) << "Using Velodyne 32 Lidar"; 272 | } 273 | else if (lidar_type == 3) 274 | { 275 | lidar_type_ = LidarType::OUST64; 276 | LOG(INFO) << "Using OUST 64 Lidar"; 277 | } 278 | else if (lidar_type == 4) 279 | { 280 | lidar_type_ = LidarType::ROBOSENSE16; 281 | LOG(INFO) << "Using Robosense 16 LIdar"; 282 | } 283 | else if (lidar_type == 5) 284 | { 285 | lidar_type_ = LidarType::PANDAR; 286 | LOG(INFO) << "Using Pandar LIdar"; 287 | } 288 | else 289 | { 290 | LOG(WARNING) << "unknown lidar_type"; 291 | } 292 | param_.lidar_type = lidar_type_; 293 | } 294 | 295 | } // namespace zjloc 296 | -------------------------------------------------------------------------------- /mad-icp/src/preprocess/cloud_convert2/cloud_convert2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mad_icp 11 | { 12 | enum class LidarType 13 | { 14 | AVIA = 1, // 大疆的固态雷达 15 | VELO32, // Velodyne 32线 16 | OUST64, // ouster 64线 17 | ROBOSENSE16, // 速腾16线 18 | PANDAR, 19 | }; 20 | 21 | struct CVTParam 22 | { 23 | LidarType lidar_type; 24 | int point_filter_num; 25 | double blind; 26 | 27 | CVTParam() 28 | { 29 | lidar_type = LidarType::AVIA; 30 | point_filter_num = 1; 31 | blind = 0.1; 32 | } 33 | CVTParam(LidarType type, int filter_num, double blind_) 34 | : lidar_type(type), point_filter_num(filter_num), blind(blind_) {} 35 | }; 36 | 37 | /** 38 | * 预处理雷达点云 39 | * 40 | * 将Velodyne, ouster, avia等数据转到FullCloud 41 | * 该类由MessageSync类持有，负责将收到的雷达消息与IMU同步并预处理后，再交给LO/LIO算法 42 | */ 43 | class CloudConvert 44 | { 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | CloudConvert() {} 49 | ~CloudConvert() {} 50 | 51 | /** 52 | * 处理livox avia 点云 53 | * @param msg 54 | * @param pcl_out 55 | */ 56 | // void Process(const livox_ros_driver::CustomMsg::ConstPtr &msg, std::vector &, std::vector &); 57 | 58 | /** 59 | * 处理sensor_msgs::PointCloud2点云 60 | * @param msg 61 | * @param pcl_out 62 | */ 63 | void Process(const sensor_msgs::PointCloud2::ConstPtr &msg, std::vector &cloud_out); 64 | 65 | /// 从YAML中读取参数 66 | void LoadFromYAML(const std::string &yaml); 67 | 68 | void initFromConfig(const CVTParam ¶m); 69 | 70 | // 返回激光的时间 71 | double getTimeSpan() { return timespan_; } 72 | 73 | LidarType lidar_type_ = LidarType::AVIA; // 雷达类型 74 | 75 | private: 76 | // void AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr &msg); 77 | void Oust64Handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 78 | void VelodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); 79 | void RobosenseHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); 80 | void PandarHandler(const sensor_msgs::PointCloud2::ConstPtr &msg); 81 | 82 | std::vector cloud_vec; 83 | std::vector ts_vec; 84 | CVTParam param_; 85 | int point_filter_num_ = 1; // 跳点 86 | double blind = 0.1; 87 | 88 | double timespan_; 89 | }; 90 | } // namespace zjloc -------------------------------------------------------------------------------- /paper_with_supplementary.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/mad-icp/4f255e043b59eb70bdd682270c1c2f925f1d81e5/paper_with_supplementary.pdf --------------------------------------------------------------------------------