├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── config.yaml ├── mapping.yaml ├── rqt.perspective ├── rviz.rviz └── track_landmark.g2o ├── docs └── tag_size_guide.svg ├── include ├── backend │ ├── backend.h │ ├── fixed_lag_backend.h │ └── isam2_backend.h ├── frontend │ ├── tag_detector.h │ ├── tag_detector_cpu.h │ └── tag_detector_cuda.h ├── tag_slam.h ├── tag_slam_zed.h └── utility_function.h ├── launch ├── tagslam.launch ├── tagslam_test.launch ├── zed_sdk.launch ├── zed_sdk_mapping.launch ├── zed_wrapper_cpu.launch └── zed_wrapper_gpu.launch ├── msg ├── AprilTagDetection.msg └── AprilTagDetectionArray.msg ├── nodelet_plugins.xml ├── nvapriltags ├── lib_aarch64_jetpack44 │ └── libapril_tagging.a ├── lib_x86_64_cuda_11_4 │ └── libapril_tagging.a └── nvapriltags │ └── nvAprilTags.h ├── package.xml └── src ├── backend ├── backend.cpp ├── fixed_lag_backend.cpp └── isam2_backend.cpp ├── frontend ├── tag_detector.cpp ├── tag_detector_cpu.cpp └── tag_detector_cuda.cpp ├── tag_slam.cpp ├── tag_slam_zed.cpp ├── tagslam_ros_node.cpp ├── tagslam_ros_zed_node.cpp └── test_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | **/*.bag 2 | .vscode/ 3 | matplotplusplus/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/AprilTagSLAM_ROS/91fdaacc26450293a0c4c7a506ce81e287379885/.gitmodules -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | language: generic # optional, just removes the language badge 5 | services: 6 | - docker 7 | cache: 8 | directories: 9 | - $HOME/.ccache 10 | git: 11 | quiet: true # optional, silences the cloning of the target repository 12 | env: 13 | global: 14 | - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci 15 | matrix: # each line is a job 16 | - ROS_DISTRO="melodic" 17 | - ROS_DISTRO="noetic" 18 | install: 19 | - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci 20 | script: 21 | - .industrial_ci/travis.sh 22 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(tagslam_ros) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | # Set the build type. Options are: 10 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 11 | # Debug : w/ debug symbols, w/o optimization 12 | # Release : w/o debug symbols, w/ optimization 13 | # RelWithDebInfo : w/ debug symbols, w/ optimization 14 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 15 | # We default to 'Release' if none was specified 16 | IF(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 17 | MESSAGE(STATUS "Setting build type to 'Release' as none was specified.") 18 | SET(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the build type" FORCE) 19 | # SET(CMAKE_BUILD_TYPE Debug CACHE STRING "Choose the build type" FORCE) 20 | SET_PROPERTY(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Coverage" "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 21 | ENDIF() 22 | message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" ) 23 | 24 | ########################################### 25 | ###### Set build parameter ################ 26 | ########################################### 27 | option(USE_CUDA "Build with CUDA" ON) 28 | option(USE_ZED "Build with ZED SDK" ON) 29 | option(USE_CUDA_OPENCV "Build with OpenCV CUDA" ON) 30 | 31 | if(NOT USE_CUDA) 32 | # these packages depend on CUDA 33 | SET(USE_ZED OFF) 34 | SET(USE_CUDA_OPENCV OFF) 35 | endif() 36 | 37 | MESSAGE("Use CUDA: " ${USE_CUDA}) 38 | MESSAGE("Use ZED SDK: " ${USE_ZED}) 39 | MESSAGE("Use OpenCV CUDA: " ${USE_CUDA_OPENCV}) 40 | 41 | set(BUILD_SHARED_LIBS ON) 42 | 43 | # add_compile_options("-O3" "-funsafe-loop-optimizations" "-fsee" "-funroll-loops" "-fno-math-errno" "-funsafe-math-optimizations" "-ffinite-math-only" "-fno-signed-zeros") 44 | 45 | # force use -march=native 46 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") 47 | 48 | find_package(catkin REQUIRED COMPONENTS 49 | cmake_modules 50 | cv_bridge 51 | geometry_msgs 52 | nav_msgs 53 | image_geometry 54 | image_transport 55 | message_generation 56 | nodelet 57 | pluginlib 58 | roscpp 59 | sensor_msgs 60 | std_msgs 61 | ) 62 | 63 | find_package(Eigen3 REQUIRED) 64 | find_package(Threads REQUIRED) 65 | find_package(OpenCV REQUIRED) 66 | find_package(apriltag REQUIRED) 67 | find_package(TBB REQUIRED) 68 | find_package(Boost 1.58 REQUIRED COMPONENTS system serialization filesystem thread date_time regex timer chrono) 69 | find_package(GTSAM REQUIRED) 70 | find_package(GTSAM_UNSTABLE REQUIRED) 71 | 72 | # CUDA 73 | if(USE_CUDA) 74 | set(CUDA_MIN_VERSION "10.2") 75 | find_package(CUDA ${CUDA_MIN_VERSION} REQUIRED) 76 | include_directories(include ${CUDA_INCLUDE_DIRS}) 77 | else() 78 | add_compile_definitions(NO_CUDA) 79 | endif() 80 | 81 | # ZED 82 | if(USE_ZED) 83 | find_package(ZED 3) 84 | else() 85 | add_compile_definitions(NO_ZED) 86 | endif() 87 | 88 | if(NOT USE_CUDA_OPENCV) 89 | add_compile_definitions(NO_CUDA_OPENCV) 90 | endif() 91 | 92 | 93 | add_message_files( 94 | FILES 95 | AprilTagDetection.msg 96 | AprilTagDetectionArray.msg 97 | ) 98 | 99 | generate_messages( 100 | DEPENDENCIES 101 | geometry_msgs 102 | sensor_msgs 103 | std_msgs 104 | ) 105 | 106 | # Extract the include directories and libraries from apriltag::apriltag as catkin_package() does not support modern cmake. 107 | get_target_property(apriltag_INCLUDE_DIRS apriltag::apriltag INTERFACE_INCLUDE_DIRECTORIES) 108 | get_target_property(apriltag_LIBRARIES apriltag::apriltag INTERFACE_LINK_LIBRARIES) 109 | 110 | catkin_package( 111 | CATKIN_DEPENDS 112 | cv_bridge 113 | geometry_msgs 114 | image_transport 115 | message_runtime 116 | nodelet 117 | pluginlib 118 | roscpp 119 | sensor_msgs 120 | std_msgs 121 | ) 122 | 123 | ########### 124 | ## Build ## 125 | ########### 126 | 127 | include_directories(include 128 | ${catkin_INCLUDE_DIRS} 129 | ${EIGEN3_INCLUDE_DIRS} 130 | ${OpenCV_INCLUDE_DIRS} 131 | ${Boost_INCLUDE_DIR} 132 | ${GTSAM_INCLUDE_DIR} 133 | ${GTSAM_UNSTABLE_INCLUDE_DIR} 134 | ) 135 | 136 | # ################################ 137 | # ## Add library for the front-end 138 | # ################################ 139 | 140 | # NVAprilTags 141 | if(USE_CUDA) 142 | execute_process(COMMAND uname -m COMMAND tr -d '\n' 143 | OUTPUT_VARIABLE ARCHITECTURE 144 | ) 145 | include_directories(nvapriltags/nvapriltags) 146 | add_library(nvapriltags STATIC IMPORTED) 147 | if( ${ARCHITECTURE} STREQUAL "x86_64" ) 148 | set_property(TARGET nvapriltags PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/nvapriltags/lib_x86_64_cuda_11_4/libapril_tagging.a) 149 | elseif( ${ARCHITECTURE} STREQUAL "aarch64" ) 150 | set_property(TARGET nvapriltags PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a) 151 | endif() 152 | endif() 153 | 154 | 155 | 156 | ################################ 157 | ## Add library for the front-end 158 | ################################ 159 | 160 | if(USE_CUDA) 161 | 162 | add_library(${PROJECT_NAME}_frontend 163 | src/frontend/tag_detector.cpp 164 | src/frontend/tag_detector_cpu.cpp 165 | src/frontend/tag_detector_cuda.cpp) 166 | target_include_directories(${PROJECT_NAME}_frontend PUBLIC 167 | ${CUDA_TOOLKIT_ROOT_DIR}/lib64) 168 | target_link_libraries(${PROJECT_NAME}_frontend 169 | ${catkin_LIBRARIES} 170 | ${OpenCV_LIBRARIES} 171 | ${CUDA_LIBRARIES} 172 | apriltag::apriltag 173 | nvapriltags) 174 | 175 | else() 176 | add_library(${PROJECT_NAME}_frontend 177 | src/frontend/tag_detector.cpp 178 | src/frontend/tag_detector_cpu.cpp) 179 | target_link_libraries(${PROJECT_NAME}_frontend 180 | ${catkin_LIBRARIES} 181 | ${OpenCV_LIBRARIES} 182 | apriltag::apriltag) 183 | endif() 184 | 185 | add_dependencies(${PROJECT_NAME}_frontend ${PROJECT_NAME}_generate_messages_cpp) 186 | 187 | 188 | ################################ 189 | ## Add library for the back-end 190 | ################################ 191 | add_library(${PROJECT_NAME}_backend 192 | src/backend/backend.cpp 193 | src/backend/isam2_backend.cpp 194 | src/backend/fixed_lag_backend.cpp) 195 | add_dependencies(${PROJECT_NAME}_backend ${PROJECT_NAME}_generate_messages_cpp) 196 | target_link_libraries(${PROJECT_NAME}_backend 197 | ${catkin_LIBRARIES} 198 | gtsam 199 | gtsam_unstable 200 | tbb 201 | ${Boost_LIBRARIES} ) 202 | 203 | # ################################ 204 | # ## Add library for the SLAM 205 | # ################################ 206 | 207 | add_library(${PROJECT_NAME}_slam src/tag_slam.cpp) 208 | add_dependencies(${PROJECT_NAME}_slam ${PROJECT_NAME}_generate_messages_cpp) 209 | target_link_libraries(${PROJECT_NAME}_slam 210 | ${PROJECT_NAME}_frontend 211 | ${PROJECT_NAME}_backend 212 | ${catkin_LIBRARIES} ) 213 | 214 | 215 | ################################ 216 | ## ZED Node 217 | ################################ 218 | add_library(${PROJECT_NAME}_slam_zed src/tag_slam_zed.cpp) 219 | add_dependencies(${PROJECT_NAME}_slam_zed ${PROJECT_NAME}_generate_messages_cpp) 220 | target_include_directories(${PROJECT_NAME}_slam_zed PUBLIC 221 | ${ZED_INCLUDE_DIRS}) 222 | target_link_libraries(${PROJECT_NAME}_slam_zed 223 | ${PROJECT_NAME}_frontend 224 | ${PROJECT_NAME}_backend 225 | ${ZED_LIBRARIES} 226 | ${CUDA_LIBRARIES} 227 | ${catkin_LIBRARIES} ) 228 | 229 | ################################ 230 | ## TAG SLAM Node 231 | ################################ 232 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) 233 | target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) 234 | 235 | 236 | ################################ 237 | ## TAG SLAM ZED Node 238 | ################################ 239 | add_executable(${PROJECT_NAME}_zed_node src/${PROJECT_NAME}_zed_node.cpp) 240 | target_link_libraries(${PROJECT_NAME}_zed_node ${catkin_LIBRARIES}) 241 | 242 | ############# 243 | ## Install ## 244 | ############# 245 | 246 | install(DIRECTORY config launch 247 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 248 | ) 249 | install(FILES nodelet_plugins.xml 250 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 251 | ) 252 | 253 | install(TARGETS 254 | ${PROJECT_NAME}_slam 255 | ${PROJECT_NAME}_slam_zed 256 | ${PROJECT_NAME}_node 257 | ${PROJECT_NAME}_zed_node 258 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 259 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 260 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 261 | ) 262 | 263 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # ROS AprilTag SLAM 3 | 4 | This repo contains a ROS implementation of a real-time landmark SLAM based on [AprilTag](https://github.com/AprilRobotics/apriltag) and [GTSAM](https://gtsam.org/). We use AprilTag as the front end, which alleviates the challengings in data association. Both CPU and CUDA versions of the AprilTag front end are included. We provide iSAM2 and batch fixed-lag smoother for SLAM and visual Inertia Odometry. 5 | 6 | The package is initially built for a [ZED2](https://www.stereolabs.com/zed-2/) camera running on [Nvidia Jetson Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-xavier-nx/). However, it can be easily adapted for other sensors and other platforms. We have tested this on JetPack 5.0.2 and regular PC with Ubuntu 20.04, ROS Noetic and Nvidia GTX 1080Ti. 7 | 8 | ## Dependency (Must) 9 | * [Apriltag 3](https://github.com/AprilRobotics/apriltag) 10 | * [ROS Noetic](http://wiki.ros.org/noetic) 11 | * A slightly modified version of [GTSAM 4.1.1](https://github.com/SafeRoboticsLab/gtsam/tree/release-4.1.1) built with *gtsam_unstable*. 12 | ## Dependency (Highly Recommended) 13 | 14 | * **[ZED SDK 3.8](https://www.stereolabs.com/developers/release/)**: 15 | 16 | If ZED SDK is not installed, you will not able to run SLAM that directly interface with ZED SDK. However, you can still run SLAM that subscribes to ROS topics. **If you do not have ZED SDK, You will have to to add `-DUSE_ZED=OFF` flag to the catkin.** 17 | 18 | * **CUDA 11+**: 19 | 20 | If CUDA is not enabled, we will only do SLAM with CPU based CUDA detector. Interface with ZED SDK will be turned off as well. **If you do not have CUDA, You will have to to add `-DUSE_CUDA=OFF` flag to the catkin.** 21 | 22 | *Note*: older JetPack with CUDA 10 should work as well, but we have not tested it. 23 | 24 | * **OpenCV 4.6.0 with CUDA support**: 25 | 26 | 27 | ## Install 28 | First, complete an environment set up on your Ubuntu 20.04 machine like [here](https://github.com/SafeRoboticsLab/PrincetonRaceCar/tree/SP2025/Jetson_Setup). This will install 29 | - [x] [ros-noetic ](http://wiki.ros.org/noetic/Installation/Ubuntu) 30 | - [x] [zed-sdk](https://www.stereolabs.com/developers/release/) 31 | - [x] [GTSAM Release 4.1.1](https://gtsam.org/build/) 32 | - [x] RoboStack (conda ROS environment) using a PySpline compiled .whl file 33 | 34 | In the last step `source set_startup_package` we build a `~/StartUp` directory similar to below. However, this also includes the [`Maestro-Controller-ROS`](https://github.com/SafeRoboticsLab/Maestro-Controller-ROS) repository. To build `AprilTagSLAM_ROS` on 35 | its own, follow these instructions: 36 | 37 | 38 | ``` 39 | mkdir tagslam_ws 40 | cd tagslam_ws 41 | mkdir src 42 | cd src 43 | 44 | git clone https://github.com/SafeRoboticsLab/AprilTagSLAM_ROS.git 45 | cd AprilTagSLAM_ROS 46 | git checkout sdk_v4 47 | cd .. 48 | 49 | git clone https://github.com/AprilRobotics/apriltag.git 50 | 51 | # We need compile cv_bridge from source files to avoid mixing OpenCV versions 52 | git clone https://github.com/ros-perception/vision_opencv.git 53 | cd vision_opencv 54 | git checkout noetic 55 | cd ../.. 56 | 57 | # build 58 | source /opt/ros/noetic/setup.bash 59 | catkin_make_isolated 60 | ``` 61 | 62 | ## Run with ZED SDK 63 | Checkout the settings in [config/config.yaml](config/config.yaml). 64 | 65 | ``` 66 | source devel_isolated/setup.bash 67 | roslaunch tagslam_ros zed_sdk.launch 68 | ``` 69 | -------------------------------------------------------------------------------- /config/config.yaml: -------------------------------------------------------------------------------- 1 | ############################################# 2 | ### Parameters only use zed sdk directly #### 3 | ############################################# 4 | camera: 5 | camera_model: zed2 # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) 6 | frame_rate: 15 # Camera frame rate (can be overwritten by the launch file) 7 | resolution: 0 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA 8 | 9 | depth: 10 | quality: 1 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL 11 | sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) 12 | min_depth: 0.5 # Minimum depth in meters 13 | max_depth: 15.0 # Maximum depth in meters 14 | 15 | sensors: 16 | sensor_rate: 200 # Maximum rate at which the sensors data are published 17 | 18 | ################################# 19 | ### Parameters for the slam #### 20 | ################################# 21 | 22 | publish: 23 | # ros publishing 24 | publish_image: true # Publish the camera image 25 | publish_image_with_tags: true # Publish the camera image with the tags detected 26 | publish_tags: true # Publish the tags detected 27 | publish_landmarks: true # Publish the landmarks detected 28 | publish_latency: true # Publish the latency between the camera and the computer 29 | use_approx_sync: false # Allow ROS subscriber to use approximate sync 30 | frontend: 31 | type: "GPU" # "CPU" or "GPU" 32 | # AprilTag parameters 33 | tag_size: 0.262 # Size of the tag in meters, default: 0.1 m 34 | 35 | # CPU detector only parameters 36 | tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 37 | tag_threads: 4 # default: 2 38 | tag_decimate: 4.0 # default: 1.0 39 | tag_blur: 1.0 # default: 0.0 40 | tag_refine_edges: 1 # default: 1 41 | tag_debug: 0 # default: 0 42 | max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) 43 | 44 | # GPU detector only parameters 45 | max_tags: 20 46 | 47 | backend: 48 | #smoother parameter 49 | smoother: "fixed_lag" # choose from "fixed_lag", "isam2", and "none" 50 | use_odom: true # use ZED's odometry as motion priors, it can work with or without imu 51 | use_imu: true # use ZED's imu as motion priors, it can work with or without odometry 52 | 53 | # Load and Save Maps 54 | prior_map: true 55 | save_graph: false 56 | load_path: "" # prior map 57 | save_path: "" # default: "", system will automatically create file with timestamp in ~/.ros folder 58 | fix_prior: true 59 | pose_offset: [1.0, 0.0, 0.0, -0.357, # camera center offset 60 | 0.0, 1.0, 0.0, -0.06, 61 | 0.0, 0.0, 1.0, 0, #wheelbase 62 | 0.0, 0.0, 0.0, 1.0] # camera to rear axis transform 63 | 64 | ################# Uncertainty parameters #################### 65 | 66 | # variance of tag pose estimation 67 | landmark_sigma_trans: 0.1 68 | landmark_sigma_rot: 0.2 69 | 70 | # variance of prior landmark loaded from file 71 | #(smaller value to enforce the landmark are not updated) 72 | landmark_prior_sigma_trans: 0.01 73 | landmark_prior_sigma_rot: 0.05 74 | 75 | # variance of initial pose if no map is loaded 76 | pose_prior_sigma_trans: 0.1 77 | pose_prior_sigma_rot: 0.3 78 | 79 | # ** VIO only parameters ** 80 | # variance of velocity assosciated with the first pose 81 | # we initialize the velocity at 0 82 | vel_prior_sigma: 0.2 83 | 84 | # variance of imu bias assosciated with the first pose 85 | # we initialize the bias with 0 86 | bias_prior_sigma: 0.2 87 | 88 | ################# Optimization Parameters #################### 89 | 90 | # isam2 parameters 91 | relinearize_threshold: 0.1 92 | relinearize_skip: 1 93 | optimizer: "GaussNewton" 94 | cacheLinearizedFactors: true 95 | 96 | # LM parameter for fixed_lag smoother 97 | lag: 0.2 # parameter for fix lag only 98 | lambda_initial: 1e-5 99 | lambda_upper_bound: 100.0 100 | lambda_lower_bound: 1e-10 101 | lambda_factor: 10 102 | max_iterations: 10 103 | error_tol: 1e-5 104 | relative_error_tol: 1e-1 105 | absolute_error_tol: 1e-3 106 | 107 | # if this is true, we will accept the local optimal solution 108 | # when the error no longer decrease rather than increase the lambda 109 | local_optimal: true 110 | 111 | ################################# 112 | ### Parameters for the tags #### 113 | ################################# 114 | 115 | # The tags are defined in the following format: 116 | # { 117 | # id_start: beginning id (included)of a list of tags 118 | # id_end: end id (include) of a list of tags 119 | # tag_size: 0.2, # size of the tag in meters 120 | # } 121 | 122 | # The tags are used to define the landmarks 123 | landmark_tags: [ 124 | { 125 | id_start: 1, # include this tag 126 | id_end: 1, # include this tag 127 | tag_size: 0.254, # size of the tag in meters 128 | }, 129 | 130 | { 131 | id_start: 2, # include this tag 132 | id_end: 26, # include this tag 133 | tag_size: 0.259, # size of the tag in meters 134 | }, 135 | { 136 | id_start: 100, # include this tag 137 | id_end: 103, # include this tag 138 | tag_size: 0.12, # size of the tag in meters 139 | } 140 | 141 | ] 142 | 143 | # tags are ignored during slam 144 | ignore_tags: [ 145 | { 146 | id_start: 27, # include this tag 147 | id_end: 99, # include this tag 148 | tag_size: 0.2, # size of the tag in meters 149 | }, 150 | { 151 | id_start: 104, # include this tag 152 | id_end: 10000, # include this tag 153 | tag_size: 0.2, # size of the tag in meters 154 | }, 155 | ] -------------------------------------------------------------------------------- /config/mapping.yaml: -------------------------------------------------------------------------------- 1 | ############################################# 2 | ### Parameters only use zed sdk directly #### 3 | ############################################# 4 | camera: 5 | camera_model: zed2 # A name for the camera (can be different from camera model and node name and can be overwritten by the launch file) 6 | frame_rate: 15 # Camera frame rate (can be overwritten by the launch file) 7 | resolution: 0 # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA 8 | 9 | depth: 10 | quality: 2 # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA, '4': NEURAL 11 | sensing_mode: 0 # '0': STANDARD, '1': FILL (not use FILL for robotic applications) 12 | min_depth: 0.5 # Minimum depth in meters 13 | max_depth: 15.0 # Maximum depth in meters 14 | 15 | sensors: 16 | sensor_rate: 200 # Maximum rate at which the sensors data are published 17 | 18 | ################################# 19 | ### Parameters for the slam #### 20 | ################################# 21 | 22 | publish: 23 | # ros publishing 24 | publish_image: true # Publish the camera image 25 | publish_image_with_tags: true # Publish the camera image with the tags detected 26 | publish_tags: true # Publish the tags detected 27 | publish_landmarks: true # Publish the landmarks detected 28 | publish_latency: true # Publish the latency between the camera and the computer 29 | use_approx_sync: false # Allow ROS subscriber to use approximate sync 30 | frontend: 31 | type: "CPU" # "CPU" or "GPU" 32 | # AprilTag parameters 33 | tag_size: 0.241 # Size of the tag in meters, default: 0.1 m 34 | 35 | # CPU detector only parameters 36 | tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 37 | tag_threads: 4 # default: 2 38 | tag_decimate: 1.0 # default: 1.0 39 | tag_blur: 0.0 # default: 0.0 40 | tag_refine_edges: 1 # default: 1 41 | tag_debug: 0 # default: 0 42 | max_hamming_dist: 2 # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.) 43 | 44 | # GPU detector only parameters 45 | max_tags: 20 46 | 47 | backend: 48 | #smoother parameter 49 | smoother: "isam2" # choose from "fixed_lag", "isam2", and "none" 50 | use_odom: true # use ZED's odometry as motion priors, it can work with or without imu 51 | use_imu: false # use ZED's imu as motion priors, it can work with or without odometry 52 | 53 | # Load and Save Maps 54 | prior_map: true 55 | save_graph: true 56 | load_path: "track_prior.g2o" # prior map 57 | save_path: "track4.g2o" # default: "", system will automatically create file with timestamp in ~/.ros folder 58 | fix_prior: false 59 | # pose_offset: [1.0, 0.0, 0.0, -0.357, # camera center offset 60 | # 0.0, 1.0, 0.0, -0.06, 61 | # 0.0, 0.0, 1.0, 0, #wheelbase 62 | # 0.0, 0.0, 0.0, 1.0] # camera to rear axis transform 63 | pose_offset: [1.0, 0.0, 0.0, 0, # camera center offset 64 | 0.0, 1.0, 0.0, 0, 65 | 0.0, 0.0, 1.0, 0, #wheelbase 66 | 0.0, 0.0, 0.0, 1.0] # camera to rear axis transform 67 | 68 | ################# Uncertainty parameters #################### 69 | 70 | # variance of tag pose estimation 71 | landmark_sigma_trans: 0.05 72 | landmark_sigma_rot: 0.1 73 | 74 | # variance of prior landmark loaded from file 75 | #(smaller value to enforce the landmark are not updated) 76 | landmark_prior_sigma_trans: 0.01 77 | landmark_prior_sigma_rot: 0.05 78 | 79 | # variance of initial pose if no map is loaded 80 | pose_prior_sigma_trans: 0.1 81 | pose_prior_sigma_rot: 0.2 82 | 83 | # ** VIO only parameters ** 84 | # variance of velocity assosciated with the first pose 85 | # we initialize the velocity at 0 86 | vel_prior_sigma: 0.2 87 | 88 | # variance of imu bias assosciated with the first pose 89 | # we initialize the bias with 0 90 | bias_prior_sigma: 0.2 91 | 92 | ################# Optimization Parameters #################### 93 | 94 | # isam2 parameters 95 | relinearize_threshold: 0.1 96 | relinearize_skip: 1 97 | optimizer: "GaussNewton" 98 | cacheLinearizedFactors: true 99 | 100 | # LM parameter for fixed_lag smoother 101 | lag: 0.2 # parameter for fix lag only 102 | lambda_initial: 1e-5 103 | lambda_upper_bound: 100.0 104 | lambda_lower_bound: 1e-10 105 | lambda_factor: 10 106 | max_iterations: 10 107 | error_tol: 1e-5 108 | relative_error_tol: 1e-1 109 | absolute_error_tol: 1e-3 110 | 111 | # if this is true, we will accept the local optimal solution 112 | # when the error no longer decrease rather than increase the lambda 113 | local_optimal: true 114 | 115 | ################################# 116 | ### Parameters for the tags #### 117 | ################################# 118 | 119 | # The tags are defined in the following format: 120 | # { 121 | # id_start: beginning id (included)of a list of tags 122 | # id_end: end id (include) of a list of tags 123 | # tag_size: 0.2, # size of the tag in meters 124 | # } 125 | 126 | # The tags are used to define the landmarks 127 | landmark_tags: [ 128 | { 129 | id_start: 1, # include this tag 130 | id_end: 1, # include this tag 131 | tag_size: 0.254, # size of the tag in meters 132 | }, 133 | 134 | { 135 | id_start: 2, # include this tag 136 | id_end: 26, # include this tag 137 | tag_size: 0.259, # size of the tag in meters 138 | }, 139 | { 140 | id_start: 100, # include this tag 141 | id_end: 103, # include this tag 142 | tag_size: 0.12, # size of the tag in meters 143 | } 144 | 145 | ] 146 | 147 | # tags are ignored during slam 148 | ignore_tags: [ 149 | { 150 | id_start: 27, # include this tag 151 | id_end: 99, # include this tag 152 | tag_size: 0.2, # size of the tag in meters 153 | }, 154 | { 155 | id_start: 104, # include this tag 156 | id_end: 10000, # include this tag 157 | tag_size: 0.2, # size of the tag in meters 158 | }, 159 | ] -------------------------------------------------------------------------------- /config/rqt.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "mainwindow": { 5 | "keys": { 6 | "geometry": { 7 | "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0003000000000080ffffffec00000730000003020000008000000036000007300000030200000000000000000f0000000080000000360000073000000302')", 8 | "type": "repr(QByteArray.hex)", 9 | "pretty-print": " 0 6 0 6 0 " 10 | }, 11 | "state": { 12 | "repr(QByteArray.hex)": "QtCore.QByteArray(b'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')", 13 | "type": "repr(QByteArray.hex)", 14 | "pretty-print": " Lrqt_tf_tree__RosTfTree__1__RosTfTreeUi Brqt_srv__Services__1__messages.ui &rqt_rviz__RViz__1__ Brqt_nav_view__NavigationView__1__ ' | _ " 15 | } 16 | }, 17 | "groups": { 18 | "toolbar_areas": { 19 | "keys": { 20 | "MinimizedDockWidgetsToolbar": { 21 | "repr": "8", 22 | "type": "repr" 23 | } 24 | }, 25 | "groups": {} 26 | } 27 | } 28 | }, 29 | "pluginmanager": { 30 | "keys": { 31 | "running-plugins": { 32 | "repr": "{'rqt_plot/Plot': [1, 2], 'rqt_service_caller/ServiceCaller': [1]}", 33 | "type": "repr" 34 | } 35 | }, 36 | "groups": { 37 | "plugin__rqt_graph__RosGraph__1": { 38 | "keys": {}, 39 | "groups": { 40 | "dock_widget__RosGraphUi": { 41 | "keys": { 42 | "dock_widget_title": { 43 | "repr": "'Node Graph'", 44 | "type": "repr" 45 | }, 46 | "dockable": { 47 | "repr": "'true'", 48 | "type": "repr" 49 | }, 50 | "parent": { 51 | "repr": "None", 52 | "type": "repr" 53 | } 54 | }, 55 | "groups": {} 56 | }, 57 | "plugin": { 58 | "keys": { 59 | "actionlib_check_box_state": { 60 | "repr": "'true'", 61 | "type": "repr" 62 | }, 63 | "auto_fit_graph_check_box_state": { 64 | "repr": "'true'", 65 | "type": "repr" 66 | }, 67 | "dead_sinks_check_box_state": { 68 | "repr": "'true'", 69 | "type": "repr" 70 | }, 71 | "filter_line_edit_text": { 72 | "repr": "'/'", 73 | "type": "repr" 74 | }, 75 | "graph_type_combo_box_index": { 76 | "repr": "'0'", 77 | "type": "repr" 78 | }, 79 | "group_image_check_box_state": { 80 | "repr": "'true'", 81 | "type": "repr" 82 | }, 83 | "group_tf_check_box_state": { 84 | "repr": "'true'", 85 | "type": "repr" 86 | }, 87 | "hide_dynamic_reconfigure_check_box_state": { 88 | "repr": "'true'", 89 | "type": "repr" 90 | }, 91 | "hide_tf_nodes_check_box_state": { 92 | "repr": "'false'", 93 | "type": "repr" 94 | }, 95 | "highlight_connections_check_box_state": { 96 | "repr": "'true'", 97 | "type": "repr" 98 | }, 99 | "leaf_topics_check_box_state": { 100 | "repr": "'true'", 101 | "type": "repr" 102 | }, 103 | "namespace_cluster_spin_box_value": { 104 | "repr": "'2'", 105 | "type": "repr" 106 | }, 107 | "quiet_check_box_state": { 108 | "repr": "'true'", 109 | "type": "repr" 110 | }, 111 | "topic_filter_line_edit_text": { 112 | "repr": "'/'", 113 | "type": "repr" 114 | }, 115 | "unreachable_check_box_state": { 116 | "repr": "'true'", 117 | "type": "repr" 118 | } 119 | }, 120 | "groups": {} 121 | } 122 | } 123 | }, 124 | "plugin__rqt_plot__Plot__1": { 125 | "keys": {}, 126 | "groups": { 127 | "dock_widget__DataPlotWidget": { 128 | "keys": { 129 | "dock_widget_title": { 130 | "repr": "'MatPlot'", 131 | "type": "repr" 132 | }, 133 | "dockable": { 134 | "repr": "True", 135 | "type": "repr" 136 | }, 137 | "parent": { 138 | "repr": "None", 139 | "type": "repr" 140 | } 141 | }, 142 | "groups": {} 143 | }, 144 | "plugin": { 145 | "keys": { 146 | "autoscroll": { 147 | "repr": "True", 148 | "type": "repr" 149 | }, 150 | "plot_type": { 151 | "repr": "1", 152 | "type": "repr" 153 | }, 154 | "topics": { 155 | "repr": "['/SLAM/Debug/Convert/data', '/SLAM/Debug/Optimize/data', '/SLAM/Debug/Detect/data', '/SLAM/Debug/Total/data']", 156 | "type": "repr" 157 | }, 158 | "x_limits": { 159 | "repr": "[0.0, 1.0]", 160 | "type": "repr" 161 | }, 162 | "y_limits": { 163 | "repr": "[0.0, 126.20262145996094]", 164 | "type": "repr" 165 | } 166 | }, 167 | "groups": {} 168 | } 169 | } 170 | }, 171 | "plugin__rqt_plot__Plot__2": { 172 | "keys": {}, 173 | "groups": { 174 | "dock_widget__DataPlotWidget": { 175 | "keys": { 176 | "dock_widget_title": { 177 | "repr": "'MatPlot (2)'", 178 | "type": "repr" 179 | }, 180 | "dockable": { 181 | "repr": "True", 182 | "type": "repr" 183 | }, 184 | "parent": { 185 | "repr": "None", 186 | "type": "repr" 187 | } 188 | }, 189 | "groups": {} 190 | }, 191 | "plugin": { 192 | "keys": { 193 | "autoscroll": { 194 | "repr": "True", 195 | "type": "repr" 196 | }, 197 | "plot_type": { 198 | "repr": "1", 199 | "type": "repr" 200 | }, 201 | "topics": { 202 | "repr": "['/SLAM/Pose/pose/pose/position/x', '/SLAM/Pose/pose/pose/position/y']", 203 | "type": "repr" 204 | }, 205 | "x_limits": { 206 | "repr": "[0.0, 1.0]", 207 | "type": "repr" 208 | }, 209 | "y_limits": { 210 | "repr": "[0.0, 10.0]", 211 | "type": "repr" 212 | } 213 | }, 214 | "groups": {} 215 | } 216 | } 217 | }, 218 | "plugin__rqt_reconfigure__Param__1": { 219 | "keys": {}, 220 | "groups": { 221 | "dock_widget___plugincontainer_top_widget": { 222 | "keys": { 223 | "dock_widget_title": { 224 | "repr": "'Dynamic Reconfigure'", 225 | "type": "repr" 226 | }, 227 | "dockable": { 228 | "repr": "'true'", 229 | "type": "repr" 230 | }, 231 | "parent": { 232 | "repr": "None", 233 | "type": "repr" 234 | } 235 | }, 236 | "groups": {} 237 | }, 238 | "plugin": { 239 | "keys": { 240 | "_splitter": { 241 | "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000001000000020000013b000000640100000009010000000200')", 242 | "type": "repr(QByteArray.hex)", 243 | "pretty-print": " ; d " 244 | }, 245 | "expanded_nodes": { 246 | "repr": "None", 247 | "type": "repr" 248 | }, 249 | "selected_nodes": { 250 | "repr": "None", 251 | "type": "repr" 252 | }, 253 | "splitter": { 254 | "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000001000000020000010f0000007301ffffffff010000000100')", 255 | "type": "repr(QByteArray.hex)", 256 | "pretty-print": " s " 257 | }, 258 | "text": { 259 | "repr": "'a'", 260 | "type": "repr" 261 | } 262 | }, 263 | "groups": {} 264 | } 265 | } 266 | }, 267 | "plugin__rqt_rviz__RViz__1": { 268 | "keys": {}, 269 | "groups": { 270 | "dock_widget__": { 271 | "keys": { 272 | "dock_widget_title": { 273 | "repr": "'config.rviz[*] - RViz'", 274 | "type": "repr" 275 | }, 276 | "dockable": { 277 | "repr": "'true'", 278 | "type": "repr" 279 | }, 280 | "parent": { 281 | "repr": "None", 282 | "type": "repr" 283 | } 284 | }, 285 | "groups": {} 286 | }, 287 | "plugin": { 288 | "keys": { 289 | "hide_menu": { 290 | "repr": "'false'", 291 | "type": "repr" 292 | }, 293 | "rviz_config_file": { 294 | "repr": "'/home/zixu/mambaforge/envs/ros_base/share/rviz/default.rviz'", 295 | "type": "repr" 296 | } 297 | }, 298 | "groups": {} 299 | } 300 | } 301 | }, 302 | "plugin__rqt_service_caller__ServiceCaller__1": { 303 | "keys": {}, 304 | "groups": { 305 | "dock_widget__ServiceCallerWidget": { 306 | "keys": { 307 | "dock_widget_title": { 308 | "repr": "'Service Caller'", 309 | "type": "repr" 310 | }, 311 | "dockable": { 312 | "repr": "True", 313 | "type": "repr" 314 | }, 315 | "parent": { 316 | "repr": "None", 317 | "type": "repr" 318 | } 319 | }, 320 | "groups": {} 321 | }, 322 | "plugin": { 323 | "keys": { 324 | "current_service_name": { 325 | "repr": "'/SLAM/start_slam'", 326 | "type": "repr" 327 | }, 328 | "splitter_orientation": { 329 | "repr": "2", 330 | "type": "repr" 331 | } 332 | }, 333 | "groups": {} 334 | } 335 | } 336 | }, 337 | "plugin__rqt_shell__Shell__1": { 338 | "keys": {}, 339 | "groups": { 340 | "plugin": { 341 | "keys": { 342 | "shell_type": { 343 | "repr": "'2'", 344 | "type": "repr" 345 | } 346 | }, 347 | "groups": {} 348 | } 349 | } 350 | }, 351 | "plugin__rqt_srv__Services__1": { 352 | "keys": {}, 353 | "groups": { 354 | "dock_widget__messages.ui": { 355 | "keys": { 356 | "dock_widget_title": { 357 | "repr": "'Service Type Browser'", 358 | "type": "repr" 359 | }, 360 | "dockable": { 361 | "repr": "'true'", 362 | "type": "repr" 363 | }, 364 | "parent": { 365 | "repr": "None", 366 | "type": "repr" 367 | } 368 | }, 369 | "groups": {} 370 | } 371 | } 372 | }, 373 | "plugin__rqt_tf_tree__RosTfTree__1": { 374 | "keys": {}, 375 | "groups": { 376 | "dock_widget__RosTfTreeUi": { 377 | "keys": { 378 | "dock_widget_title": { 379 | "repr": "'TF Tree'", 380 | "type": "repr" 381 | }, 382 | "dockable": { 383 | "repr": "'true'", 384 | "type": "repr" 385 | }, 386 | "parent": { 387 | "repr": "None", 388 | "type": "repr" 389 | } 390 | }, 391 | "groups": {} 392 | }, 393 | "plugin": { 394 | "keys": { 395 | "auto_fit_graph_check_box_state": { 396 | "repr": "'true'", 397 | "type": "repr" 398 | }, 399 | "highlight_connections_check_box_state": { 400 | "repr": "'true'", 401 | "type": "repr" 402 | } 403 | }, 404 | "groups": {} 405 | } 406 | } 407 | }, 408 | "plugin__rqt_top__TOP__1": { 409 | "keys": {}, 410 | "groups": { 411 | "plugin": { 412 | "keys": { 413 | "filter_text": { 414 | "repr": "''", 415 | "type": "repr" 416 | }, 417 | "is_regex": { 418 | "repr": "'0'", 419 | "type": "repr" 420 | } 421 | }, 422 | "groups": {} 423 | } 424 | } 425 | } 426 | } 427 | } 428 | } 429 | } -------------------------------------------------------------------------------- /config/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Image1 10 | - /MarkerArray1 11 | - /Odometry1 12 | - /Odometry1/Shape1 13 | Splitter Ratio: 0.5 14 | Tree Height: 1464 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: Image 33 | Preferences: 34 | PromptSaveOnExit: true 35 | Toolbars: 36 | toolButtonStyle: 2 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: false 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 10 56 | Reference Frame: 57 | Value: false 58 | - Class: rviz/Image 59 | Enabled: false 60 | Image Topic: /SLAM/Tag_Detection_Image 61 | Max Value: 1 62 | Median window: 5 63 | Min Value: 0 64 | Name: Image 65 | Normalize Range: true 66 | Queue Size: 2 67 | Transport Hint: raw 68 | Unreliable: false 69 | Value: false 70 | - Class: rviz/MarkerArray 71 | Enabled: true 72 | Marker Topic: /SLAM/Landmarks 73 | Name: MarkerArray 74 | Namespaces: 75 | {} 76 | Queue Size: 100 77 | Value: true 78 | - Angle Tolerance: 0.10000000149011612 79 | Class: rviz/Odometry 80 | Covariance: 81 | Orientation: 82 | Alpha: 0.5 83 | Color: 255; 255; 127 84 | Color Style: Unique 85 | Frame: Local 86 | Offset: 1 87 | Scale: 1 88 | Value: true 89 | Position: 90 | Alpha: 0.30000001192092896 91 | Color: 204; 51; 204 92 | Scale: 1 93 | Value: true 94 | Value: false 95 | Enabled: true 96 | Keep: 1 97 | Name: Odometry 98 | Position Tolerance: 0.10000000149011612 99 | Queue Size: 10 100 | Shape: 101 | Alpha: 0.5 102 | Axes Length: 1 103 | Axes Radius: 0.10000000149011612 104 | Color: 255; 25; 0 105 | Head Length: 0.05000000074505806 106 | Head Radius: 0.03999999910593033 107 | Shaft Length: 0.10000000149011612 108 | Shaft Radius: 0.019999999552965164 109 | Value: Arrow 110 | Topic: /SLAM/Pose 111 | Unreliable: false 112 | Value: true 113 | - Class: rviz/MarkerArray 114 | Enabled: true 115 | Marker Topic: /Routing/Map 116 | Name: MarkerArray 117 | Namespaces: 118 | lanelet: true 119 | Queue Size: 100 120 | Value: true 121 | Enabled: true 122 | Global Options: 123 | Background Color: 48; 48; 48 124 | Default Light: true 125 | Fixed Frame: map 126 | Frame Rate: 30 127 | Name: root 128 | Tools: 129 | - Class: rviz/Interact 130 | Hide Inactive Objects: true 131 | - Class: rviz/MoveCamera 132 | - Class: rviz/Select 133 | - Class: rviz/FocusCamera 134 | - Class: rviz/Measure 135 | - Class: rviz/SetInitialPose 136 | Theta std deviation: 0.2617993950843811 137 | Topic: /initialpose 138 | X std deviation: 0.5 139 | Y std deviation: 0.5 140 | - Class: rviz/SetGoal 141 | Topic: /move_base_simple/goal 142 | - Class: rviz/PublishPoint 143 | Single click: true 144 | Topic: /clicked_point 145 | Value: true 146 | Views: 147 | Current: 148 | Angle: 0 149 | Class: rviz/TopDownOrtho 150 | Enable Stereo Rendering: 151 | Stereo Eye Separation: 0.05999999865889549 152 | Stereo Focal Distance: 1 153 | Swap Stereo Eyes: false 154 | Value: false 155 | Invert Z Axis: false 156 | Name: Current View 157 | Near Clip Distance: 0.009999999776482582 158 | Scale: 227.94485473632812 159 | Target Frame: 160 | X: 3 161 | Y: 3 162 | Saved: ~ 163 | Window Geometry: 164 | Displays: 165 | collapsed: false 166 | Height: 2032 167 | Hide Left Dock: false 168 | Hide Right Dock: false 169 | Image: 170 | collapsed: false 171 | QMainWindow State: 000000ff00000000fd0000000400000000000002ab0000073bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000005d0000073b00000000000000000000000100000456000006eafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000006e000006ea000001bc01000039fa000000010100000004fb0000000a0049006d0061006700650100000000ffffffff0000009100fffffffb000000100044006900730070006c0061007900730100000000ffffffff000001f700fffffffb0000000a0049006d0061006700650100000000ffffffff0000000000000000fb0000000a00560069006500770073010000127f000001390000015f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e800000005efc0100000002fb0000000800540069006d0065010000000000000e80000006dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000a1e000006ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 172 | Selection: 173 | collapsed: false 174 | Time: 175 | collapsed: false 176 | Tool Properties: 177 | collapsed: false 178 | Views: 179 | collapsed: false 180 | Width: 3712 181 | X: 128 182 | Y: 54 183 | -------------------------------------------------------------------------------- /config/track_landmark.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT l 1 7.06882 1.09942 0.545102 -0.506737 0.511255 0.490918 -0.490751 2 | VERTEX_SE3:QUAT l 2 6.24981 -0.0951828 0.366506 0.488192 -0.506553 -0.503282 0.501776 3 | VERTEX_SE3:QUAT l 3 1.42467 -0.63375 0.392011 0.00073936 0.711244 0.702932 0.00422009 4 | VERTEX_SE3:QUAT l 4 6.76828 6.84053 0.58412 0.708032 0.000799073 -0.0112181 0.706091 5 | VERTEX_SE3:QUAT l 5 3.14964 6.94253 0.42475 0.710203 0.0324996 -0.00384386 0.703236 6 | VERTEX_SE3:QUAT l 6 1.36753 6.62312 0.393934 0.493248 0.516079 0.480339 0.509552 7 | VERTEX_SE3:QUAT l 7 -0.460469 4.90812 0.433766 0.514365 0.50093 0.50275 0.481394 8 | VERTEX_SE3:QUAT l 8 -0.278369 -0.635599 0.349607 -0.00720584 0.710294 0.703742 0.0132815 9 | VERTEX_SE3:QUAT l 9 4.30139 6.62501 0.395812 -0.500913 0.498044 0.51455 -0.486083 10 | VERTEX_SE3:QUAT l 10 7.0371 5.45021 0.509318 0.505935 -0.492819 -0.50524 0.495875 11 | VERTEX_SE3:QUAT l 11 6.45159 0.101219 0.47168 0.0163124 0.709521 0.704334 -0.0150749 12 | VERTEX_SE3:QUAT l 12 5.08674 -0.684363 0.473753 -0.0026624 0.710753 0.703411 0.00606203 13 | VERTEX_SE3:QUAT l 13 -0.459362 0.386763 0.341008 0.502203 0.500273 0.495691 0.501807 14 | VERTEX_SE3:QUAT l 14 3.92761 -0.564377 0.440138 -0.00710646 0.711997 0.702086 0.00921721 15 | VERTEX_SE3:QUAT l 15 -0.446682 2.62076 0.422506 0.494704 0.514223 0.486295 0.504341 16 | VERTEX_SE3:QUAT l 16 1.72629 6.92566 0.409325 0.707968 0.00234754 0.0161759 0.706055 17 | VERTEX_SE3:QUAT l 17 -0.449527 1.25505 0.418812 0.505372 0.501452 0.495853 0.497267 18 | VERTEX_SE3:QUAT l 18 -0.454269 3.78421 0.417875 0.499735 0.51143 0.495075 0.493563 19 | VERTEX_SE3:QUAT l 19 6.5559 0.110118 0.872503 0.482496 0.520047 0.521519 -0.474095 20 | VERTEX_SE3:QUAT l 20 6.51466 6.80617 0.987381 0.4873 0.52895 -0.502441 0.479899 21 | VERTEX_SE3:QUAT l 21 6.21465 -0.233991 0.837903 0.000726019 -0.712129 0.00942543 0.701985 22 | VERTEX_SE3:QUAT l 22 0.588526 -0.635257 0.470238 0.00810281 0.711346 0.702795 -0.000788687 23 | VERTEX_SE3:QUAT l 23 -0.464314 5.80382 0.329929 0.709649 0.0209944 0.704242 0.000986485 24 | VERTEX_SE3:QUAT l 24 3.86683 6.92807 0.539096 0.71271 0.0195911 0.00464524 0.70117 25 | VERTEX_SE3:QUAT l 25 7.0626 3.10409 0.528481 0.499736 -0.496996 -0.500258 0.502992 26 | VERTEX_SE3:QUAT l 26 2.20771 6.93894 0.487545 0.704428 0.00744935 0.00976569 0.709669 27 | VERTEX_SE3:QUAT l 100 5.94973 5.98168 -0.000648397 -0.0041153 -0.00031874 -0.705751 0.708448 28 | VERTEX_SE3:QUAT l 101 5.99351 0.0361238 0.00246018 0.00155971 -0.00895478 -0.706506 0.707649 29 | VERTEX_SE3:QUAT l 102 0.0683097 5.99362 -0.00408588 -0.00352345 0.121631 -0.703543 0.700158 30 | VERTEX_SE3:QUAT l 103 0.0885111 0.0880129 -0.000729262 -0.0105796 -0.00369413 -0.706569 0.707556 31 | -------------------------------------------------------------------------------- /include/backend/backend.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** backend.h ************************************ 33 | * 34 | * Header file of Backend base class. 35 | * 36 | ******************************************************************************/ 37 | 38 | #ifndef backend_h 39 | #define backend_h 40 | 41 | #include "utility_function.h" 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | using namespace std; 62 | using namespace gtsam; 63 | #define LINESIZE 81920 64 | 65 | /* 66 | This class is the base class for all landmark slam backends. 67 | 68 | We assume that the front end will provide the following information: 69 | 1. The landmark detection result including its id, relateive pose wrt current camera pose. 70 | 2. The odometry measurement between current and previous camera pose (either from IMU or a seperate VIO module). 71 | 72 | Using April Tag as landmark alleviates the uncertainty in data association. 73 | */ 74 | namespace tagslam_ros 75 | { 76 | class Backend 77 | { 78 | 79 | public: 80 | Backend(ros::NodeHandle pnh); 81 | 82 | ~Backend(); 83 | /* 84 | Update the smoother with landmark detections and odometry measurement. 85 | */ 86 | virtual nav_msgs::OdometryPtr updateSLAM(TagDetectionArrayPtr landmark_ptr, 87 | EigenPose odom, EigenPoseCov odom_cov) = 0; 88 | 89 | /* 90 | Update the smoother with landmark detections, preintegrated IMU factor and odometry measurement. 91 | */ 92 | virtual nav_msgs::OdometryPtr updateVIO(TagDetectionArrayPtr landmark_ptr, 93 | EigenPose odom, EigenPoseCov odom_cov, bool use_odom) = 0; 94 | 95 | void setupIMU(double accel_noise_sigma, double accel_bias_rw_sigma, 96 | double gyro_noise_sigma, double gyro_bias_rw_sigma, EigenPose T_sensor2cam); 97 | 98 | /* 99 | Add IMU message to the queue 100 | */ 101 | void updateIMU(sensor_msgs::ImuPtr imu_msg_ptr); 102 | 103 | /* 104 | Retrive the smoothed poses from the smoother as Eigen::Matrix4d, and save it to the given container. 105 | */ 106 | virtual void getPoses(EigenPoseMap &container, const unsigned char filter_char) = 0; 107 | 108 | virtual void reset() = 0; 109 | 110 | Values getLandmarks() 111 | { 112 | return landmark_values_; 113 | } 114 | 115 | /* 116 | create visualization_msgs/MarkerArray message from landmark_values_ 117 | */ 118 | visualization_msgs::MarkerArrayPtr createMarkerArray(std_msgs::Header header); 119 | 120 | protected: 121 | /* 122 | read Values from a file 123 | */ 124 | Values::shared_ptr read_from_file(const string &filename); 125 | 126 | /* 127 | write Values to a file 128 | */ 129 | void write_to_file(const Values &estimate, const string &filename); 130 | 131 | /* 132 | initialize the slam 133 | */ 134 | Pose3 initSLAM(double cur_img_t); 135 | 136 | /* 137 | add combined imu factor to connect previous NavState to current NavState 138 | */ 139 | Pose3 addImuFactor(double cur_img_t); 140 | 141 | /* 142 | add odometry factor to connect previous Pose3 to current Pose3 143 | */ 144 | Pose3 addOdomFactor(EigenPose odom, EigenPoseCov odom_cov); 145 | 146 | /* 147 | add odometry factor to connect previous Pose3 to current Pose3 along with IMU preintegration 148 | */ 149 | Pose3 addImuFactor(EigenPose odom, EigenPoseCov odom_cov, double cur_img_t, bool use_odom); 150 | 151 | /* 152 | create nav_msg/odometry message from Eigen::Matrix4d 153 | */ 154 | nav_msgs::OdometryPtr createOdomMsg(Pose3 pose, EigenPoseCov pose_cov, 155 | Vector3 linear_v, Vector3 angular_w, 156 | double time, int seq); 157 | 158 | /* 159 | Template member function to add landmark factors into factor graph 160 | */ 161 | template 162 | void addLandmarkFactor(T &smoother, 163 | TagDetectionArrayPtr landmark_ptr, 164 | Pose3 &cur_pose_init) 165 | { 166 | Key cur_pose_key = Symbol(kPoseSymbol, pose_count_); 167 | 168 | // add prior factor for landmarks 169 | for (auto &landmark : landmark_ptr->detections) 170 | { 171 | if (!landmark.static_tag) 172 | { 173 | // skip non-static tags 174 | ROS_WARN("Found no static tag! This should not happen! Check the tag detection result."); 175 | continue; 176 | } 177 | Key landmark_key = Symbol(kLandmarkSymbol, landmark.id); 178 | // landmark.pose is geometry_msgs::Pose 179 | Pose3 landmark_factor = Pose3(getTransform(landmark.pose)); 180 | auto landmark_noise = noiseModel::Gaussian::Covariance(landmark_factor_cov_); 181 | 182 | // add prior factor to the local graph 183 | factor_graph_.emplace_shared>( 184 | cur_pose_key, landmark_key, landmark_factor, landmark_noise); 185 | 186 | if (!smoother.valueExists(landmark_key)) 187 | { 188 | // check if the landmark is in the prior map 189 | if (landmark_values_.exists(landmark_key)) 190 | { 191 | // if the landmark is in the prior map, we add it to the initial estimate 192 | Pose3 landmark_prior = landmark_values_.at(landmark_key); 193 | initial_estimate_.insert(landmark_key, landmark_prior); 194 | // insert prior factor to the graph 195 | auto landmark_prior_noise = noiseModel::Gaussian::Covariance(landmark_cov_[landmark_key]); 196 | factor_graph_.add(PriorFactor(landmark_key, landmark_prior, landmark_prior_noise)); 197 | 198 | // Equality Constraints can only use QR solver It is too slow 199 | // factor_graph_.add(NonlinearEquality1(landmark_prior, landmark_key)); 200 | } 201 | else 202 | { 203 | // if this a new landmark, we add it by reprojecting the landmark from the current predicted pose 204 | Pose3 landmark_init = cur_pose_init * landmark_factor; 205 | initial_estimate_.insert(landmark_key, landmark_init); 206 | } 207 | } 208 | } 209 | } 210 | 211 | private: 212 | /* 213 | set the gravity vector for preint_param_ using the imu message 214 | */ 215 | void setGravity(sensor_msgs::ImuPtr imu_msg_ptr); 216 | 217 | /* 218 | loda map from load_map_path_ 219 | */ 220 | void loadMap(); 221 | 222 | protected: 223 | // system parameters 224 | bool initialized_; 225 | bool prior_map_; // if true, the system will use prior factor to initialize the map 226 | bool save_graph_; // if true, the system will save the factor graph to a file 227 | 228 | // file path to the map file 229 | // if the map file is not empty, the system will load the map from the file 230 | // the robot will localize or do SLAM based using the loaded map as prior 231 | std::string load_map_path_; 232 | std::string save_map_path_; 233 | 234 | // Factor graph and inital value of each variables 235 | NonlinearFactorGraph factor_graph_; 236 | Values initial_estimate_; 237 | 238 | bool fix_prior_; // if true, the system will fix the prior factor for fixed lag smoother 239 | Values fixed_landmark_; 240 | Values landmark_values_; 241 | std::unordered_map landmark_cov_; 242 | 243 | // keep track of the number of poses 244 | int pose_count_; 245 | 246 | // noise parameters 247 | double landmark_factor_sigma_trans_; 248 | double landmark_factor_sigma_rot_; 249 | EigenPoseCov landmark_factor_cov_; 250 | 251 | double landmark_prior_sigma_trans_; 252 | double landmark_prior_sigma_rot_; 253 | EigenPoseCov landmark_prior_cov_; 254 | 255 | double pose_prior_sigma_trans_; 256 | double pose_prior_sigma_rot_; 257 | double vel_prior_sigma_; 258 | double bias_prior_sigma_; 259 | 260 | // concurrent queue for imu data 261 | tbb::concurrent_queue imu_queue_; 262 | boost::shared_ptr preint_param_; 263 | std::shared_ptr preint_meas_ = nullptr; 264 | 265 | // values to track of previous states 266 | Pose3 prev_pose_; 267 | Vector3 prev_vel_; 268 | NavState prev_state_; 269 | imuBias::ConstantBias prev_bias_; // assume zero initial bias 270 | 271 | bool need_pose_offset_ = false; 272 | EigenPose pose_offset_ = EigenPose::Identity(); 273 | EigenPose pose_offset_inv_ = EigenPose::Identity(); 274 | 275 | // correct the acceleration and angular velocity using the bias 276 | Vector3 correct_acc_ = Vector3::Zero(); 277 | Vector3 correct_gyro_ = Vector3::Zero(); 278 | 279 | double prev_img_t_ = 0.0; 280 | 281 | // mutex for reset 282 | std::mutex reset_mutex_; 283 | }; 284 | 285 | } 286 | 287 | #endif -------------------------------------------------------------------------------- /include/backend/fixed_lag_backend.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** fixed_lag_backend.h ************************************ 33 | * 34 | * Header file of FixedLagBackend base class based on GTSAM::BatchFixedLagSmoother. 35 | * 36 | ******************************************************************************/ 37 | 38 | #ifndef FIXED_LAG_BACKEND_H 39 | #define FIXED_LAG_BACKEND_H 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | /* 52 | This class implement a FixedLagBackend using the gtsam "IncrementalFixedLagSmoother". 53 | 54 | Basically, it is a ISAM2 with a moving window of fixed time. This allows us to 55 | have bounded runtime and memory usage, even with growing number of poses and landmark. 56 | Technically, if we do not update the "KeyTimestampMap", this behaves like a normal ISAM2. 57 | However, since this is a module from gtsam_unstable, we implement a separate class for normal isam2. 58 | 59 | */ 60 | using namespace gtsam; 61 | namespace tagslam_ros 62 | { 63 | 64 | class FixedLagBackend : public Backend 65 | { 66 | public: 67 | FixedLagBackend(ros::NodeHandle pnh); 68 | 69 | ~FixedLagBackend(); 70 | 71 | nav_msgs::OdometryPtr updateSLAM(TagDetectionArrayPtr landmark_ptr, 72 | EigenPose odom, EigenPoseCov odom_cov); 73 | 74 | nav_msgs::OdometryPtr updateVIO(TagDetectionArrayPtr landmark_ptr, 75 | EigenPose odom, EigenPoseCov odom_cov, bool use_odom); 76 | 77 | void getPoses(EigenPoseMap & container, const unsigned char filter_char); 78 | 79 | void reset(); 80 | 81 | private: 82 | void loadMap(); 83 | 84 | void updateLandmarkValues(Values& estimated_vals, Marginals & marginals); 85 | 86 | private: 87 | 88 | // Fixed lag smoother 89 | // IncrementalFixedLagSmoother isam_; 90 | bool local_optimal_; 91 | 92 | double lag_; 93 | LevenbergMarquardtParams lm_params_; 94 | BatchFixedLagSmoother smoother_; 95 | FixedLagSmoother::KeyTimestampMap newTimestamps_; 96 | double t0_; 97 | 98 | 99 | }; // class FactorGraph 100 | } // namespace backend 101 | 102 | 103 | #endif -------------------------------------------------------------------------------- /include/backend/isam2_backend.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** isam2_backend.h ************************************ 33 | * 34 | * Header file of iSAM2Backend base class based on GTSAM::ISAM2. 35 | * 36 | * This class implement a standard iSAM2 backend for landmark slam. 37 | * https://www.cs.cmu.edu/~kaess/pub/Kaess12ijrr.pdf 38 | 39 | * 40 | ******************************************************************************/ 41 | 42 | #ifndef isam2_backend_h 43 | #define isam2_backend_h 44 | 45 | #include 46 | #include 47 | 48 | using namespace gtsam; 49 | namespace tagslam_ros 50 | { 51 | 52 | class iSAM2Backend : public Backend 53 | { 54 | public: 55 | iSAM2Backend(ros::NodeHandle pnh); 56 | 57 | ~iSAM2Backend(); 58 | 59 | nav_msgs::OdometryPtr updateSLAM(TagDetectionArrayPtr landmark_ptr, 60 | EigenPose odom, EigenPoseCov odom_cov); 61 | 62 | // EigenPose updateVIO(TagDetectionArrayPtr landmark_ptr); 63 | 64 | nav_msgs::OdometryPtr updateVIO(TagDetectionArrayPtr landmark_ptr, 65 | EigenPose odom, EigenPoseCov odom_cov, bool use_odom); 66 | 67 | void getPoses(EigenPoseMap & container, const unsigned char filter_char); 68 | 69 | void setupIMU(double accel_noise_sigma, double accel_bias_rw_sigma, 70 | double gyro_noise_sigma, double gyro_bias_rw_sigma, 71 | EigenPose T_sensor2cam); 72 | 73 | void updateIMU(sensor_msgs::ImuPtr imu_msg_ptr); 74 | 75 | void reset(); 76 | 77 | private: 78 | void loadMap(); 79 | 80 | void updateLandmarkValues(Values& estimated_vals); 81 | 82 | private: 83 | // The ISAM2 smoother 84 | ISAM2Params isam_params_; 85 | ISAM2 isam_; 86 | 87 | }; // class FactorGraph 88 | } // namespace backend 89 | 90 | 91 | #endif -------------------------------------------------------------------------------- /include/frontend/tag_detector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** tag_detector.h ************************************ 33 | * 34 | * Header file of TagDetector class. It is the base class of TagDetectorGPU and TagDetectorCPU. 35 | * 36 | ******************************************************************************/ 37 | 38 | #ifndef TAG_DETECTOR_ABS_H 39 | #define TAG_DETECTOR_ABS_H 40 | 41 | #include "utility_function.h" 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "image_geometry/pinhole_camera_model.h" 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | #ifndef NO_CUDA_OPENCV 54 | #include 55 | #include 56 | #include 57 | #endif 58 | 59 | 60 | namespace tagslam_ros{ 61 | using SizeStaticPair = std::pair; 62 | class TagDetector 63 | { 64 | public: 65 | TagDetector(ros::NodeHandle pnh); 66 | 67 | ~TagDetector() = default; 68 | 69 | // Detect tags in an image 70 | virtual void detectTags(const sensor_msgs::ImageConstPtr& msg_img, 71 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, 72 | TagDetectionArrayPtr static_tag_array_ptr, 73 | TagDetectionArrayPtr dyn_tag_array_ptr) = 0; 74 | 75 | #ifndef NO_CUDA_OPENCV 76 | virtual void detectTags(cv::cuda::GpuMat& cv_mat_gpu, 77 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header, 78 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) = 0; 79 | #endif 80 | 81 | virtual void detectTags(cv::Mat& cv_mat_cpu, 82 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header, 83 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) = 0; 84 | 85 | void drawDetections(cv_bridge::CvImagePtr image, 86 | TagDetectionArrayPtr tag_detection); 87 | 88 | void drawDetections(cv::Mat & image, 89 | TagDetectionArrayPtr tag_detection); 90 | 91 | protected: 92 | void praseTagGroup(std::map & tag_group_map, XmlRpc::XmlRpcValue& tag_groups, bool static_tag); 93 | 94 | // Get the pose of the tag in the camera frame 95 | // Returns homogeneous transformation matrix [R,t;[0 0 0 1]] which 96 | // takes a point expressed in the tag frame to the same point 97 | // expressed in the camera frame. As usual, R is the (passive) 98 | // rotation from the tag frame to the camera frame and t is the 99 | // vector from the camera frame origin to the tag frame origin, 100 | // expressed in the camera frame. 101 | EigenPose getRelativeTransform( 102 | std::vector objectPoints, 103 | std::vector imagePoints, 104 | double fx, double fy, double cx, double cy) const; 105 | 106 | EigenPose getRelativeTransform( 107 | std::vector objectPoints, 108 | std::vector imagePoints, 109 | cv::Matx33d cameraMatrix, cv::Mat distCoeffs) const; 110 | 111 | protected: 112 | std::map tag_size_list_; 113 | 114 | EigenPose T_cam_to_ros_; 115 | }; 116 | } // namespace tagslam_ros 117 | #endif // APRILTAG_ROS_COMMON_FUNCTIONS_H 118 | -------------------------------------------------------------------------------- /include/frontend/tag_detector_cpu.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** tag_detector_cpu.h ************************************ 33 | * 34 | * Header file of TagDetectorCPU class which interface with AprilTag3, detect the tags 35 | * and estimate their relative poses wrt the camera. 36 | * 37 | ******************************************************************************/ 38 | 39 | #ifndef TAG_DETECTOR_CPU_H 40 | #define TAG_DETECTOR_CPU_H 41 | 42 | #include "utility_function.h" 43 | #include "frontend/tag_detector.h" 44 | 45 | #include 46 | 47 | namespace tagslam_ros{ 48 | class TagDetectorCPU: public TagDetector 49 | { 50 | public: 51 | TagDetectorCPU(ros::NodeHandle pnh); 52 | 53 | ~TagDetectorCPU(); 54 | 55 | // Detect tags in an image 56 | void detectTags(const sensor_msgs::ImageConstPtr&, 57 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, 58 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 59 | 60 | #ifndef NO_CUDA_OPENCV 61 | void detectTags(cv::cuda::GpuMat& cv_mat_gpu, 62 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header header, 63 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) 64 | { 65 | throw std::logic_error("CPU based Apriltag only support cv::Mat"); 66 | } 67 | #endif 68 | 69 | void detectTags(cv::Mat& cv_mat_cpu, const sensor_msgs::CameraInfoConstPtr& msg_cam_info, 70 | std_msgs::Header header, 71 | TagDetectionArrayPtr static_tag_array_ptr, 72 | TagDetectionArrayPtr dyn_tag_array_ptr); 73 | 74 | // Draw the detected tags' outlines and payload values on the image 75 | void drawDetections(cv_bridge::CvImagePtr image); 76 | 77 | private: 78 | // AprilTag 2 code's attributes 79 | std::string family_; 80 | int threads_; 81 | double decimate_; 82 | double blur_; 83 | int refine_edges_; 84 | int debug_; 85 | int max_hamming_distance_ = 2; // Tunable, but really, 2 is a good choice. Values of >=3 86 | // consume prohibitively large amounts of memory, and otherwise 87 | // you want the largest value possible. 88 | double tag_size_ = 1.0; 89 | 90 | // AprilTag 2 objects 91 | apriltag_family_t *tf_; 92 | apriltag_detector_t *td_; 93 | zarray_t *detections_; 94 | 95 | }; 96 | } // namespace tagslam_ros 97 | #endif // APRILTAG_ROS_COMMON_FUNCTIONS_H 98 | -------------------------------------------------------------------------------- /include/frontend/tag_detector_cuda.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** tag_detector_cuda.h ************************************ 33 | * 34 | * Header file of TagDetectorGPU class which interface with nvapriltags, detect the tags 35 | * and estimate their relative poses wrt the camera. 36 | * 37 | ******************************************************************************/ 38 | 39 | #ifndef TAG_DETECTOR_CUDA_H_ 40 | #define TAG_DETECTOR_CUDA_H_ 41 | 42 | #include "utility_function.h" 43 | #include "frontend/tag_detector.h" 44 | 45 | #include "cuda.h" // NOLINT - include .h without directory 46 | #include "cuda_runtime.h" // NOLINT - include .h without directory 47 | #include "nvAprilTags.h" 48 | 49 | 50 | namespace tagslam_ros 51 | { 52 | 53 | class TagDetectorCUDA : public TagDetector 54 | { 55 | public: 56 | TagDetectorCUDA(ros::NodeHandle pnh); 57 | 58 | ~TagDetectorCUDA(); 59 | 60 | void detectTags(const sensor_msgs::ImageConstPtr&, 61 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, 62 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 63 | 64 | #ifndef NO_CUDA_OPENCV 65 | // takes in RGBA8 cv::cuda::GpuMat 66 | void detectTags(cv::cuda::GpuMat& cv_mat_gpu, 67 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header header, 68 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 69 | #endif 70 | 71 | void detectTags(cv::Mat& cv_mat_cpu, 72 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header header, 73 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 74 | 75 | private: 76 | geometry_msgs::Pose DetectionToPose(const nvAprilTagsID_t & detection); 77 | 78 | void runDetection(TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 79 | 80 | 81 | const std::string tag_family_ = "36h11"; // cuda version only support this family 82 | const double tag_size_; 83 | const int max_tags_; 84 | 85 | struct AprilTagsImpl; 86 | std::unique_ptr impl_; 87 | 88 | cv::Matx33d cameraMatrix_; 89 | cv::Mat distCoeffs_; 90 | }; 91 | 92 | } // namespace tagslam_ros 93 | 94 | #endif // TAG_DETECTOR_CUDA_H_ 95 | -------------------------------------------------------------------------------- /include/tag_slam.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** tag_slam.h ************************************ 33 | * 34 | * Header file of TagSlam class which estimate the camera's pose once 35 | * received camera image and odometry information through ROS subscriber. 36 | * 37 | ******************************************************************************/ 38 | 39 | #ifndef TAG_SLAM_H 40 | #define TAG_SLAM_H 41 | 42 | #include "frontend/tag_detector.h" 43 | #include "frontend/tag_detector_cpu.h" 44 | 45 | #ifndef NO_CUDA 46 | #include "frontend/tag_detector_cuda.h" 47 | #endif 48 | 49 | #include "backend/backend.h" 50 | #include "backend/fixed_lag_backend.h" 51 | #include "backend/isam2_backend.h" 52 | 53 | #include 54 | 55 | #include 56 | #include 57 | 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | 67 | namespace tagslam_ros 68 | { 69 | 70 | class TagSlam : public nodelet::Nodelet 71 | { 72 | public: 73 | TagSlam() = default; 74 | ~TagSlam(); 75 | 76 | void onInit(); 77 | 78 | // private function 79 | private: 80 | void DetectionOnlyCallback(const sensor_msgs::ImageConstPtr &image, 81 | const sensor_msgs::CameraInfoConstPtr &camera_info); 82 | 83 | void imageOdomCallback( 84 | const sensor_msgs::ImageConstPtr &image, 85 | const sensor_msgs::CameraInfoConstPtr &camera_info, 86 | const nav_msgs::OdometryConstPtr &odom); 87 | 88 | // front end 89 | private: 90 | std::unique_ptr tag_detector_; 91 | 92 | // parameters 93 | bool if_pub_tag_det_; 94 | bool if_pub_tag_det_image_; 95 | bool use_approx_sync_; 96 | std::string image_topic_; 97 | std::string camera_info_topic_; 98 | std::string odom_topic_; 99 | std::string slam_pose_topic_; 100 | cv_bridge::CvImagePtr cv_image_; 101 | 102 | bool detection_only_; 103 | 104 | // runtime parameters 105 | int num_frames_; 106 | EigenPose prev_pose_; 107 | std::unordered_map tag_frequency_; 108 | 109 | // subscribers 110 | std::shared_ptr it_; 111 | image_transport::CameraSubscriber camera_image_subscriber_; 112 | 113 | image_transport::SubscriberFilter image_subscriber_; 114 | message_filters::Subscriber camera_info_subscriber_; 115 | message_filters::Subscriber odom_subscriber_; 116 | 117 | // sync policy 118 | typedef message_filters::sync_policies::ApproximateTime 121 | ApproxSyncPolicy; 122 | 123 | typedef message_filters::sync_policies::ExactTime 126 | ExactSyncPolicy; 127 | 128 | typedef message_filters::Synchronizer ApproxSync; 129 | typedef message_filters::Synchronizer ExactSync; 130 | 131 | // synchronizer 132 | std::shared_ptr approx_sync_; 133 | std::shared_ptr exact_sync_; 134 | 135 | // publishers 136 | image_transport::Publisher tag_detections_image_publisher_; 137 | ros::Publisher static_tag_det_pub_; 138 | ros::Publisher dyn_tag_det_pub_; 139 | ros::Publisher slam_pose_publisher_; 140 | 141 | // back end 142 | private: 143 | std::unique_ptr slam_backend_; 144 | EigenPose prev_vio_pose_; 145 | }; 146 | 147 | } // namespace apriltag_ros 148 | 149 | #endif // TAG_SLAM_H 150 | -------------------------------------------------------------------------------- /include/tag_slam_zed.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** tag_slam_zed.h ************************************ 33 | * 34 | * Header file of TagSlamZED class which estimate the camera's pose once 35 | * received camera image and odometry information through ZED SDK. 36 | * 37 | ******************************************************************************/ 38 | 39 | 40 | #ifndef TAG_SLAM_ZED_H 41 | #define TAG_SLAM_ZED_H 42 | 43 | 44 | 45 | // OpenCV includes 46 | #include 47 | #include 48 | 49 | #include "utility_function.h" 50 | #include "frontend/tag_detector.h" 51 | #include "frontend/tag_detector_cpu.h" 52 | 53 | #include "backend/backend.h" 54 | #include "backend/fixed_lag_backend.h" 55 | #include "backend/isam2_backend.h" 56 | 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include 71 | 72 | #ifndef NO_CUDA_OPENCV 73 | #include 74 | #include 75 | #endif 76 | 77 | using Trigger = std_srvs::Trigger; 78 | 79 | #ifndef NO_ZED 80 | // zed include 81 | #include 82 | 83 | // Note: ZED SDK depends on CUDA, 84 | // therefore if we can have zed sdk, then cuda detector will run 85 | #include "frontend/tag_detector_cuda.h"// Topics 86 | 87 | namespace tagslam_ros { 88 | class TagSlamZED : public nodelet::Nodelet { 89 | 90 | public: 91 | /*! \brief Default constructor 92 | */ 93 | TagSlamZED(); 94 | 95 | /*! \brief \ref destructor 96 | */ 97 | ~TagSlamZED(); 98 | 99 | protected: 100 | /*! \brief Initialization function called by the Nodelet base class 101 | */ 102 | void onInit(); 103 | 104 | /*! \brief Reads parameters from the param server 105 | */ 106 | void readParameters(); 107 | 108 | /*! \brief Set up ros publisher 109 | */ 110 | void setup_publisher(); 111 | 112 | /*! \brief Set up ros publisher 113 | */ 114 | void setup_service(); 115 | 116 | /*! \brief Set up ros dynamic reconfigure 117 | */ 118 | void setup_dynmaic_reconfig(); 119 | 120 | /*! \brief Turn on the zed camera 121 | */ 122 | void turn_on_zed(); 123 | 124 | /*! \brief ZED camera polling thread function 125 | */ 126 | void gpu_image_thread_func(); 127 | 128 | /*! \brief ZED camera polling thread function 129 | */ 130 | void cpu_image_thread_func(); 131 | 132 | /*! \brief Sensors data publishing function 133 | */ 134 | void sensors_thread_func(); 135 | 136 | /*! \brief publish the ros image 137 | */ 138 | void publishImages(TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 139 | 140 | /*! \brief publish the ros detection array 141 | */ 142 | void publishDetectionArray(TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr); 143 | 144 | private: 145 | void estimateState(TagDetectionArrayPtr tag_array_ptr); 146 | 147 | void checkResolFps(); 148 | 149 | void slMatToROSmsg(sensor_msgs::ImagePtr imgMsgPtr, sl::Mat img, std_msgs::Header header); 150 | 151 | void fillCameraInfo(sensor_msgs::CameraInfoPtr CamInfoMsg, sl::CalibrationParameters zedParam); 152 | 153 | ros::Time slTime2Ros(sl::Timestamp t); 154 | 155 | EigenPose slTrans2Eigen(sl::Transform& pose); 156 | 157 | EigenPose slPose2Eigen(sl::Pose& pose); 158 | 159 | EigenPoseSigma slPose2Sigma(sl::Pose& pose); 160 | 161 | EigenPoseCov slPose2Cov(sl::Pose& pose); 162 | 163 | cv::Mat slMat2cvMat(sl::Mat& input); 164 | 165 | bool resetCallback(Trigger::Request& req, Trigger::Response& res) 166 | { 167 | run_slam_ = false; 168 | // wait for one second to allow the slam thread to finish current computation 169 | slam_backend_->reset(); 170 | res.success = true; 171 | res.message = "reset slam."; 172 | return true; 173 | } 174 | 175 | bool startCallback(Trigger::Request& req, Trigger::Response& res) 176 | { 177 | run_slam_ = true; 178 | res.success = true; 179 | res.message = "Start slam."; 180 | NODELET_INFO("SLAM Started."); 181 | return true; 182 | } 183 | 184 | bool stopCallback(Trigger::Request& req, Trigger::Response& res) 185 | { 186 | run_slam_ = false; 187 | res.success = true; 188 | res.message = "Stop slam."; 189 | NODELET_INFO("SLAM Stopped."); 190 | return true; 191 | } 192 | 193 | #ifndef NO_CUDA_OPENCV 194 | cv::cuda::GpuMat slMat2cvMatGPU(sl::Mat& input); 195 | #endif 196 | 197 | int getOCVtype(sl::MAT_TYPE type); 198 | private: 199 | uint64_t frame_count_ = 0; 200 | 201 | /* 202 | *** ZED Parameters **** 203 | */ 204 | // Launch file parameters 205 | sl::RESOLUTION zed_resol_; 206 | int zed_frame_rate_; 207 | sl::DEPTH_MODE zed_depth_mode_; 208 | sl::SENSING_MODE zed_sensing_mode_; 209 | // double zed_sensor_rate_ = 400.0; 210 | double zed_min_depth_; 211 | double zed_max_depth_; 212 | 213 | sl::InitParameters zed_init_param_; 214 | sl::MODEL zed_user_model_; 215 | sl::MODEL zed_real_model_; 216 | sl::Camera zed_camera_; 217 | 218 | // Positional tracking 219 | bool zed_pos_tracking_enabled_ = false; 220 | 221 | sl::RuntimeParameters zed_runtime_param_; 222 | 223 | /* 224 | *** ROS Parameters **** 225 | */ 226 | ros::NodeHandle nh_; 227 | ros::NodeHandle pnh_; 228 | std::thread cam_thread_; // camera data thread 229 | std::thread sens_thread_; // Sensors data thread 230 | 231 | // Publishers 232 | bool if_pub_image_; 233 | bool if_pub_tag_det_; 234 | bool if_pub_tag_det_image_; 235 | bool if_pub_landmark_; 236 | bool if_pub_latency_; 237 | image_transport::CameraPublisher img_pub_; // 238 | image_transport::Publisher det_img_pub_; // 239 | 240 | ros::Publisher static_tag_det_pub_; 241 | ros::Publisher dyn_tag_det_pub_; 242 | ros::Publisher slam_pose_pub_; 243 | ros::Publisher imu_pub_; 244 | ros::Publisher landmark_pub_; 245 | 246 | ros::Publisher debug_convert_pub_; 247 | ros::Publisher debug_det_pub_; 248 | ros::Publisher debug_opt_pub_; 249 | ros::Publisher debug_total_pub_; 250 | 251 | // Services 252 | ros::ServiceServer srv_start_slam_; 253 | ros::ServiceServer srv_stop_slam_; 254 | ros::ServiceServer srv_reset_slam_; 255 | 256 | /* 257 | *** SLAM Parameters **** 258 | */ 259 | // front end 260 | bool use_gpu_detector_; 261 | std::unique_ptr tag_detector_; 262 | sl::VIEW zed_imge_type_; 263 | 264 | // back end 265 | std::string backend_type_; 266 | std::unique_ptr slam_backend_; 267 | bool detection_only_ = false; 268 | bool use_imu_odom_ = false; 269 | EigenPose pose_prev_ = EigenPose::Identity(); 270 | 271 | // Camera info 272 | sensor_msgs::CameraInfoPtr cam_info_msg_; 273 | 274 | // ROS services parameters 275 | std::atomic run_slam_ = false; 276 | 277 | }; 278 | } 279 | #else 280 | namespace tagslam_ros { 281 | class TagSlamZED : public nodelet::Nodelet { 282 | 283 | public: 284 | 285 | TagSlamZED() = default; 286 | 287 | ~TagSlamZED() = default; 288 | 289 | void onInit() 290 | { 291 | NODELET_ERROR("TagSlamZED not built. Use '-DUSE_ZED=ON' with catkin"); 292 | ros::shutdown(); 293 | } 294 | }; 295 | } 296 | #endif // NO_ZED 297 | #endif // TAG_SLAM_ZED_H 298 | -------------------------------------------------------------------------------- /include/utility_function.h: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | /***************************** utility_function.h ************************************ 33 | * 34 | * Header file of utility functions. 35 | * 36 | ******************************************************************************/ 37 | 38 | #ifndef UTIL_H 39 | #define UTIL_H 40 | 41 | // system includes 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | // front end includes 54 | #include 55 | #include 56 | #include 57 | 58 | // ros includes 59 | #include 60 | #include 61 | #include 62 | 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | #include 70 | #include 71 | #include 72 | #include 73 | 74 | #include "tagslam_ros/AprilTagDetection.h" 75 | #include "tagslam_ros/AprilTagDetectionArray.h" 76 | 77 | namespace tagslam_ros{ 78 | 79 | using TagDetectionArrayPtr = std::shared_ptr; 80 | using EigenPose = Eigen::Matrix4d; 81 | using EigenPoseSigma = Eigen::Matrix; // rotx, roty, rotz, x, y, z 82 | using EigenPoseCov = Eigen::Matrix; // rotx, roty, rotz, x, y, z 83 | using EigenPoseWithSigma = std::pair; 84 | 85 | using EigenPoseMap = std::unordered_map; 86 | using EigenPoseMapPtr = std::shared_ptr; 87 | 88 | // symbol used in backend 89 | static constexpr unsigned char kPoseSymbol = 'x'; 90 | static constexpr unsigned char kLandmarkSymbol = 'l'; 91 | static constexpr unsigned char kVelSymbol = 'V'; // (xdot,ydot,zdot) 92 | static constexpr unsigned char kBiasSymbol = 'B'; //(ax,ay,az,gx,gy,gz) 93 | 94 | template 95 | T getRosOption(ros::NodeHandle& pnh, 96 | const std::string& param_name, const T & default_val) 97 | { 98 | if(!pnh.hasParam(param_name)) 99 | { 100 | ROS_WARN_STREAM("Prameter "<< param_name<<" does not exist on server, set to default value: "<(param_name, param_val, default_val); 104 | return param_val; 105 | } 106 | 107 | static inline EigenPose getTransform(const geometry_msgs::Pose& pose_msg) 108 | { 109 | Eigen::Affine3d transform; 110 | transform.translation() << pose_msg.position.x, pose_msg.position.y, pose_msg.position.z; 111 | transform.linear() = Eigen::Quaterniond(pose_msg.orientation.w, pose_msg.orientation.x, 112 | pose_msg.orientation.y, pose_msg.orientation.z).toRotationMatrix(); 113 | return transform.matrix(); 114 | } 115 | 116 | static inline EigenPoseSigma getTransformSigma(const geometry_msgs::PoseWithCovariance & pose_cov_msg) 117 | { 118 | // gtsam have order rotx, roty, rotz, x, y, z 119 | // ros have order x, y, z, rotx, roty, rotz 120 | EigenPoseSigma sigma; 121 | sigma << std::sqrt(pose_cov_msg.covariance[21]), std::sqrt(pose_cov_msg.covariance[28]), 122 | std::sqrt(pose_cov_msg.covariance[35]), std::sqrt(pose_cov_msg.covariance[0]), 123 | std::sqrt(pose_cov_msg.covariance[7]), std::sqrt(pose_cov_msg.covariance[14]); 124 | 125 | return sigma; 126 | } 127 | 128 | static inline EigenPoseCov getTransformCov(const geometry_msgs::PoseWithCovariance & pose_cov_msg) 129 | { 130 | // gtsam have order rotx, roty, rotz, x, y, z 131 | // ros have order x, y, z, rotx, roty, rotz 132 | 133 | // [TT, TR; 134 | // RT, RR] 135 | EigenPoseCov cov_ros; 136 | for(int i=0; i<6; i++){ 137 | for(int j=0; j<6; j++) 138 | { 139 | int k = i*6+j; 140 | cov_ros(i,j) = pose_cov_msg.covariance[k]; 141 | } 142 | } 143 | 144 | 145 | Eigen::Matrix3d TT = cov_ros.block<3,3>(0,0); 146 | Eigen::Matrix3d TR = cov_ros.block<3,3>(0,3); 147 | Eigen::Matrix3d RT = cov_ros.block<3,3>(3,0); 148 | Eigen::Matrix3d RR = cov_ros.block<3,3>(3,3); 149 | 150 | // [RR, RT; 151 | // [TR, TT] 152 | EigenPoseCov cov_gtsam; 153 | cov_gtsam.block<3,3>(0,0) = RR; 154 | cov_gtsam.block<3,3>(0,3) = RT; 155 | cov_gtsam.block<3,3>(3,0) = TR; 156 | cov_gtsam.block<3,3>(3,3) = TT; 157 | 158 | return cov_gtsam; 159 | } 160 | 161 | static inline EigenPoseWithSigma getTransformWithSigma(const geometry_msgs::PoseWithCovariance & pose_cov_msg) 162 | { 163 | EigenPose transform = getTransform(pose_cov_msg.pose); 164 | EigenPoseSigma sigma = getTransformSigma(pose_cov_msg); 165 | return std::make_pair(transform, sigma); 166 | } 167 | 168 | static inline geometry_msgs::Pose createPoseMsg(const EigenPose &transform) 169 | { 170 | geometry_msgs::Pose pose; 171 | // translation 172 | pose.position.x = transform(0, 3); 173 | pose.position.y = transform(1, 3); 174 | pose.position.z = transform(2, 3); 175 | 176 | // rotation 177 | Eigen::Matrix3d rotation = transform.block<3, 3>(0, 0); 178 | Eigen::Quaternion rot_quaternion(rotation); 179 | pose.orientation.x = rot_quaternion.x(); 180 | pose.orientation.y = rot_quaternion.y(); 181 | pose.orientation.z = rot_quaternion.z(); 182 | pose.orientation.w = rot_quaternion.w(); 183 | return pose; 184 | } 185 | } // namespace tagslam_ros 186 | #endif /* util_h */ -------------------------------------------------------------------------------- /launch/tagslam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/tagslam_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/zed_sdk.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | - 'image_transport/compressedDepth' 24 | - 'image_transport/compressed' 25 | - 'image_transport/theora' 26 | 27 | 28 | 29 | 30 | 31 | - 'image_transport/compressedDepth' 32 | - 'image_transport/compressed' 33 | - 'image_transport/theora' 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /launch/zed_sdk_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | - 'image_transport/compressedDepth' 23 | - 'image_transport/compressed' 24 | - 'image_transport/theora' 25 | 26 | 27 | 28 | 29 | 30 | - 'image_transport/compressedDepth' 31 | - 'image_transport/compressed' 32 | - 'image_transport/theora' 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /launch/zed_wrapper_cpu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /launch/zed_wrapper_gpu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /msg/AprilTagDetection.msg: -------------------------------------------------------------------------------- 1 | # Tag ID 2 | int32 id 3 | 4 | # Tag size 5 | float64 size 6 | 7 | # Indicator of if this tag is static 8 | bool static_tag 9 | 10 | # Pose in the camera frame, obtained from homography transform. 11 | geometry_msgs/Pose pose 12 | 13 | geometry_msgs/Point center # centre in (x,y) pixel coordinates 14 | geometry_msgs/Point[4] corners # corners of tag ((x1,y1),(x2,y2),...) -------------------------------------------------------------------------------- /msg/AprilTagDetectionArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | AprilTagDetection[] detections 3 | -------------------------------------------------------------------------------- /nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | SLAM based on AprilTag 3 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | SLAM based on AprilTag 3 using ZED camera 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/AprilTagSLAM_ROS/91fdaacc26450293a0c4c7a506ce81e287379885/nvapriltags/lib_aarch64_jetpack44/libapril_tagging.a -------------------------------------------------------------------------------- /nvapriltags/lib_x86_64_cuda_11_4/libapril_tagging.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SafeRoboticsLab/AprilTagSLAM_ROS/91fdaacc26450293a0c4c7a506ce81e287379885/nvapriltags/lib_x86_64_cuda_11_4/libapril_tagging.a -------------------------------------------------------------------------------- /nvapriltags/nvapriltags/nvAprilTags.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #ifndef ISAAC_ROS_APRILTAG__NVAPRILTAGS__NVAPRILTAGS__NVAPRILTAGS_H_ 12 | #define ISAAC_ROS_APRILTAG__NVAPRILTAGS__NVAPRILTAGS__NVAPRILTAGS_H_ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | // Forward declaration for CUDA API 20 | // CUstream and cudaStream_t are CUstream_st* 21 | struct CUstream_st; 22 | 23 | // Decoded AprilTag 24 | typedef struct 25 | { 26 | float2 corners[4]; 27 | uint16_t id; 28 | uint8_t hamming_error; 29 | 30 | // Rotation transform, when expressed as a 3x3 matrix acting on a column vector, is column major. 31 | float orientation[9]; 32 | 33 | // Translation vector from the camera, in the same units as used for the tag_size. 34 | float translation[3]; 35 | } nvAprilTagsID_t; 36 | 37 | // Input data type for image buffer 38 | typedef struct 39 | { 40 | uchar4 * dev_ptr; // Device pointer to the buffer 41 | size_t pitch; // Pitch in bytes 42 | uint16_t width; // Width in pixels 43 | uint16_t height; // Buffer height 44 | } nvAprilTagsImageInput_t; 45 | 46 | typedef struct nvAprilTagsCameraIntrinsics_st 47 | { 48 | float fx, fy, cx, cy; 49 | } nvAprilTagsCameraIntrinsics_t; 50 | 51 | typedef enum 52 | { 53 | NVAT_TAG36H11, // Default, currently the only tag family supported 54 | NVAT_ENUM_SIZE = 0x7fffffff // Force int32_t 55 | } 56 | nvAprilTagsFamily; 57 | 58 | //! AprilTags Detector instance handle. Used to reference the detector after creation 59 | typedef struct nvAprilTagsHandle_st * nvAprilTagsHandle; 60 | 61 | #ifdef __cplusplus 62 | extern "C" { 63 | #endif 64 | 65 | /** 66 | * @brief Creates and initializes an AprilTags detector instance that detects and decodes April tags 67 | * 68 | * @param hApriltags Pointer to the handle of newly created AprilTags detector 69 | * @param img_width Width of images to be fed in to AprilTags detector 70 | * @param img_height Height of images to be fed in to AprilTags detector 71 | * @param tag_family Enum representing the Tag Family to be detected; default NVAT_TAG36H11 72 | * @param cam Camera intrinsic parameters, or NULL, if the orientation and translation are not desired 73 | * @param tag_dim The linear dimension of the square tag. The translation will be expressed in the same units. 74 | * @return int 0 - Success, else - Failure 75 | */ 76 | int nvCreateAprilTagsDetector( 77 | // TODO(hemals): We usually return the result in the last parameter, not first. 78 | nvAprilTagsHandle * hApriltags, 79 | const uint32_t img_width, const uint32_t img_height, 80 | const nvAprilTagsFamily tag_family, 81 | const nvAprilTagsCameraIntrinsics_t * cam, 82 | float tag_dim 83 | ); 84 | 85 | /** 86 | * @brief Runs the algorithms to detect potential April tags in the image and decodes valid April tags 87 | * 88 | * @param hApriltags AprilTags detector handle 89 | * @param img_input Input buffer containing the undistorted image on which to detect/decode April tags 90 | * @param tags_out C-array containing detected Tags, after detection and decoding 91 | * @param num_tags Number of tags detected 92 | * @param max_tags Maximum number of tags that can be returned, based on allocated size of tags_out array 93 | * @param input_stream CUDA stream on which the computation is to occur, or 0 to use the default stream 94 | * @return int 0 - Success, else - Failure 95 | */ 96 | int nvAprilTagsDetect( 97 | nvAprilTagsHandle hApriltags, const nvAprilTagsImageInput_t * img_input, 98 | nvAprilTagsID_t * tags_out, uint32_t * num_tags, const uint32_t max_tags, 99 | CUstream_st * input_stream); 100 | 101 | /** 102 | * @brief Destroys an instance of AprilTags detector 103 | * 104 | * @param hApriltags AprilTags detector handle to be destroyed 105 | * @return int 0 - Success, else - Failure 106 | */ 107 | int nvAprilTagsDestroy(nvAprilTagsHandle hApriltags); 108 | 109 | #ifdef __cplusplus 110 | } 111 | #endif 112 | 113 | #endif // ISAAC_ROS_APRILTAG__NVAPRILTAGS__NVAPRILTAGS__NVAPRILTAGS_H_ 114 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tagslam_ros 4 | 3.2.1 5 | 6 | TODO: Package description 7 | 8 | 9 | Danylo Malyuta 10 | Wolfgang Merkt 11 | 12 | TODO 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | 19 | 20 | boost 21 | geometry_msgs 22 | nav_msgs 23 | image_transport 24 | image_geometry 25 | roscpp 26 | sensor_msgs 27 | message_generation 28 | std_msgs 29 | cv_bridge 30 | nodelet 31 | pluginlib 32 | eigen 33 | libopencv-dev 34 | cmake_modules 35 | 36 | 37 | 38 | geometry_msgs 39 | nav_msgs 40 | boost 41 | image_transport 42 | image_geometry 43 | roscpp 44 | sensor_msgs 45 | message_runtime 46 | std_msgs 47 | cv_bridge 48 | nodelet 49 | pluginlib 50 | eigen 51 | libopencv-dev 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /src/backend/fixed_lag_backend.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | #include "backend/fixed_lag_backend.h" 32 | 33 | using namespace gtsam; 34 | 35 | namespace tagslam_ros 36 | { 37 | FixedLagBackend::FixedLagBackend(ros::NodeHandle pnh) : Backend(pnh) 38 | { 39 | // load backend parameters 40 | lm_params_.lambdaInitial = getRosOption(pnh, "backend/lambda_initial", 1e-5); 41 | lm_params_.lambdaUpperBound = getRosOption(pnh, "backend/lambda_upper_bound", 1e5); 42 | lm_params_.lambdaLowerBound = getRosOption(pnh, "backend/lambda_lower_bound", 0); 43 | lm_params_.lambdaFactor = getRosOption(pnh, "backend/lambda_factor", 10.0); 44 | lm_params_.maxIterations = getRosOption(pnh, "backend/max_iterations", 100); 45 | lm_params_.errorTol = getRosOption(pnh, "backend/error_tol", 1e-5); 46 | lm_params_.relativeErrorTol = getRosOption(pnh, "backend/relative_error_tol", 1e-4); 47 | lm_params_.absoluteErrorTol = getRosOption(pnh, "backend/absolute_error_tol", 1e-5); 48 | local_optimal_ = getRosOption(pnh, "backend/local_optimal", false); 49 | 50 | 51 | lag_ = getRosOption(pnh, "backend/lag", 1.0); 52 | smoother_ = BatchFixedLagSmoother(lag_, lm_params_, true, local_optimal_); 53 | } 54 | 55 | void FixedLagBackend::reset() 56 | { 57 | reset_mutex_.lock(); 58 | initialized_ = false; 59 | pose_count_ = 0; 60 | prev_img_t_ = 0.0; 61 | imu_queue_.clear(); 62 | landmark_values_.clear(); 63 | landmark_cov_.clear(); 64 | if (prior_map_){ 65 | loadMap(); 66 | } 67 | smoother_ = BatchFixedLagSmoother(lag_, lm_params_, true, local_optimal_); 68 | ROS_INFO("Reset iSAM2"); 69 | reset_mutex_.unlock(); 70 | } 71 | 72 | nav_msgs::OdometryPtr FixedLagBackend::updateSLAM(TagDetectionArrayPtr landmark_ptr, EigenPose odom, EigenPoseCov odom_cov) 73 | { 74 | if(reset_mutex_.try_lock()){ 75 | int num_landmarks_detected = landmark_ptr->detections.size(); 76 | Key cur_pose_key = Symbol(kPoseSymbol, pose_count_); 77 | Pose3 cur_pose_init; 78 | double cur_img_t = landmark_ptr->header.stamp.toSec(); 79 | 80 | if (initialized_) 81 | { 82 | cur_pose_init = addOdomFactor(odom, odom_cov); 83 | } 84 | else if (num_landmarks_detected > 0) 85 | { 86 | cur_pose_init = initSLAM(cur_img_t); 87 | } 88 | else 89 | { 90 | ROS_WARN_ONCE("System not initialized, waiting for landmarks"); 91 | return nullptr; 92 | } 93 | 94 | // create new timesampe map for the current pose 95 | newTimestamps_[cur_pose_key] = cur_img_t; 96 | 97 | // add prior factor for landmarks 98 | addLandmarkFactor(smoother_, landmark_ptr, cur_pose_init); 99 | 100 | // update landmark timestamp 101 | for (auto &landmark : landmark_ptr->detections) 102 | { 103 | Key landmark_key = Symbol(kLandmarkSymbol, landmark.id); 104 | newTimestamps_[landmark_key] = cur_img_t; 105 | } 106 | 107 | // do a batch optimization 108 | smoother_.update(factor_graph_, initial_estimate_, newTimestamps_); 109 | 110 | // get the current pose and save it for next iteration 111 | Values estimated_vals = smoother_.calculateEstimate(); 112 | 113 | // get the marginals 114 | Marginals marginals(smoother_.getFactors(), estimated_vals); 115 | 116 | prev_pose_ = estimated_vals.at(cur_pose_key); 117 | EigenPoseCov pose_cov = marginals.marginalCovariance(cur_pose_key); 118 | 119 | updateLandmarkValues(estimated_vals, marginals); 120 | 121 | // make message 122 | auto odom_msg = createOdomMsg(prev_pose_, pose_cov, Vector3::Zero(), Vector3::Zero(), cur_img_t, pose_count_); 123 | 124 | pose_count_++; 125 | 126 | // reset local graph and values 127 | factor_graph_.resize(0); 128 | initial_estimate_.clear(); 129 | newTimestamps_.clear(); 130 | reset_mutex_.unlock(); 131 | return odom_msg; 132 | } 133 | else{ 134 | ROS_WARN_ONCE("Resetting, waiting for reset to finish"); 135 | return nullptr; 136 | } 137 | } 138 | 139 | nav_msgs::OdometryPtr FixedLagBackend::updateVIO(TagDetectionArrayPtr landmark_ptr, 140 | EigenPose odom, EigenPoseCov odom_cov, bool use_odom) 141 | { 142 | if(reset_mutex_.try_lock()){ 143 | 144 | int num_landmarks_detected = landmark_ptr->detections.size(); 145 | 146 | Key cur_pose_key = Symbol(kPoseSymbol, pose_count_); 147 | Key cur_vel_key = Symbol(kVelSymbol, pose_count_); 148 | Key cur_bias_key = Symbol(kBiasSymbol, pose_count_); 149 | 150 | Pose3 cur_pose_init; 151 | 152 | Vector3 corrected_acc = Vector3::Zero(); 153 | Vector3 corrected_gyro = Vector3::Zero(); 154 | 155 | double cur_img_t = landmark_ptr->header.stamp.toSec(); 156 | 157 | if (initialized_) 158 | { 159 | cur_pose_init = addImuFactor(odom, odom_cov, cur_img_t, use_odom); 160 | } 161 | else if (num_landmarks_detected > 0) 162 | { 163 | cur_pose_init = initSLAM(cur_img_t); 164 | } 165 | else 166 | { 167 | ROS_WARN_ONCE("System not initialized, waiting for landmarks"); 168 | // dump all previous inserted imu measurement 169 | while (!imu_queue_.empty()) 170 | { 171 | sensor_msgs::ImuPtr imu_msg_ptr; 172 | while (!imu_queue_.try_pop(imu_msg_ptr)) 173 | { 174 | } 175 | if (imu_msg_ptr->header.stamp.toSec() >= cur_img_t) 176 | { 177 | break; 178 | } 179 | } 180 | return nullptr; 181 | } 182 | 183 | // create new timesampe map for the current pose 184 | newTimestamps_[cur_pose_key] = cur_img_t; 185 | newTimestamps_[cur_vel_key] = cur_img_t; 186 | newTimestamps_[cur_bias_key] = cur_img_t; 187 | 188 | // add prior factor for landmarks 189 | addLandmarkFactor(smoother_, landmark_ptr, cur_pose_init); 190 | 191 | // update landmark timestamp 192 | for (auto &landmark : landmark_ptr->detections) 193 | { 194 | Key landmark_key = Symbol(kLandmarkSymbol, landmark.id); 195 | newTimestamps_[landmark_key] = cur_img_t; 196 | } 197 | 198 | // do a batch optimization 199 | smoother_.update(factor_graph_, initial_estimate_, newTimestamps_); 200 | 201 | // get the current pose and save it for next iteration 202 | Values estimated_vals = smoother_.calculateEstimate(); 203 | 204 | // calculate marginals based on the current estimate 205 | Marginals marginals(smoother_.getFactors(), estimated_vals); 206 | 207 | prev_pose_ = estimated_vals.at(cur_pose_key); 208 | prev_vel_ = estimated_vals.at(cur_vel_key); 209 | prev_bias_ = estimated_vals.at(cur_bias_key); 210 | 211 | EigenPoseCov pose_cov = marginals.marginalCovariance(cur_pose_key); 212 | 213 | // ROS_INFO_STREAM("Pose covariance: " << pose_cov); 214 | 215 | prev_state_ = NavState(prev_pose_, prev_vel_); 216 | 217 | Vector3 body_vel = prev_state_.bodyVelocity(); 218 | 219 | updateLandmarkValues(estimated_vals, marginals); 220 | 221 | // make message 222 | auto odom_msg = createOdomMsg(prev_pose_, pose_cov, body_vel, correct_gyro_, cur_img_t, pose_count_); 223 | 224 | prev_img_t_ = cur_img_t; 225 | 226 | pose_count_++; 227 | 228 | // Reset the preintegration object. 229 | preint_meas_->resetIntegrationAndSetBias(prev_bias_); 230 | 231 | // reset local graph and values 232 | factor_graph_.resize(0); 233 | initial_estimate_.clear(); 234 | newTimestamps_.clear(); 235 | reset_mutex_.unlock(); 236 | return odom_msg; 237 | }else{ 238 | ROS_WARN_ONCE("Resetting, waiting for reset to finish"); 239 | return nullptr; 240 | } 241 | } 242 | 243 | void FixedLagBackend::getPoses(EigenPoseMap &container, const unsigned char filter_char) 244 | { 245 | for (const auto key_value : landmark_values_) 246 | { 247 | Key landmark_key = key_value.key; 248 | int landmark_id = Symbol(landmark_key).index(); 249 | container[landmark_id] = key_value.value.cast().matrix(); 250 | } 251 | } 252 | 253 | void FixedLagBackend::updateLandmarkValues(Values &estimated_vals, Marginals &marginals) 254 | { 255 | 256 | Values estimated_landmarks = estimated_vals.filter(Symbol::ChrTest(kLandmarkSymbol)); 257 | 258 | // iterate through landmarks, and update them to priors 259 | for (const auto key_value : estimated_landmarks) 260 | { 261 | Key temp_key = key_value.key; 262 | EigenPoseCov cov = marginals.marginalCovariance(temp_key); 263 | if(fixed_landmark_.exists(temp_key)){ 264 | continue; 265 | } 266 | else if (landmark_values_.exists(temp_key)) 267 | { 268 | landmark_values_.update(temp_key, key_value.value); 269 | } 270 | else 271 | { 272 | landmark_values_.insert(temp_key, key_value.value); 273 | } 274 | landmark_cov_[temp_key] = cov; 275 | } 276 | } 277 | 278 | } // namespace tagslam_ros 279 | -------------------------------------------------------------------------------- /src/backend/isam2_backend.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | #include "backend/isam2_backend.h" 33 | 34 | using namespace gtsam; 35 | 36 | namespace tagslam_ros 37 | { 38 | iSAM2Backend::iSAM2Backend(ros::NodeHandle pnh) : 39 | Backend(pnh) 40 | { 41 | // initialize ISAM2 42 | if(getRosOption(pnh, "backend/optimizer", "GaussNewton") == "Dogleg"){ 43 | isam_params_.optimizationParams = ISAM2DoglegParams(); 44 | } 45 | isam_params_.relinearizeSkip = std::max(getRosOption(pnh, "backend/relinearize_skip", 10), 1); 46 | isam_params_.cacheLinearizedFactors=getRosOption(pnh, "backend/cacheLinearizedFactors", true); 47 | isam_params_.relinearizeThreshold = getRosOption(pnh, "backend/relinearize_threshold", 0.1); 48 | 49 | isam_ = ISAM2(isam_params_); 50 | } 51 | 52 | void iSAM2Backend::reset() 53 | { 54 | reset_mutex_.lock(); 55 | initialized_ = false; 56 | pose_count_ = 0; 57 | prev_img_t_ = 0.0; 58 | imu_queue_.clear(); 59 | landmark_values_.clear(); 60 | landmark_cov_.clear(); 61 | if (prior_map_){ 62 | loadMap(); 63 | } 64 | isam_ = ISAM2(isam_params_); 65 | reset_mutex_.unlock(); 66 | ROS_INFO("Reset iSAM2"); 67 | } 68 | 69 | nav_msgs::OdometryPtr iSAM2Backend::updateSLAM(TagDetectionArrayPtr landmark_ptr, EigenPose odom, EigenPoseCov odom_cov) 70 | { 71 | if(reset_mutex_.try_lock()){ 72 | int num_landmarks_detected = landmark_ptr->detections.size(); 73 | Key cur_pose_key = Symbol(kPoseSymbol, pose_count_); 74 | Pose3 cur_pose_init; 75 | double cur_img_t = landmark_ptr->header.stamp.toSec(); 76 | 77 | if(initialized_){ 78 | cur_pose_init = addOdomFactor(odom, odom_cov); 79 | }else if(num_landmarks_detected>0){ 80 | cur_pose_init = initSLAM(cur_img_t); 81 | }else{ 82 | ROS_WARN_ONCE("System not initialized, waiting for landmarks"); 83 | return nullptr; 84 | } 85 | 86 | addLandmarkFactor(isam_, landmark_ptr, cur_pose_init); 87 | 88 | // do a isam update 89 | isam_.update(factor_graph_, initial_estimate_); 90 | isam_.update(); 91 | 92 | // get the current pose and save it for next iteration 93 | Values current_estimate = isam_.calculateEstimate(); 94 | prev_pose_ = current_estimate.at(cur_pose_key); 95 | 96 | // landmark_values_ = current_estimate.filter(Symbol::ChrTest(kLandmarkSymbol)); 97 | updateLandmarkValues(current_estimate); 98 | 99 | // prev_pose_ = isam_.calculateEstimate(cur_pose_key); 100 | EigenPoseCov pose_cov = isam_.marginalCovariance(cur_pose_key); 101 | 102 | auto odom_msg = createOdomMsg(prev_pose_, pose_cov, Vector3::Zero(), Vector3::Zero(), cur_img_t, pose_count_); 103 | 104 | pose_count_++; 105 | 106 | // reset local graph and values 107 | factor_graph_.resize(0); 108 | initial_estimate_.clear(); 109 | reset_mutex_.unlock(); 110 | return odom_msg; 111 | }else{ 112 | ROS_WARN_ONCE("Resetting, waiting for reset to finish"); 113 | return nullptr; 114 | } 115 | } 116 | 117 | nav_msgs::OdometryPtr iSAM2Backend::updateVIO(TagDetectionArrayPtr landmark_ptr, 118 | EigenPose odom, EigenPoseCov odom_cov, bool use_odom) 119 | { 120 | if(reset_mutex_.try_lock()){ 121 | int num_landmarks_detected = landmark_ptr->detections.size(); 122 | 123 | Key cur_pose_key = Symbol(kPoseSymbol, pose_count_); 124 | Key cur_vel_key = Symbol(kVelSymbol, pose_count_); 125 | Key cur_bias_key = Symbol(kBiasSymbol, pose_count_); 126 | 127 | Pose3 cur_pose_init; 128 | 129 | double cur_img_t = landmark_ptr->header.stamp.toSec(); 130 | 131 | if(initialized_){ 132 | cur_pose_init = addImuFactor(odom, odom_cov, cur_img_t, use_odom); 133 | }else if(num_landmarks_detected>0){ 134 | // if the system is not initialized, initialize the slam 135 | cur_pose_init = initSLAM(cur_img_t); 136 | }else{ 137 | ROS_WARN_ONCE("System not initialized, waiting for landmarks"); 138 | // dump all previous inserted imu measurement 139 | while(!imu_queue_.empty()) 140 | { 141 | sensor_msgs::ImuPtr imu_msg_ptr; 142 | while(!imu_queue_.try_pop(imu_msg_ptr)){} 143 | if(imu_msg_ptr->header.stamp.toSec()>=cur_img_t) 144 | { 145 | break; 146 | } 147 | } 148 | return nullptr; 149 | } 150 | 151 | addLandmarkFactor(isam_, landmark_ptr, cur_pose_init); 152 | 153 | // do a isam update 154 | isam_.update(factor_graph_, initial_estimate_); 155 | isam_.update(); 156 | 157 | // get the current pose and save it for next iteration 158 | Values current_estimate = isam_.calculateEstimate(); 159 | prev_pose_ = current_estimate.at(cur_pose_key); 160 | prev_vel_ = current_estimate.at(cur_vel_key); 161 | prev_bias_ = current_estimate.at(cur_bias_key); 162 | 163 | updateLandmarkValues(current_estimate); 164 | // landmark_values_ = current_estimate.filter(Symbol::ChrTest(kLandmarkSymbol)); 165 | 166 | prev_state_ = NavState(prev_pose_, prev_vel_); 167 | 168 | Vector3 body_vel = prev_state_.bodyVelocity(); 169 | 170 | EigenPoseCov pose_cov = isam_.marginalCovariance(cur_pose_key); 171 | 172 | auto odom_msg = createOdomMsg(prev_pose_, pose_cov, body_vel, correct_gyro_, cur_img_t, pose_count_); 173 | 174 | prev_img_t_ = cur_img_t; 175 | 176 | pose_count_++; 177 | 178 | // Reset the preintegration object. 179 | preint_meas_->resetIntegrationAndSetBias(prev_bias_); 180 | 181 | // reset local graph and values 182 | factor_graph_.resize(0); 183 | initial_estimate_.clear(); 184 | reset_mutex_.unlock(); 185 | return odom_msg; 186 | }else{ 187 | ROS_WARN_ONCE("Resetting, waiting for reset to finish"); 188 | return nullptr; 189 | } 190 | } 191 | 192 | 193 | void iSAM2Backend::getPoses(EigenPoseMap & container, const unsigned char filter_char) 194 | { 195 | Values landmark_values = isam_.calculateEstimate().filter(Symbol::ChrTest(filter_char)); 196 | for(const auto key_value: landmark_values) { 197 | Key landmark_key = key_value.key; 198 | int landmark_id = Symbol(landmark_key).index(); 199 | Pose3 landmark_pose = landmark_values.at(landmark_key); 200 | container[landmark_id] = landmark_pose.matrix(); 201 | } 202 | } 203 | 204 | void iSAM2Backend::updateLandmarkValues(Values &estimated_vals) 205 | { 206 | 207 | Values estimated_landmarks = estimated_vals.filter(Symbol::ChrTest(kLandmarkSymbol)); 208 | 209 | // iterate through landmarks, and update them to priors 210 | for (const auto key_value : estimated_landmarks) 211 | { 212 | Key temp_key = key_value.key; 213 | if (landmark_values_.exists(temp_key)) 214 | { 215 | landmark_values_.update(temp_key, key_value.value); 216 | } 217 | else 218 | { 219 | landmark_values_.insert(temp_key, key_value.value); 220 | } 221 | } 222 | } 223 | 224 | 225 | } // namespace backend 226 | 227 | -------------------------------------------------------------------------------- /src/frontend/tag_detector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | #include "frontend/tag_detector.h" 33 | #include 34 | 35 | 36 | namespace tagslam_ros 37 | { 38 | TagDetector::TagDetector(ros::NodeHandle pnh) 39 | { 40 | // Transform from camera frame to ROS frame 41 | T_cam_to_ros_ << 0, 0, 1, 0, 42 | -1, 0, 0, 0, 43 | 0, -1, 0, 0, 44 | 0, 0, 0, 1; 45 | 46 | // parse landmark tag group 47 | XmlRpc::XmlRpcValue landmark_groups; 48 | if(!pnh.getParam("landmark_tags", landmark_groups)) 49 | { 50 | ROS_WARN("Failed to get landmark_tags"); 51 | }else 52 | { 53 | try{ 54 | praseTagGroup(tag_size_list_, landmark_groups, true); 55 | } 56 | catch(XmlRpc::XmlRpcException e) 57 | { 58 | ROS_ERROR_STREAM("Error loading landmark_tags descriptions: " << 59 | e.getMessage().c_str()); 60 | } 61 | } 62 | 63 | XmlRpc::XmlRpcValue ignore_groups; 64 | if(!pnh.getParam("ignore_tags", ignore_groups)) 65 | { 66 | ROS_WARN("Failed to get ignore_tags"); 67 | }else 68 | { 69 | try{ 70 | praseTagGroup(tag_size_list_, ignore_groups, false); 71 | } 72 | catch(XmlRpc::XmlRpcException e) 73 | { 74 | ROS_ERROR_STREAM("Error loading ignore_tags descriptions: " << 75 | e.getMessage().c_str()); 76 | } 77 | } 78 | } 79 | 80 | void TagDetector::drawDetections(cv_bridge::CvImagePtr image, 81 | TagDetectionArrayPtr tag_detection) 82 | { 83 | drawDetections(image->image, tag_detection); 84 | } 85 | 86 | void TagDetector::drawDetections(cv::Mat & image, 87 | TagDetectionArrayPtr tag_detection) 88 | { 89 | for (auto & det : tag_detection->detections) 90 | { 91 | 92 | // Draw tag outline with edge colors green, blue, blue, red 93 | // (going counter-clockwise, starting from lower-left corner in 94 | // tag coords). cv::Scalar(Blue, Green, Red) format for the edge 95 | // colors! 96 | 97 | cv::Point p1((int)det.corners[0].x, (int)det.corners[0].y); 98 | cv::Point p2((int)det.corners[1].x, (int)det.corners[1].y); 99 | cv::Point p3((int)det.corners[2].x, (int)det.corners[2].y); 100 | cv::Point p4((int)det.corners[3].x, (int)det.corners[3].y); 101 | 102 | line(image, p1, p2, cv::Scalar(0,0xff,0)); // green 103 | line(image, p2, p3, cv::Scalar(0,0,0xff)); 104 | line(image, p3, p4, cv::Scalar(0xff,0,0)); // green 105 | line(image, p4, p1, cv::Scalar(0xff,0,0)); // green 106 | 107 | // Print tag ID in the middle of the tag 108 | std::stringstream ss; 109 | ss << det.id; 110 | cv::String text = ss.str(); 111 | int fontface = cv::FONT_HERSHEY_SCRIPT_SIMPLEX; 112 | double fontscale = 0.5; 113 | int baseline; 114 | cv::Size textsize = cv::getTextSize(text, fontface, 115 | fontscale, 2, &baseline); 116 | cv::Point center((int)det.center.x, (int)det.center.y); 117 | cv::putText(image, text, center, 118 | fontface, fontscale, cv::Scalar(0xff, 0x99, 0), 2); 119 | } 120 | } 121 | 122 | void TagDetector::praseTagGroup(std::map & tag_group_map, 123 | XmlRpc::XmlRpcValue& tag_groups, bool static_tag) 124 | { 125 | for (int i = 0; i < tag_groups.size(); i++) 126 | { 127 | XmlRpc::XmlRpcValue& tag_group = tag_groups[i]; 128 | int id_start = tag_group["id_start"]; 129 | int id_end = tag_group["id_end"]; 130 | double tag_size = tag_group["tag_size"]; 131 | ROS_INFO_STREAM("Tag group from " << id_start << " to " << id_end << " has size " << tag_size); 132 | 133 | if(id_end objectPoints, 149 | std::vector imagePoints, 150 | double fx, double fy, double cx, double cy) const 151 | { 152 | // perform Perspective-n-Point camera pose estimation using the 153 | // above 3D-2D point correspondences 154 | cv::Mat rvec, tvec; 155 | cv::Matx33d cameraMatrix(fx, 0, cx, 156 | 0, fy, cy, 157 | 0, 0, 1); 158 | cv::Vec4f distCoeffs(0, 0, 0, 0); // distortion coefficients 159 | // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would 160 | // need to first check WHAT is a bottleneck in this code, and only 161 | // do this if PnP solution is the bottleneck. 162 | cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); 163 | cv::Matx33d R; 164 | cv::Rodrigues(rvec, R); 165 | Eigen::Matrix3d wRo; 166 | wRo << R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2), R(2, 0), R(2, 1), R(2, 2); 167 | 168 | EigenPose T; // homogeneous transformation matrix 169 | T.topLeftCorner(3, 3) = wRo; 170 | T.col(3).head(3) << tvec.at(0), tvec.at(1), tvec.at(2); 171 | T.row(3) << 0, 0, 0, 1; 172 | return T; 173 | } 174 | 175 | EigenPose TagDetector::getRelativeTransform( 176 | std::vector objectPoints, 177 | std::vector imagePoints, 178 | cv::Matx33d cameraMatrix, cv::Mat distCoeffs) const 179 | { 180 | // perform Perspective-n-Point camera pose estimation using the 181 | // above 3D-2D point correspondences 182 | cv::Mat rvec, tvec; 183 | 184 | // TODO Perhaps something like SOLVEPNP_EPNP would be faster? Would 185 | // need to first check WHAT is a bottleneck in this code, and only 186 | // do this if PnP solution is the bottleneck. 187 | cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, 6); 188 | cv::Matx33d R; 189 | cv::Rodrigues(rvec, R); 190 | Eigen::Matrix3d wRo; 191 | wRo << R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2), R(2, 0), R(2, 1), R(2, 2); 192 | 193 | EigenPose T; // homogeneous transformation matrix 194 | T.topLeftCorner(3, 3) = wRo; 195 | T.col(3).head(3) << tvec.at(0), tvec.at(1), tvec.at(2); 196 | T.row(3) << 0, 0, 0, 1; 197 | return T; 198 | } 199 | } -------------------------------------------------------------------------------- /src/frontend/tag_detector_cpu.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Code is adapted from the AprilTag_ROS package by Danylo Malyuta, JPL 3 | */ 4 | 5 | /** 6 | * Copyright (c) 2017, California Institute of Technology. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright notice, 13 | * this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright notice, 15 | * this list of conditions and the following disclaimer in the documentation 16 | * and/or other materials provided with the distribution. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * The views and conclusions contained in the software and documentation are 31 | * those of the authors and should not be interpreted as representing official 32 | * policies, either expressed or implied, of the California Institute of 33 | * Technology. 34 | */ 35 | #include "frontend/tag_detector_cpu.h" 36 | 37 | #include "common/homography.h" 38 | #include "tagStandard52h13.h" 39 | #include "tagStandard41h12.h" 40 | #include "tag36h11.h" 41 | #include "tag25h9.h" 42 | #include "tag16h5.h" 43 | #include "tagCustom48h12.h" 44 | #include "tagCircle21h7.h" 45 | #include "tagCircle49h12.h" 46 | 47 | namespace tagslam_ros 48 | { 49 | 50 | TagDetectorCPU::TagDetectorCPU(ros::NodeHandle pnh) : 51 | TagDetector(pnh), 52 | family_(getRosOption(pnh, "frontend/tag_family", "tag36h11")), 53 | threads_(getRosOption(pnh, "frontend/tag_threads", 4)), 54 | decimate_(getRosOption(pnh, "frontend/tag_decimate", 1.0)), 55 | blur_(getRosOption(pnh, "frontend/tag_blur", 0.0)), 56 | refine_edges_(getRosOption(pnh, "frontend/tag_refine_edges", 1)), 57 | debug_(getRosOption(pnh, "frontend/tag_debug", 0)), 58 | max_hamming_distance_(getRosOption(pnh, "frontend/max_hamming_dist", 2)), 59 | tag_size_(getRosOption(pnh, "frontend/tag_size", 1.0)) 60 | { 61 | // 62 | ROS_INFO_STREAM("Initializing cpu AprilTag detector with family " << family_); 63 | ROS_INFO_STREAM("Tag Size: " << tag_size_); 64 | ROS_INFO_STREAM("Threads: " << threads_); 65 | ROS_INFO_STREAM("Decimate: " << decimate_); 66 | ROS_INFO_STREAM("Blur: " << blur_); 67 | ROS_INFO_STREAM("Refine edges: " << refine_edges_); 68 | ROS_INFO_STREAM("Debug: " << debug_); 69 | ROS_INFO_STREAM("Max hamming distance: " << max_hamming_distance_); 70 | 71 | // Define the tag family whose tags should be searched for in the camera 72 | // images 73 | if (family_ == "tagStandard52h13") 74 | { 75 | tf_ = tagStandard52h13_create(); 76 | } 77 | else if (family_ == "tagStandard41h12") 78 | { 79 | tf_ = tagStandard41h12_create(); 80 | } 81 | else if (family_ == "tag36h11") 82 | { 83 | tf_ = tag36h11_create(); 84 | } 85 | else if (family_ == "tag25h9") 86 | { 87 | tf_ = tag25h9_create(); 88 | } 89 | else if (family_ == "tag16h5") 90 | { 91 | tf_ = tag16h5_create(); 92 | } 93 | else if (family_ == "tagCustom48h12") 94 | { 95 | tf_ = tagCustom48h12_create(); 96 | } 97 | else if (family_ == "tagCircle21h7") 98 | { 99 | tf_ = tagCircle21h7_create(); 100 | } 101 | else if (family_ == "tagCircle49h12") 102 | { 103 | tf_ = tagCircle49h12_create(); 104 | } 105 | else 106 | { 107 | ROS_WARN("Invalid tag family specified! Aborting"); 108 | exit(1); 109 | } 110 | 111 | // Create the AprilTag 2 detector 112 | td_ = apriltag_detector_create(); 113 | apriltag_detector_add_family_bits(td_, tf_, max_hamming_distance_); 114 | td_->quad_decimate = (float)decimate_; 115 | td_->quad_sigma = (float)blur_; 116 | td_->nthreads = threads_; 117 | td_->debug = debug_; 118 | td_->refine_edges = refine_edges_; 119 | detections_ = NULL; 120 | } 121 | 122 | // destructor 123 | TagDetectorCPU::~TagDetectorCPU() 124 | { 125 | // free memory associated with tag detector 126 | apriltag_detector_destroy(td_); 127 | 128 | // Free memory associated with the array of tag detections 129 | if (detections_) 130 | { 131 | apriltag_detections_destroy(detections_); 132 | } 133 | 134 | // free memory associated with tag family 135 | if (family_ == "tagStandard52h13") 136 | { 137 | tagStandard52h13_destroy(tf_); 138 | } 139 | else if (family_ == "tagStandard41h12") 140 | { 141 | tagStandard41h12_destroy(tf_); 142 | } 143 | else if (family_ == "tag36h11") 144 | { 145 | tag36h11_destroy(tf_); 146 | } 147 | else if (family_ == "tag25h9") 148 | { 149 | tag25h9_destroy(tf_); 150 | } 151 | else if (family_ == "tag16h5") 152 | { 153 | tag16h5_destroy(tf_); 154 | } 155 | else if (family_ == "tagCustom48h12") 156 | { 157 | tagCustom48h12_destroy(tf_); 158 | } 159 | else if (family_ == "tagCircle21h7") 160 | { 161 | tagCircle21h7_destroy(tf_); 162 | } 163 | else if (family_ == "tagCircle49h12") 164 | { 165 | tagCircle49h12_destroy(tf_); 166 | } 167 | } 168 | 169 | // detect april tag from image 170 | void TagDetectorCPU::detectTags(const sensor_msgs::ImageConstPtr& msg_img, 171 | const sensor_msgs::CameraInfoConstPtr &camera_info, 172 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) 173 | { 174 | // Convert image to AprilTag code's format 175 | cv::Mat gray_image; 176 | try{ 177 | gray_image = cv_bridge::toCvShare(msg_img, "mono8")->image; 178 | }catch (cv_bridge::Exception& e){ 179 | ROS_ERROR("cv_bridge exception: %s", e.what()); 180 | return; 181 | } 182 | 183 | detectTags(gray_image, camera_info, msg_img->header, static_tag_array_ptr, dyn_tag_array_ptr); 184 | } 185 | 186 | void TagDetectorCPU::detectTags(cv::Mat& cv_mat_cpu, 187 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header header, 188 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) 189 | { 190 | image_u8_t apriltag_image = {.width = cv_mat_cpu.cols, 191 | .height = cv_mat_cpu.rows, 192 | .stride = cv_mat_cpu.cols, 193 | .buf = cv_mat_cpu.data}; 194 | 195 | image_geometry::PinholeCameraModel camera_model; 196 | camera_model.fromCameraInfo(msg_cam_info); 197 | 198 | // Get camera intrinsic properties for rectified image. 199 | double fx = camera_model.fx(); // focal length in camera x-direction [px] 200 | double fy = camera_model.fy(); // focal length in camera y-direction [px] 201 | double cx = camera_model.cx(); // optical center x-coordinate [px] 202 | double cy = camera_model.cy(); // optical center y-coordinate [px] 203 | 204 | cv::Matx33d cameraMatrix(fx, 0, cx, 205 | 0, fy, cy, 206 | 0, 0, 1); 207 | 208 | cv::Mat distCoeffs = camera_model.distortionCoeffs(); 209 | 210 | // std::cout<<"camera matrix "<header = header; 224 | dyn_tag_array_ptr->header = header; 225 | 226 | for (int i = 0; i < zarray_size(detections_); i++) 227 | { 228 | // Get the i-th detected tag 229 | apriltag_detection_t *detection; 230 | zarray_get(detections_, i, &detection); 231 | 232 | // Get the tag ID 233 | int tagID = detection->id; 234 | 235 | double cur_tag_size = tag_size_; 236 | bool cur_tag_static = true; 237 | // try to see if the tag is in the list of tags to be detected 238 | if(tag_size_list_.find(tagID) != tag_size_list_.end()) 239 | { 240 | cur_tag_size = tag_size_list_[tagID].first; 241 | cur_tag_static = tag_size_list_[tagID].second; 242 | } 243 | 244 | // Get estimated tag pose in the camera frame. 245 | // 246 | // Note on frames: 247 | // we want: 248 | // - camera frame: looking from behind the camera (like a 249 | // photographer), x is right, y is down and z is straight 250 | // ahead 251 | // - tag frame: looking straight at the tag (oriented correctly), 252 | // x is right, y is up and z is towards you (out of the tag). 253 | 254 | std::vector TagImagePoints; 255 | for (int corner_idx = 0; corner_idx < 4; corner_idx++) { 256 | TagImagePoints.push_back(cv::Point2d(detection->p[corner_idx][0], 257 | detection->p[corner_idx][1])); 258 | } 259 | 260 | std::vector TagObjectPoints; 261 | // from bottom left to bottom left and going counter clockwise 262 | TagObjectPoints.push_back(cv::Point3d(-cur_tag_size / 2, -cur_tag_size / 2, 0)); 263 | TagObjectPoints.push_back(cv::Point3d(cur_tag_size / 2, -cur_tag_size / 2, 0)); 264 | TagObjectPoints.push_back(cv::Point3d(cur_tag_size / 2, cur_tag_size / 2, 0)); 265 | TagObjectPoints.push_back(cv::Point3d(-cur_tag_size / 2, cur_tag_size / 2, 0)); 266 | 267 | EigenPose T_tag_to_cam = getRelativeTransform(TagObjectPoints,TagImagePoints,cameraMatrix, distCoeffs); 268 | EigenPose T_tag_to_ros = T_cam_to_ros_ * T_tag_to_cam; 269 | 270 | geometry_msgs::Pose tag_pose = createPoseMsg(T_tag_to_ros); 271 | 272 | // Add the detection to the back of the tag detection array 273 | AprilTagDetection tag_detection; 274 | tag_detection.pose = tag_pose; 275 | tag_detection.id = tagID; 276 | tag_detection.static_tag = cur_tag_static; 277 | tag_detection.size = cur_tag_size; 278 | 279 | // corners 280 | for (int corner_idx = 0; corner_idx < 4; corner_idx++) { 281 | tag_detection.corners.data()[corner_idx].x = detection->p[corner_idx][0]; 282 | tag_detection.corners.data()[corner_idx].y = detection->p[corner_idx][1]; 283 | } 284 | 285 | // center 286 | tag_detection.center.x = detection->c[0]; 287 | tag_detection.center.y = detection->c[1]; 288 | 289 | if(cur_tag_static) 290 | { 291 | static_tag_array_ptr->detections.push_back(tag_detection); 292 | } 293 | else 294 | { 295 | dyn_tag_array_ptr->detections.push_back(tag_detection); 296 | } 297 | } 298 | } 299 | 300 | } // namespace tagslam_ros -------------------------------------------------------------------------------- /src/frontend/tag_detector_cuda.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved. 3 | * 4 | * NVIDIA CORPORATION and its licensors retain all intellectual property 5 | * and proprietary rights in and to this software, related documentation 6 | * and any modifications thereto. Any use, reproduction, disclosure or 7 | * distribution of this software and related documentation without an express 8 | * license agreement from NVIDIA CORPORATION is strictly prohibited. 9 | */ 10 | 11 | #include "frontend/tag_detector_cuda.h" 12 | 13 | namespace tagslam_ros 14 | { 15 | struct TagDetectorCUDA::AprilTagsImpl 16 | { 17 | // Handle used to interface with the stereo library. 18 | nvAprilTagsHandle april_tags_handle = nullptr; 19 | 20 | // Camera intrinsics 21 | nvAprilTagsCameraIntrinsics_t cam_intrinsics; 22 | 23 | // Output vector of detected Tags 24 | std::vector tags; 25 | 26 | // CUDA stream 27 | cudaStream_t main_stream = {}; 28 | 29 | // CUDA buffers to store the input image. 30 | nvAprilTagsImageInput_t input_image; 31 | 32 | // CUDA memory buffer container for RGBA images. 33 | char * input_image_buffer = nullptr; 34 | 35 | // Size of image buffer 36 | size_t input_image_buffer_size = 0; 37 | 38 | // boolean to indicate if buffer has been created 39 | bool buffer_created = false; 40 | 41 | void initialize( 42 | const double tag_edge_size, 43 | const int max_tag, 44 | const size_t image_buffer_size, 45 | const size_t pitch_bytes, 46 | const sensor_msgs::CameraInfoConstPtr & camera_info, 47 | bool create_buffer = true) 48 | { 49 | assert(april_tags_handle == nullptr && "Already initialized."); 50 | 51 | input_image_buffer_size = image_buffer_size; 52 | 53 | // Get camera intrinsics 54 | 55 | // const double * k = camera_info->K; 56 | const float fx = static_cast(camera_info->K[0]); 57 | const float fy = static_cast(camera_info->K[4]); 58 | const float cx = static_cast(camera_info->K[2]); 59 | const float cy = static_cast(camera_info->K[5]); 60 | cam_intrinsics = {fx, fy, cx, cy}; 61 | 62 | const uint32_t height = camera_info->height; 63 | const uint32_t width = camera_info->width; 64 | 65 | // Create AprilTags detector instance and get handle 66 | const int error = nvCreateAprilTagsDetector( 67 | &april_tags_handle, width, height, nvAprilTagsFamily::NVAT_TAG36H11, 68 | &cam_intrinsics, (float) tag_edge_size); 69 | if (error != 0) { 70 | throw std::runtime_error( 71 | "Failed to create NV April Tags detector (error code " + 72 | std::to_string(error) + ")"); 73 | } 74 | 75 | // Create stream for detection 76 | cudaStreamCreate(&main_stream); 77 | 78 | // Allocate the output vector to contain detected AprilTags. 79 | tags.resize(max_tag); 80 | 81 | if(create_buffer) 82 | { 83 | setup_image_buffer(); 84 | } 85 | 86 | // Setup input image. 87 | input_image.width = width; 88 | input_image.height = height; 89 | if(create_buffer){ 90 | 91 | } 92 | input_image.pitch = pitch_bytes; 93 | } 94 | 95 | void setup_image_buffer() 96 | { 97 | // Setup input image CUDA buffer. 98 | const cudaError_t cuda_error = cudaMalloc(&input_image_buffer, input_image_buffer_size); 99 | if (cuda_error != cudaSuccess) { 100 | throw std::runtime_error("Could not allocate CUDA memory (error code " + 101 | std::to_string(cuda_error) + ")"); 102 | } 103 | // assigne pointer to the image buffer 104 | input_image.dev_ptr = reinterpret_cast(input_image_buffer); 105 | buffer_created = true; 106 | } 107 | 108 | void copy_to_buffer(cv::Mat & cv_mat_cpu) 109 | { 110 | if(!buffer_created) 111 | { 112 | setup_image_buffer(); 113 | } 114 | // copy cv mat from host to iamge buffer on device 115 | const cudaError_t cuda_error = cudaMemcpy(input_image_buffer, cv_mat_cpu.ptr(), 116 | input_image_buffer_size, cudaMemcpyHostToDevice); 117 | if (cuda_error != cudaSuccess) { 118 | ROS_ERROR("Could not memcpy to device CUDA memory (error code %s)", std::to_string(cuda_error).c_str()); 119 | } 120 | } 121 | 122 | void assign_image(cv::cuda::GpuMat& cv_mat_gpu) 123 | { 124 | // assign a image that already on the device as input image 125 | if(buffer_created) 126 | { 127 | cudaFree(input_image_buffer); 128 | input_image_buffer = nullptr; 129 | } 130 | input_image.dev_ptr = reinterpret_cast(cv_mat_gpu.ptr()); 131 | } 132 | 133 | ~AprilTagsImpl() 134 | { 135 | if (april_tags_handle != nullptr) { 136 | cudaStreamDestroy(main_stream); 137 | nvAprilTagsDestroy(april_tags_handle); 138 | if(buffer_created) 139 | cudaFree(input_image_buffer); 140 | } 141 | } 142 | }; 143 | 144 | TagDetectorCUDA::TagDetectorCUDA(ros::NodeHandle pnh): 145 | TagDetector(pnh), 146 | // parameter 147 | tag_size_(getRosOption(pnh, "frontend/tag_size", 1.0)), 148 | max_tags_(getRosOption(pnh, "frontend/max_tags", 20)) 149 | { 150 | // Create the AprilTags detector instance. 151 | impl_ = std::make_unique(); 152 | } 153 | 154 | void TagDetectorCUDA::detectTags(const sensor_msgs::ImageConstPtr& msg_img, 155 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, 156 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) 157 | { 158 | /* 159 | Perform Detection from a sensor_msgs::ImageConstPtr on CPU 160 | */ 161 | // Convert frame to 8-bit RGBA image 162 | cv::Mat img_rgba8; 163 | 164 | try{ 165 | img_rgba8 = cv_bridge::toCvShare(msg_img, "rgba8")->image; 166 | }catch (cv_bridge::Exception& e){ 167 | ROS_ERROR("cv_bridge exception: %s", e.what()); 168 | } 169 | 170 | detectTags(img_rgba8, msg_cam_info, msg_img->header, 171 | static_tag_array_ptr, dyn_tag_array_ptr); 172 | } 173 | 174 | #ifndef NO_CUDA_OPENCV 175 | void TagDetectorCUDA::detectTags(cv::cuda::GpuMat& cv_mat_gpu, 176 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header header, 177 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) 178 | { 179 | image_geometry::PinholeCameraModel camera_model; 180 | camera_model.fromCameraInfo(msg_cam_info); 181 | 182 | // Get camera intrinsic properties for rectified image. 183 | double fx = camera_model.fx(); // focal length in camera x-direction [px] 184 | double fy = camera_model.fy(); // focal length in camera y-direction [px] 185 | double cx = camera_model.cx(); // optical center x-coordinate [px] 186 | double cy = camera_model.cy(); // optical center y-coordinate [px] 187 | 188 | cameraMatrix_ = cv::Matx33d(fx, 0, cx, 189 | 0, fy, cy, 190 | 0, 0, 1); 191 | 192 | distCoeffs_ = camera_model.distortionCoeffs(); 193 | 194 | // Setup detector on first frame 195 | if (impl_->april_tags_handle == nullptr) { 196 | size_t buffer_size = cv_mat_gpu.rows*cv_mat_gpu.cols*cv_mat_gpu.elemSize(); 197 | impl_->initialize( 198 | tag_size_, 199 | max_tags_, 200 | buffer_size, 201 | cv_mat_gpu.step, 202 | msg_cam_info); 203 | ROS_INFO("CUDA Apriltag Detector Initialized."); 204 | } 205 | 206 | // assign the pointer to gpu mat 207 | impl_->assign_image(cv_mat_gpu); 208 | 209 | runDetection(static_tag_array_ptr, dyn_tag_array_ptr); 210 | 211 | static_tag_array_ptr->header = header; 212 | dyn_tag_array_ptr->header = header; 213 | 214 | } 215 | #endif 216 | 217 | void TagDetectorCUDA::detectTags(cv::Mat& cv_mat_cpu, 218 | const sensor_msgs::CameraInfoConstPtr& msg_cam_info, std_msgs::Header header, 219 | TagDetectionArrayPtr static_tag_array_ptr, TagDetectionArrayPtr dyn_tag_array_ptr) 220 | { 221 | image_geometry::PinholeCameraModel camera_model; 222 | camera_model.fromCameraInfo(msg_cam_info); 223 | 224 | // Get camera intrinsic properties for rectified image. 225 | double fx = camera_model.fx(); // focal length in camera x-direction [px] 226 | double fy = camera_model.fy(); // focal length in camera y-direction [px] 227 | double cx = camera_model.cx(); // optical center x-coordinate [px] 228 | double cy = camera_model.cy(); // optical center y-coordinate [px] 229 | 230 | cameraMatrix_ = cv::Matx33d(fx, 0, cx, 231 | 0, fy, cy, 232 | 0, 0, 1); 233 | 234 | distCoeffs_ = camera_model.distortionCoeffs(); 235 | 236 | // Setup detector on first frame 237 | if (impl_->april_tags_handle == nullptr) { 238 | impl_->initialize( 239 | tag_size_, 240 | max_tags_, 241 | cv_mat_cpu.total() * cv_mat_cpu.elemSize(), 242 | cv_mat_cpu.step, 243 | msg_cam_info); 244 | 245 | ROS_INFO("CUDA Apriltag Detector Initialized."); 246 | } 247 | 248 | impl_->copy_to_buffer(cv_mat_cpu); 249 | 250 | runDetection(static_tag_array_ptr, dyn_tag_array_ptr); 251 | 252 | static_tag_array_ptr->header = header; 253 | dyn_tag_array_ptr->header = header; 254 | 255 | } 256 | 257 | void TagDetectorCUDA::runDetection(TagDetectionArrayPtr static_tag_array_ptr, 258 | TagDetectionArrayPtr dyn_tag_array_ptr){ 259 | 260 | // Perform detection 261 | uint32_t num_detections; 262 | const int error = nvAprilTagsDetect( 263 | impl_->april_tags_handle, &(impl_->input_image), impl_->tags.data(), 264 | &num_detections, max_tags_, impl_->main_stream); 265 | if (error != 0) { 266 | ROS_ERROR("Failed to run AprilTags detector (error code %d)", error); 267 | return; 268 | } 269 | 270 | // Parse detections into published protos 271 | 272 | for (uint32_t i = 0; i < num_detections; i++) { 273 | // detection 274 | const nvAprilTagsID_t & detection = impl_->tags[i]; 275 | 276 | int tagID = detection.id; 277 | double cur_tag_size = tag_size_; 278 | bool cur_tag_static = true; 279 | // try to see if the tag is in the list of tags to be detected 280 | if(tag_size_list_.find(tagID) != tag_size_list_.end()) 281 | { 282 | cur_tag_size = tag_size_list_[tagID].first; 283 | cur_tag_static = tag_size_list_[tagID].second; 284 | } 285 | 286 | //make pose 287 | // geometry_msgs::Pose tag_pose = DetectionToPose(detection); 288 | // instead of pose generated from detection, we solve them us PnP due to various tag size 289 | 290 | // Get estimated tag pose in the camera frame. 291 | // 292 | // Note on frames: 293 | // we want: 294 | // - camera frame: looking from behind the camera (like a 295 | // photographer), x is right, y is down and z is straight 296 | // ahead 297 | // - tag frame: looking straight at the tag (oriented correctly), 298 | // x is right, y is up and z is towards you (out of the tag). 299 | 300 | //from top left to bottom left and going clockwise 301 | std::vector TagImagePoints; 302 | for (int corner_idx = 0; corner_idx < 4; corner_idx++) { 303 | TagImagePoints.push_back(cv::Point2d(detection.corners[corner_idx].x, 304 | detection.corners[corner_idx].y)); 305 | } 306 | 307 | std::vector TagObjectPoints; 308 | 309 | //from top left to bottom left and going clockwise 310 | TagObjectPoints.push_back(cv::Point3d(-cur_tag_size / 2, cur_tag_size / 2, 0)); 311 | TagObjectPoints.push_back(cv::Point3d(cur_tag_size / 2, cur_tag_size / 2, 0)); 312 | TagObjectPoints.push_back(cv::Point3d(cur_tag_size / 2, -cur_tag_size / 2, 0)); 313 | TagObjectPoints.push_back(cv::Point3d(-cur_tag_size / 2, -cur_tag_size / 2, 0)); 314 | 315 | EigenPose T_tag_to_cam = getRelativeTransform(TagObjectPoints,TagImagePoints, cameraMatrix_, distCoeffs_); 316 | EigenPose T_tag_to_ros = T_cam_to_ros_ * T_tag_to_cam; 317 | 318 | geometry_msgs::Pose tag_pose = createPoseMsg(T_tag_to_ros); 319 | 320 | // create message 321 | AprilTagDetection tag_detection; 322 | tag_detection.id = detection.id; 323 | tag_detection.size = cur_tag_size; 324 | tag_detection.static_tag = cur_tag_static; 325 | tag_detection.pose = tag_pose; 326 | 327 | // corners 328 | for (int corner_idx = 0; corner_idx < 4; corner_idx++) { 329 | tag_detection.corners.data()[corner_idx].x = 330 | detection.corners[corner_idx].x; 331 | tag_detection.corners.data()[corner_idx].y = 332 | detection.corners[corner_idx].y; 333 | } 334 | 335 | // center 336 | const float slope_1 = (detection.corners[2].y - detection.corners[0].y) / 337 | (detection.corners[2].x - detection.corners[0].x); 338 | const float slope_2 = (detection.corners[3].y - detection.corners[1].y) / 339 | (detection.corners[3].x - detection.corners[1].x); 340 | const float intercept_1 = detection.corners[0].y - 341 | (slope_1 * detection.corners[0].x); 342 | const float intercept_2 = detection.corners[3].y - 343 | (slope_2 * detection.corners[3].x); 344 | tag_detection.center.x = (intercept_2 - intercept_1) / (slope_1 - slope_2); 345 | tag_detection.center.y = (slope_2 * intercept_1 - slope_1 * intercept_2) / 346 | (slope_2 - slope_1); 347 | 348 | if(cur_tag_static){ 349 | static_tag_array_ptr->detections.push_back(tag_detection); 350 | }else{ 351 | dyn_tag_array_ptr->detections.push_back(tag_detection); 352 | } 353 | } 354 | } 355 | 356 | geometry_msgs::Pose TagDetectorCUDA::DetectionToPose(const nvAprilTagsID_t & detection) 357 | { 358 | geometry_msgs::Pose pose; 359 | pose.position.x = detection.translation[0]; 360 | pose.position.y = detection.translation[1]; 361 | pose.position.z = detection.translation[2]; 362 | 363 | // Rotation matrix from nvAprilTags is column major 364 | const Eigen::Map> 365 | orientation(detection.orientation); 366 | const Eigen::Quaternion q(orientation); 367 | 368 | pose.orientation.w = q.w(); 369 | pose.orientation.x = q.x(); 370 | pose.orientation.y = q.y(); 371 | pose.orientation.z = q.z(); 372 | 373 | return pose; 374 | } 375 | 376 | TagDetectorCUDA::~TagDetectorCUDA() = default; 377 | 378 | 379 | 380 | } // namespace tagslam_ros 381 | 382 | -------------------------------------------------------------------------------- /src/tag_slam.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | #include "tag_slam.h" 33 | 34 | #include 35 | 36 | PLUGINLIB_EXPORT_CLASS(tagslam_ros::TagSlam, nodelet::Nodelet); 37 | 38 | namespace tagslam_ros 39 | { 40 | TagSlam::~TagSlam() 41 | { 42 | std::cout<<"Shutting down TagSlam nodelet..."<first << " detected " << it->second << " times."<(pnh, "publish/publish_tags", false); 59 | if_pub_tag_det_image_ = getRosOption(pnh, "publish/publish_image_with_tags", false); 60 | use_approx_sync_ = getRosOption(pnh, "publish/use_approx_sync", false); 61 | 62 | image_topic_ = getRosOption(pnh, "image_topic", "/camera/image_raw"); 63 | camera_info_topic_ = getRosOption(pnh, "camera_info_topic", "/camera/camera_info"); 64 | odom_topic_ = getRosOption(pnh, "odom_topic", "/odom"); 65 | slam_pose_topic_ = getRosOption(pnh, "slam_pose_topic", "/slam_pose"); 66 | 67 | std::string transport_hint; 68 | pnh.param("transport_hint", transport_hint, "raw"); 69 | 70 | // Initialize AprilTag detector 71 | std::string detector_type_ = pnh.param("frontend/type", "CPU"); 72 | if(detector_type_ == "CPU"){ 73 | tag_detector_ = std::make_unique(pnh); 74 | }else if(detector_type_ == "GPU"){ 75 | #ifndef NO_CUDA 76 | tag_detector_ = std::make_unique(pnh); 77 | #else 78 | NODELET_ERROR("CUDA AprilTag detector is not built. Add '-DUSE_CUDA=ON' flag to catkin"); 79 | #endif 80 | }else{ 81 | NODELET_ERROR("Invalid detector type: %s", detector_type_.c_str()); 82 | } 83 | 84 | // Initialize SLAM backend 85 | std::string backend_type_ = pnh.param("backend/smoother", "isam2"); 86 | if(backend_type_=="isam2"){ 87 | slam_backend_ = std::make_unique(pnh); 88 | NODELET_INFO("Using iSAM2 backend."); 89 | }else if(backend_type_=="fixed_lag"){ 90 | slam_backend_ = std::make_unique(pnh); 91 | NODELET_INFO("Using fixed-lag backend."); 92 | }else if(backend_type_ == "none") 93 | { 94 | slam_backend_ = nullptr; 95 | detection_only_ = true; 96 | NODELET_INFO("AprilTag Detector Mode."); 97 | } 98 | 99 | prev_vio_pose_ = EigenPose::Identity(); 100 | 101 | // set up ros publishers and subscribers 102 | it_ = std::shared_ptr(new image_transport::ImageTransport(nh)); 103 | 104 | if(detection_only_) 105 | { 106 | // We only use images to find april tags as landmarks 107 | camera_image_subscriber_ = 108 | it_->subscribeCamera(image_topic_, 1, 109 | &TagSlam::DetectionOnlyCallback, this, 110 | image_transport::TransportHints(transport_hint)); 111 | }else{ 112 | // do slam 113 | // we want to use camera VIO as the odometry factor between frames 114 | NODELET_INFO_STREAM("Subscribe to camera: "<< image_topic_); 115 | image_subscriber_.subscribe(*it_, image_topic_, 1, 116 | image_transport::TransportHints(transport_hint)); 117 | camera_info_subscriber_.subscribe(nh, camera_info_topic_, 1); 118 | odom_subscriber_.subscribe(nh, odom_topic_, 1); 119 | 120 | // set up synchronizer 121 | if (use_approx_sync_) 122 | { 123 | approx_sync_.reset(new ApproxSync(ApproxSyncPolicy(10), 124 | image_subscriber_, 125 | camera_info_subscriber_, 126 | odom_subscriber_)); 127 | approx_sync_->registerCallback(boost::bind(&TagSlam::imageOdomCallback, this, _1, _2, _3)); 128 | }else{ 129 | exact_sync_.reset(new ExactSync(ExactSyncPolicy(10), 130 | image_subscriber_, 131 | camera_info_subscriber_, 132 | odom_subscriber_)); 133 | exact_sync_->registerCallback(boost::bind(&TagSlam::imageOdomCallback, this, _1, _2, _3)); 134 | } 135 | } 136 | 137 | // set up publishers for tag detections and visualization 138 | if (if_pub_tag_det_){ 139 | static_tag_det_pub_ = nh.advertise("tag_detections", 1); 140 | dyn_tag_det_pub_ = nh.advertise("tag_detections_dynamic", 1); 141 | } 142 | 143 | if (if_pub_tag_det_image_){ 144 | tag_detections_image_publisher_ = it_->advertise("tag_detections_image", 1); 145 | } 146 | 147 | if(!detection_only_){ 148 | slam_pose_publisher_ = nh.advertise("slam_pose", 1); 149 | } 150 | } 151 | 152 | void TagSlam::DetectionOnlyCallback ( 153 | const sensor_msgs::ImageConstPtr& image, 154 | const sensor_msgs::CameraInfoConstPtr& camera_info) 155 | { 156 | auto start = std::chrono::system_clock::now(); 157 | 158 | auto static_tag_array_ptr = std::make_shared(); 159 | auto dyn_tag_array_ptr = std::make_shared(); 160 | tag_detector_->detectTags(image, camera_info, 161 | static_tag_array_ptr, dyn_tag_array_ptr); 162 | 163 | auto end = std::chrono::system_clock::now(); 164 | auto elapsed = std::chrono::duration_cast(end - start); 165 | ROS_INFO_STREAM("detection "< 0 && if_pub_tag_det_){ 169 | // Publish detected tags in the image by AprilTag 2 170 | static_tag_det_pub_.publish(*static_tag_array_ptr); 171 | } 172 | 173 | if(dyn_tag_det_pub_.getNumSubscribers() > 0 && if_pub_tag_det_){ 174 | // Publish detected tags in the image by AprilTag 2 175 | dyn_tag_det_pub_.publish(*dyn_tag_array_ptr); 176 | } 177 | 178 | // Publish the camera image overlaid by outlines of the detected tags and their ids 179 | 180 | if (tag_detections_image_publisher_.getNumSubscribers() > 0 && 181 | if_pub_tag_det_image_) 182 | { 183 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image, "bgr8"); 184 | tag_detector_->drawDetections(cv_ptr, static_tag_array_ptr); 185 | tag_detections_image_publisher_.publish(cv_ptr->toImageMsg()); 186 | } 187 | num_frames_++; 188 | } 189 | 190 | void TagSlam::imageOdomCallback ( 191 | const sensor_msgs::ImageConstPtr& image, 192 | const sensor_msgs::CameraInfoConstPtr& camera_info, 193 | const nav_msgs::OdometryConstPtr& odom) 194 | { 195 | auto static_tag_array_ptr = std::make_shared(); 196 | auto dyn_tag_array_ptr = std::make_shared(); 197 | tag_detector_->detectTags(image, camera_info, 198 | static_tag_array_ptr, dyn_tag_array_ptr); 199 | 200 | // get the camera pose from VIO 201 | EigenPose vio_pose = getTransform(odom->pose.pose); 202 | EigenPoseCov vio_cov = getTransformCov(odom->pose); 203 | 204 | // get relative vio pose between this frame and the previous frame 205 | EigenPose vio_pose_delta = prev_vio_pose_.inverse() * vio_pose; 206 | 207 | // do one step of slam 208 | auto slam_pose_msg = slam_backend_->updateSLAM(static_tag_array_ptr, vio_pose_delta, vio_cov); 209 | 210 | //publish the pose message 211 | if(slam_pose_msg){ 212 | slam_pose_publisher_.publish(slam_pose_msg); 213 | } 214 | 215 | // publish tag detections 216 | if (static_tag_det_pub_.getNumSubscribers() > 0 && if_pub_tag_det_){ 217 | // Publish detected tags in the image by AprilTag 2 218 | static_tag_det_pub_.publish(*static_tag_array_ptr); 219 | } 220 | 221 | if(dyn_tag_det_pub_.getNumSubscribers() > 0 && if_pub_tag_det_){ 222 | // Publish detected tags in the image by AprilTag 2 223 | dyn_tag_det_pub_.publish(*dyn_tag_array_ptr); 224 | } 225 | 226 | // Publish the camera image overlaid by outlines of the detected tags and their ids 227 | if (tag_detections_image_publisher_.getNumSubscribers() > 0 && 228 | if_pub_tag_det_image_) 229 | { 230 | cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image, "bgr8"); 231 | tag_detector_->drawDetections(cv_ptr, static_tag_array_ptr); 232 | tag_detector_->drawDetections(cv_ptr, dyn_tag_array_ptr); 233 | tag_detections_image_publisher_.publish(cv_ptr->toImageMsg()); 234 | } 235 | 236 | // record the current vio pose for the next frame 237 | prev_vio_pose_ = vio_pose; 238 | num_frames_++; 239 | 240 | } 241 | 242 | } // namespace tagslam_ros 243 | -------------------------------------------------------------------------------- /src/tagslam_ros_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | int main(int argc, char **argv) 39 | { 40 | ros::init(argc, argv, "tagslam_ros"); 41 | 42 | nodelet::Loader nodelet; 43 | nodelet::M_string remap(ros::names::getRemappings()); 44 | nodelet::V_string nargv; 45 | 46 | nodelet.load(ros::this_node::getName(), 47 | "tagslam_ros::TagSlam", 48 | remap, nargv); 49 | 50 | ros::spin(); 51 | return 0; 52 | } -------------------------------------------------------------------------------- /src/tagslam_ros_zed_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | int main(int argc, char **argv) 39 | { 40 | ros::init(argc, argv, "tagslam_ros"); 41 | 42 | nodelet::Loader nodelet; 43 | nodelet::M_string remap(ros::names::getRemappings()); 44 | nodelet::V_string nargv; 45 | 46 | nodelet.load(ros::this_node::getName(), 47 | "tagslam_ros::TagSlamZED", 48 | remap, nargv); 49 | 50 | ros::spin(); 51 | return 0; 52 | } -------------------------------------------------------------------------------- /src/test_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * BSD 3-Clause License 3 | * Copyright (c) 2021, The Trustees of Princeton University. All rights reserved. 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are 6 | * met: 7 | * 1. Redistributions of source code must retain the above copyrigh 8 | * notice, this list of conditions and the following disclaimer. 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution 13 | * 3. Neither the name of the copyright holder nor the names of its 14 | * contributors may be used to endorse or promote products derived 15 | * from this software without specific prior written permission. 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, TH 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE US 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | 27 | * Please contact the author(s) of this library if you have any questions. 28 | * Authors: Zixu Zhang ( zixuz@princeton.edu ) 29 | 30 | **/ 31 | 32 | #include "backend/isam2_backend.h" 33 | #include "backend/fixed_lag_backend.h" 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include // std::abs 39 | 40 | using namespace tagslam_ros; 41 | using namespace gtsam; 42 | 43 | istream &operator>>(istream &is, Quaternion &q) { 44 | double x, y, z, w; 45 | is >> x >> y >> z >> w; 46 | const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm; 47 | q = Quaternion(f * w, f * x, f * y, f * z); 48 | return is; 49 | } 50 | 51 | /* ************************************************************************* */ 52 | // parse Rot3 from roll, pitch, yaw 53 | istream &operator>>(istream &is, Rot3 &R) { 54 | double yaw, pitch, roll; 55 | is >> roll >> pitch >> yaw; // notice order ! 56 | R = Rot3::Ypr(yaw, pitch, roll); 57 | return is; 58 | } 59 | 60 | 61 | EigenPose genNormalRandomPose(double sigma_trans, double sigma_rot){ 62 | std::random_device mch; 63 | std::default_random_engine generator(mch()); 64 | std::normal_distribution trans_distribution(0, sigma_trans); 65 | std::normal_distribution rot_distribution(0, sigma_rot); 66 | 67 | double x = trans_distribution(generator); 68 | double y = trans_distribution(generator); 69 | double z = trans_distribution(generator); 70 | 71 | double rot_x = rot_distribution(generator); 72 | double rot_y = rot_distribution(generator); 73 | double rot_z = rot_distribution(generator); 74 | 75 | Eigen::Affine3d pose; 76 | 77 | pose.translation() << x, y, z; 78 | Eigen::Quaterniond rot; 79 | rot = Eigen::AngleAxisd(rot_x, Eigen::Vector3d::UnitX()) 80 | * Eigen::AngleAxisd(rot_y, Eigen::Vector3d::UnitY()) 81 | * Eigen::AngleAxisd(rot_z, Eigen::Vector3d::UnitZ()); 82 | pose.linear() = rot.toRotationMatrix(); 83 | return pose.matrix(); 84 | } 85 | 86 | struct Motion{ 87 | int step = 0; 88 | EigenPose odom = EigenPose::Identity(); 89 | TagDetectionArrayPtr obs = nullptr; 90 | }; 91 | 92 | class Env 93 | { 94 | public: 95 | Env(int num_landmarks, double space_lim, double p_obs = 0.8){ 96 | generator_ = std::default_random_engine(mch_()); 97 | // initialize random number generator 98 | obs_distribution_ = std::bernoulli_distribution(p_obs); 99 | trans_distribution_ = std::uniform_real_distribution(0.0, space_lim); 100 | rot_distribution_ = std::uniform_real_distribution(-1*M_PI, M_PI); 101 | 102 | // initialize landmarks 103 | num_landmarks_ = num_landmarks; 104 | for(int i=0; i> tag) { 128 | if (tag == "VERTEX3") { 129 | unsigned char key_char; 130 | size_t id; 131 | double x, y, z; 132 | gtsam::Rot3 R; 133 | is >> key_char >> id >> x >> y >> z >> R; 134 | if(key_char == kLandmarkSymbol){ 135 | landmarks[new_id] = gtsam::Pose3(R, gtsam::Point3(x, y, z)).matrix(); 136 | new_id++; 137 | } 138 | } else if (tag == "VERTEX_SE3:QUAT") { 139 | unsigned char key_char; 140 | size_t id; 141 | double x, y, z; 142 | gtsam::Quaternion q; 143 | is >> key_char >> id >> x >> y >> z >> q; 144 | gtsam::Symbol key(key_char, id); 145 | if(key_char == kLandmarkSymbol){ 146 | landmarks[new_id] = gtsam::Pose3(q, gtsam::Point3(x, y, z)).matrix(); 147 | new_id++; 148 | } 149 | } 150 | is.ignore(LINESIZE, '\n'); 151 | 152 | } 153 | ROS_INFO_STREAM("Loaded " << new_id << " landmarks from file " << filename); 154 | 155 | num_landmarks_ = new_id; 156 | } 157 | 158 | TagDetectionArrayPtr getObs(const EigenPose& pose, double sigma_trans, double sigma_rot){ 159 | TagDetectionArrayPtr observations(new AprilTagDetectionArray); 160 | for (int i=0; idetections.push_back(detection); 169 | } 170 | } 171 | 172 | return observations; 173 | } 174 | 175 | void visualize_landmark(EigenPoseMap & estimated_landmarks){ 176 | using namespace matplot; 177 | std::vector pos_error; 178 | std::vector rot_error; 179 | std::vector index; 180 | for(int i = 0; i(0,3) - gt_landmark.block<3,1>(0,3)).norm()); 186 | Eigen::Quaterniond q_est(est_landmark.block<3,3>(0,0)); 187 | Eigen::Quaterniond q_gt(gt_landmark.block<3,3>(0,0)); 188 | rot_error.push_back(1 - std::abs(q_est.dot(q_gt))); 189 | index.push_back(i); 190 | } 191 | } 192 | auto f = figure(true); 193 | f->size(1800, 1000); 194 | subplot(2,1,1); 195 | plot(index, pos_error, "k*"); 196 | title("pos error"); 197 | subplot(2,1,2); 198 | plot(index, rot_error, "k*"); 199 | title("rot error"); 200 | 201 | show(); 202 | } 203 | 204 | 205 | private: 206 | EigenPose genUniformRandomPose(){ 207 | double x = trans_distribution_(generator_); 208 | double y = trans_distribution_(generator_); 209 | double z = trans_distribution_(generator_); 210 | 211 | double rot_x = rot_distribution_(generator_); 212 | double rot_y = rot_distribution_(generator_); 213 | double rot_z = rot_distribution_(generator_); 214 | 215 | Eigen::Affine3d pose; 216 | 217 | pose.translation() << x, y, z; 218 | 219 | Eigen::Quaterniond rot; 220 | rot = Eigen::AngleAxisd(rot_x, Eigen::Vector3d::UnitX()) 221 | * Eigen::AngleAxisd(rot_y, Eigen::Vector3d::UnitY()) 222 | * Eigen::AngleAxisd(rot_z, Eigen::Vector3d::UnitZ()); 223 | pose.linear() = rot.toRotationMatrix(); 224 | return pose.matrix(); 225 | } 226 | 227 | private: 228 | int num_landmarks_; 229 | EigenPoseMap landmarks; 230 | 231 | // random generator 232 | std::random_device mch_; 233 | std::default_random_engine generator_; 234 | std::uniform_real_distribution trans_distribution_; 235 | std::uniform_real_distribution rot_distribution_; 236 | std::bernoulli_distribution obs_distribution_; 237 | }; 238 | 239 | class Agent{ 240 | public: 241 | Agent(double sigma_trans, double sigma_rot, 242 | double odom_noise_trans, double odom_noise_rot, 243 | double landmark_noise_trans, double landmark_noise_rot): 244 | sigma_trans_(sigma_trans), 245 | sigma_rot_(sigma_rot), 246 | odom_noise_trans_(odom_noise_trans), 247 | odom_noise_rot_(odom_noise_rot), 248 | landmark_noise_trans_(landmark_noise_trans), 249 | landmark_noise_rot_(landmark_noise_rot) 250 | { 251 | // initialize pose at origin 252 | pose = EigenPose::Identity(); 253 | step_ = 0; 254 | } 255 | 256 | Motion one_step(Env & env){ 257 | EigenPose odom = genNormalRandomPose(sigma_trans_, sigma_rot_); 258 | //ROS_INFO_STREAM("odom: " << odom); 259 | // slam treat first step as origin 260 | if (step_ > 0) 261 | pose = pose * odom; 262 | 263 | TagDetectionArrayPtr obs = env.getObs(pose, landmark_noise_trans_, landmark_noise_rot_); 264 | 265 | Motion motion; 266 | motion.step = step_; 267 | motion.odom = odom*genNormalRandomPose(odom_noise_trans_, odom_noise_rot_); 268 | motion.obs = obs; 269 | 270 | trajectory[step_] = pose; 271 | step_++; 272 | 273 | return motion; 274 | } 275 | 276 | void visualize_trajectory(EigenPoseMap & estimated_trajectory){ 277 | using namespace matplot; 278 | std::vector x_est, y_est, z_est, x_gt, y_gt, z_gt; 279 | std::vector rot_error; 280 | std::vector steps; 281 | for(int i =0; i(0,0)); 293 | Eigen::Quaterniond q_gt(trajectory[i].block<3,3>(0,0)); 294 | rot_error.push_back(1 - std::abs(q_est.dot(q_gt))); 295 | } 296 | auto f = figure(true); 297 | f->size(1800, 1000); 298 | subplot(2,2,1); 299 | plot(steps, x_est, "r--", steps, x_gt, "k-"); 300 | title("x"); 301 | subplot(2,2,2); 302 | plot(steps, y_est, "r--", steps, y_gt, "k-"); 303 | title("y"); 304 | subplot(2,2,3); 305 | plot(steps, z_est, "r--", steps, z_gt, "k-"); 306 | title("z"); 307 | subplot(2,2,4); 308 | plot(steps, rot_error, "k-"); 309 | title("rot error"); 310 | 311 | 312 | show(); 313 | } 314 | 315 | private: 316 | int step_ = 0; 317 | 318 | double sigma_trans_; 319 | double sigma_rot_; 320 | 321 | double odom_noise_trans_; 322 | double odom_noise_rot_; 323 | 324 | double landmark_noise_trans_; 325 | double landmark_noise_rot_; 326 | 327 | EigenPoseMap trajectory; 328 | 329 | EigenPose pose; 330 | }; 331 | 332 | int main(int argc, char **argv) 333 | { 334 | 335 | ros::init(argc, argv, "test_node"); 336 | ros::NodeHandle pnh("~"); 337 | 338 | ros::Rate loop_rate(10); 339 | 340 | double dt = 0.05; 341 | 342 | // Read parameters 343 | int num_steps = getRosOption(pnh, "num_steps", 100); 344 | // landmark parameters 345 | int num_landmarks = getRosOption(pnh, "num_landmarks", 50); 346 | double space_lim = getRosOption(pnh, "space_lim", 10.0); 347 | 348 | // pose parameters 349 | double sigma_trans = getRosOption(pnh, "sigma_trans", 1); 350 | double sigma_rot = getRosOption(pnh, "sigma_rot", 0.5); 351 | 352 | double odom_noise_trans = getRosOption(pnh, "odom_noise_trans", 0.2); 353 | double odom_noise_rot = getRosOption(pnh, "odom_noise_rot", 0.3); 354 | 355 | double p_obs = getRosOption(pnh, "p_obs", 0.5); 356 | double landmark_noise_trans = getRosOption(pnh, "landmark_noise_trans", 0.2); 357 | double landmark_noise_rot = getRosOption(pnh, "landmark_noise_rot", 0.3); 358 | 359 | std::string backend_type = getRosOption(pnh, "backend/type", "isam2"); 360 | std::string load_path = getRosOption(pnh, "backend/load_path", ""); 361 | 362 | bool plot_figure = getRosOption(pnh, "plot_figure", false); 363 | 364 | // Create environment 365 | std::shared_ptr env; 366 | if (load_path.empty()) 367 | env = std::make_shared(num_landmarks, space_lim, p_obs); 368 | else 369 | env = std::make_shared(load_path, p_obs); 370 | 371 | Agent agent(sigma_trans, sigma_rot, odom_noise_trans, odom_noise_rot, 372 | landmark_noise_trans, landmark_noise_rot); 373 | 374 | 375 | // SLAM backend 376 | std::shared_ptr slam_backend; 377 | if(backend_type=="isam2") 378 | slam_backend = std::make_shared(pnh); 379 | else 380 | slam_backend = std::make_shared(pnh); 381 | 382 | int step = 0; 383 | 384 | EigenPoseSigma sigma; 385 | sigma << Vector3::Constant(odom_noise_rot),Vector3::Constant(odom_noise_trans); 386 | 387 | EigenPoseCov cov = sigma.array().pow(2).matrix().asDiagonal(); 388 | 389 | std::vector time_duration; 390 | EigenPoseMap estimated_poses; 391 | while (step < num_steps) 392 | { 393 | Motion motion = agent.one_step(*env); 394 | step = motion.step; 395 | TagDetectionArrayPtr obs = motion.obs; 396 | obs->header.stamp = ros::Time(step*dt); 397 | EigenPose odom = motion.odom; 398 | clock_t t0 = clock(); 399 | estimated_poses[step] = slam_backend->updateSLAM(obs, odom, cov); 400 | double dur = ((double)clock() - t0)/CLOCKS_PER_SEC; 401 | time_duration.push_back(dur); 402 | } 403 | 404 | double sum = std::accumulate(time_duration.begin(), time_duration.end(), 0.0); 405 | double mean = sum / time_duration.size(); 406 | 407 | std::vector diff(time_duration.size()); 408 | std::transform(time_duration.begin(), time_duration.end(), diff.begin(), 409 | std::bind2nd(std::minus(), mean)); 410 | double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0); 411 | double stdev = std::sqrt(sq_sum / time_duration.size()); 412 | 413 | std::cout << "mean: " << mean << " [s] with std: "<< stdev<size(1800, 1000); 418 | plot(time_duration); 419 | title("time duration"); 420 | show(); 421 | 422 | // get landmark poses 423 | EigenPoseMap estimated_landmarks; 424 | slam_backend->getPoses(estimated_landmarks, kLandmarkSymbol); 425 | agent.visualize_trajectory(estimated_poses); 426 | env->visualize_landmark(estimated_landmarks); 427 | } 428 | 429 | ros::shutdown(); 430 | return 0; 431 | } --------------------------------------------------------------------------------