├── .catkin_workspace ├── .gitattributes ├── .gitignore ├── LICENSE ├── README.md ├── my_convert.py ├── paper └── iros2022.pdf └── src ├── CMakeLists.txt ├── ls_extractor ├── CMakeLists.txt ├── README.md ├── config │ ├── example.rviz │ └── example.yaml ├── include │ └── ls_extractor │ │ ├── defs.h │ │ ├── impl │ │ ├── hough.h │ │ ├── smc.h │ │ └── smf.h │ │ ├── matplotlibcpp.h │ │ ├── ros_utils.h │ │ ├── utils.h │ │ └── visualizer.h ├── launch │ └── laser.launch ├── msg │ ├── LineSegment.msg │ ├── LineSegmentList.msg │ ├── PointCloudWithCov.msg │ └── PointWithCov.msg ├── package.xml └── src │ ├── impl │ ├── smc.cpp │ └── smf.cpp │ ├── laser_node.cpp │ ├── ls_test.cpp │ └── visualizer.cpp └── sparse_gslam ├── CMakeLists.txt ├── cmake └── FindG2O.cmake ├── config ├── gamepad.yaml └── swarm.yaml ├── datasets ├── aces │ ├── aces-30pts.txt │ ├── aces-60pts.txt │ ├── line_extractor.yaml │ ├── slam-10.yaml │ ├── slam-11.yaml │ ├── slam-13.yaml │ ├── slam-30.yaml │ ├── slam-4.yaml │ ├── slam-5.yaml │ ├── slam-6.yaml │ ├── slam-7.yaml │ └── slam-8.yaml ├── albertb │ ├── line_extractor.yaml │ └── slam.yaml ├── calc_time.py ├── download.sh ├── eval.sh ├── fixlog.py ├── gen_acc_table.py ├── gen_time_table.py ├── intel-lab │ ├── 180pts.txt │ ├── 30pts.txt │ ├── 45pts.txt │ ├── 60pts.txt │ ├── line_extractor.yaml │ ├── slam-10.yaml │ ├── slam-11.yaml │ ├── slam-13.yaml │ ├── slam-30.yaml │ ├── slam-4.yaml │ ├── slam-45.yaml │ ├── slam-5.yaml │ ├── slam-6.yaml │ ├── slam-60.yaml │ ├── slam-7.yaml │ ├── slam-8.yaml │ └── slam-9.yaml ├── intel-oregon │ ├── line_extractor.yaml │ └── slam.yaml ├── mit-csail │ └── line_extractor.yaml ├── mit-killian │ ├── line_extractor.yaml │ ├── slam-10.yaml │ ├── slam-11.yaml │ ├── slam-30-2.yaml │ ├── slam-30.yaml │ └── slam.yaml ├── nsh_level_a │ ├── line_extractor.yaml │ └── slam.yaml ├── olsson-3loop │ ├── line_extractor.yaml │ ├── olsson-3loop.log │ └── slam.yaml ├── olsson-demo │ ├── line_extractor.yaml │ ├── olsson-demo.log │ └── slam.yaml ├── relation_stats.py ├── rice │ ├── line_extractor.yaml │ ├── rice.log │ └── slam.yaml ├── slam_config_example.yaml ├── stanford-gates │ ├── line_extractor.yaml │ └── slam.yaml ├── sweep.py └── usc-sal │ ├── line_extractor.yaml │ └── slam.yaml ├── include ├── cartographer_bindings │ ├── ceres_scan_matcher_2d.h │ ├── correlative_scan_matcher_2d.h │ ├── fast_correlative_scan_matcher_2d.h │ ├── range_data_2d.h │ ├── range_data_inserter_2d.h │ └── ray_to_pixel_mask.h ├── ctpl_stl.h ├── data_provider.h ├── delta_vector.h ├── drone.h ├── g2o_bindings │ ├── edge_se2_rhotheta.h │ └── vertex_rhotheta.h ├── graphs.h ├── loop_closer │ └── submap_loop_closer.h ├── multicloud2.h ├── odom_error_propagator.h ├── pose_with_observation.h ├── submap.h ├── visualizer.h └── wallfollowing_multirange_onboard.h ├── launch ├── controller.launch ├── log_runner.launch ├── physical.launch ├── takeoff.launch └── virtual.launch ├── msg └── RawData.msg ├── package.xml ├── rviz_config ├── gmapping.rviz ├── landmark.rviz ├── odom.rviz ├── pose_graph.rviz ├── simulator.rviz ├── sparse_demo.rviz └── test_bag.rviz ├── scripts ├── controller.py └── takeoff.py └── src ├── cartographer_bindings ├── correlative_scan_matcher_2d.cc ├── fast_correlative_scan_matcher_2d.cc ├── range_data_2d.cc └── range_data_inserter_2d.cc ├── converter.cpp ├── data_provider.cpp ├── drone.cpp ├── g2o_bindings ├── edge_se2_rhotheta.cpp └── vertex_rhotheta.cpp ├── graphs.cpp ├── log_runner.cpp ├── multicloud2.cpp ├── pose_with_observation.cpp ├── submap.cpp ├── submap_loop_closer.cpp ├── submap_test.cpp └── visualizer.cpp /.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | CImg.h linguist-vendored 2 | matplotlibcpp.h linguist-vendored -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | .idea 3 | build 4 | devel 5 | build_isolated 6 | devel_isolated 7 | .catkin_tools 8 | logs 9 | temp 10 | compile_commands.json 11 | *.bag 12 | install 13 | .mypy_cache 14 | cache 15 | __pycache__ 16 | temp.* 17 | *.bag 18 | *.aux 19 | *.fls 20 | *.log 21 | *.fdb* 22 | *.gz 23 | *.bag.filtered 24 | *.out 25 | *.pdf 26 | *.pyc 27 | 28 | /convert.py 29 | /params.py 30 | *.clf 31 | *.pcd 32 | *.relations 33 | metricEvaluator 34 | 35 | *.csv 36 | **/line_extration_image/*.png 37 | /log 38 | rospack* 39 | *.bin 40 | *.png 41 | pwd 42 | *.g2o 43 | *.result 44 | *.errors 45 | *.time 46 | *.*time -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 shiftlab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Efficient 2D Graph SLAM for Sparse Sensing 2 | 3 | This repository contains code for `Efficient 2D Graph SLAM for Sparse Sensing`. The paper can be accessed [here](paper/iros2022.pdf). 4 | 5 | ## Installation 6 | 7 | We recommend Ubuntu 20.04 with ROS noetic, but it should also work on Ubuntu 18.04 with ROS melodic. Our code requires a C++14 capable compiler. 8 | 9 | ## Prereqs 10 | 11 | 1. Install dependencies using package manager: 12 | 13 | ``` 14 | sudo apt install ros-noetic-jsk-rviz-plugins ros-noetic-navigation ros-noetic-joy 15 | ``` 16 | 17 | 2. Follow the instruction on https://google-cartographer.readthedocs.io/en/latest/ to build and install google cartographer 18 | 19 | 3. Clone the libg2o-release repository (https://github.com/ros-gbp/libg2o-release) and 20 | > We use this repo rather than the latest g2o because g2o comes with its own version of ceres-solver, which may conflict with the version used by cartographer. This makes sure that they will not conflict. 21 | - checkout release/{your-ros-distro}/libg2o branch 22 | - in config.h.in, make sure that the macro `#define G2O_DELETE_IMPLICITLY_OWNED_OBJECTS 0` is enabled 23 | > we manage memories for edges and vertices ourselves. You will get segfault if you miss this step. 24 | - in CMakeLists.txt, search for `BUILD_WITH_MARCH_NATIVE` and make sure it is ON 25 | > You may get segfault if you miss this step 26 | - follow the readme to build and install it 27 | - run `sudo ldconfig` command to update links 28 | 29 | 4. Clone this repository and build 30 | 31 | ```bash 32 | catkin_make -DCMAKE_BUILD_TYPE=Release 33 | ``` 34 | 35 | 5. Download datasets and evaluation scripts by running 36 | 37 | ```bash 38 | cd src/sparse_gslam/datasets 39 | ./download.sh 40 | ``` 41 | 42 | ## Running our code 43 | 44 | ```bash 45 | roslaunch sparse_gslam log_runner.launch dataset:={dataset_name} 46 | ``` 47 | 48 | where {dataset_name} is one of the directory names under src/sparse_gslam/datasets (e.g. intel-lab). 49 | 50 | ## Compute SLAM Metrics 51 | 52 | For aces, intel-lab and mit-killian, ground truths are available and SLAM metrics can be calculated. After running our code with the command above and waiting for it to finish, you can use our evaluation script to compute the metrics 53 | 54 | ```bash 55 | cd src/sparse_gslam/datasets 56 | ./eval.sh {dataset_name} 57 | ``` 58 | 59 | -------------------------------------------------------------------------------- /my_convert.py: -------------------------------------------------------------------------------- 1 | import rosbag 2 | import rospy 3 | from std_msgs.msg import Int32, String 4 | from sensor_msgs.msg import LaserScan 5 | from geometry_msgs.msg import TransformStamped 6 | import math 7 | import tf 8 | from tf2_msgs.msg import TFMessage 9 | import numpy as np 10 | 11 | def convert_carmen(fpath, out_name): 12 | bag = rosbag.Bag('src/sparse_gslam/bags/%s.bag' % out_name, 'w') 13 | 14 | with open(fpath, "r") as f: 15 | message = [] 16 | for line in f.readlines(): 17 | if line.startswith("FLASER"): 18 | line = line.split(" ")[1:] 19 | 20 | laser_scan = LaserScan() 21 | laser_scan.header.frame_id = "base_link" 22 | laser_scan.angle_min = math.radians(-90) 23 | laser_scan.angle_max = math.radians(90) 24 | laser_scan.range_min = 0.0 25 | laser_scan.range_max = 4.0 26 | num_readings = int(line[0]) 27 | laser_scan.angle_increment = math.pi / num_readings 28 | for i in range(1, num_readings + 1): 29 | f = float(line[i]) 30 | if f >= 10: 31 | laser_scan.ranges.append(float('nan')) 32 | else: 33 | laser_scan.ranges.append(f) 34 | i += 1 35 | 36 | trans = TransformStamped() 37 | trans.header.frame_id = "odom" 38 | trans.child_frame_id = "base_link" 39 | trans.transform.translation.x = float(line[i+3]) 40 | trans.transform.translation.y = float(line[i+4]) 41 | trans.transform.translation.z = 0 42 | 43 | quat = tf.transformations.quaternion_from_euler(0.0, 0.0, float(line[i+5])) 44 | trans.transform.rotation.x = quat[0] 45 | trans.transform.rotation.y = quat[1] 46 | trans.transform.rotation.z = quat[2] 47 | trans.transform.rotation.w = quat[3] 48 | 49 | timestamp = float(line[i+6]) 50 | trans.header.stamp = laser_scan.header.stamp = rospy.Time(timestamp) 51 | 52 | tf_msg = TFMessage() 53 | tf_msg.transforms.append(trans) 54 | message.append((timestamp, tf_msg, laser_scan)) 55 | 56 | message.sort(key=lambda x: x[0]) 57 | print(len(message)) 58 | for i, (_, tf_msg, laser_scan) in enumerate(message): 59 | tf_msg.transforms[0].header.seq = i 60 | laser_scan.header.seq = i 61 | bag.write("tf", tf_msg, laser_scan.header.stamp) 62 | bag.write("scan", laser_scan, laser_scan.header.stamp) 63 | 64 | bag.close() 65 | 66 | def convert_carmen_bspline(fpath, out_name): 67 | with open(fpath, "r") as f: 68 | with open(out_name, 'w') as f2: 69 | for line in f.readlines(): 70 | if line.startswith("FLASER"): 71 | line = line.split(" ")[1:] 72 | message = [] 73 | timestamp = line[-1].strip() 74 | num_readings = int(line[0]) 75 | laser_scan = [] 76 | for i in range(1, num_readings + 1): 77 | laser_tmp = line[i] 78 | laser_scan.append(laser_tmp) 79 | i += 1 80 | odomx = line[i+3] 81 | odomy = line[i+4] 82 | odomt = line[i+5] 83 | 84 | message.append(timestamp) 85 | message.append(odomx) 86 | message.append(odomy) 87 | message.append(odomt) 88 | message.extend(laser_scan) 89 | 90 | message = " ".join(message) 91 | message = message + "\n" 92 | f2.write(message) 93 | 94 | 95 | def convert_radish(fpath, out_name): 96 | bag = rosbag.Bag('src/sparse_gslam/bags/%s.bag' % out_name, 'w') 97 | 98 | with open(fpath, "r") as f: 99 | seq = 0 100 | 101 | for line in f.readlines(): 102 | if line.startswith("position"): 103 | line = line.split(" ")[3:] 104 | last_x = float(line[0]) 105 | last_y = float(line[1]) 106 | last_theta = float(line[2]) 107 | 108 | elif line.startswith("laser"): 109 | line = line.split(" ") 110 | tstamp = float(line[2]) 111 | line = line[3:-1] 112 | 113 | laser_scan = LaserScan() 114 | laser_scan.header.frame_id = "base_link" 115 | laser_scan.angle_min = math.radians(-90) 116 | laser_scan.angle_max = math.radians(90) 117 | laser_scan.angle_increment = math.radians(1) 118 | laser_scan.range_min = 0.0 119 | laser_scan.range_max = 4.0 120 | num_readings = 181 121 | for i in range(num_readings): 122 | laser_scan.ranges.append(float(line[3 * i])) 123 | 124 | trans = TransformStamped() 125 | trans.header.frame_id = "odom" 126 | trans.child_frame_id = "base_link" 127 | trans.transform.translation.x = last_x 128 | trans.transform.translation.y = last_y 129 | trans.transform.translation.z = 0 130 | 131 | quat = tf.transformations.quaternion_from_euler(0.0, 0.0, last_theta) 132 | trans.transform.rotation.x = quat[0] 133 | trans.transform.rotation.y = quat[1] 134 | trans.transform.rotation.z = quat[2] 135 | trans.transform.rotation.w = quat[3] 136 | 137 | trans.header.stamp = laser_scan.header.stamp = rospy.Time(tstamp) 138 | trans.header.seq = laser_scan.header.seq = seq 139 | 140 | tf_msg = TFMessage() 141 | tf_msg.transforms.append(trans) 142 | bag.write("tf", tf_msg, trans.header.stamp) 143 | bag.write("scan", laser_scan, trans.header.stamp) 144 | seq += 1 145 | 146 | bag.close() 147 | 148 | 149 | def convert_stanford_gates(fpath, out_name): 150 | bag = rosbag.Bag('src/sparse_gslam/bags/%s.bag' % out_name, 'w') 151 | 152 | with open(fpath, "r") as f: 153 | seq = 0 154 | last_x = -1e10 155 | for line in f.readlines(): 156 | if line.startswith("#"): 157 | continue 158 | line = line.split()[3:] 159 | if line[0] == "position": 160 | line = line[3:] 161 | last_x = float(line[0]) 162 | last_y = float(line[1]) 163 | last_theta = float(line[2]) 164 | 165 | elif line[0] == "laser": 166 | if last_x == -1e10: 167 | continue 168 | tstamp = float(line[2]) 169 | line = line[7:] 170 | # print(len(line), line[-1]) 171 | # break 172 | 173 | laser_scan = LaserScan() 174 | laser_scan.header.frame_id = "base_link" 175 | laser_scan.angle_min = math.radians(-90) 176 | laser_scan.angle_max = math.radians(90) 177 | laser_scan.angle_increment = math.radians(1) 178 | laser_scan.range_min = 0.0 179 | laser_scan.range_max = 4.0 180 | num_readings = 181 181 | for i in range(num_readings): 182 | laser_scan.ranges.append(float(line[2 * i])) 183 | 184 | trans = TransformStamped() 185 | trans.header.frame_id = "odom" 186 | trans.child_frame_id = "base_link" 187 | trans.transform.translation.x = last_x 188 | trans.transform.translation.y = last_y 189 | trans.transform.translation.z = 0 190 | 191 | quat = tf.transformations.quaternion_from_euler(0.0, 0.0, last_theta) 192 | trans.transform.rotation.x = quat[0] 193 | trans.transform.rotation.y = quat[1] 194 | trans.transform.rotation.z = quat[2] 195 | trans.transform.rotation.w = quat[3] 196 | 197 | trans.header.stamp = laser_scan.header.stamp = rospy.Time(tstamp) 198 | trans.header.seq = laser_scan.header.seq = seq 199 | 200 | tf_msg = TFMessage() 201 | tf_msg.transforms.append(trans) 202 | bag.write("tf", tf_msg, trans.header.stamp) 203 | bag.write("scan", laser_scan, trans.header.stamp) 204 | seq += 1 205 | 206 | last_x = -1e10 207 | 208 | bag.close() 209 | 210 | 211 | if __name__ == "__main__": 212 | # convert_radish("run02.log", "usc-sal200-021120") 213 | # convert_stanford_gates("stanford-gates1.log", "stanford-gates1") 214 | convert_carmen("fr079.txt", "fr079") 215 | # convert_carmen_bspline("fr079.txt", "fr079-spline.log") 216 | # convert_carmen("mit-csail.clf", "mit-csail") 217 | -------------------------------------------------------------------------------- /paper/iros2022.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shiftlab-nanodrone/sparse-gslam/c772618f3470c73e17c728d19364e99464afb436/paper/iros2022.pdf -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # toplevel CMakeLists.txt for a catkin workspace 2 | # catkin/cmake/toplevel.cmake 3 | 4 | cmake_minimum_required(VERSION 3.0.2) 5 | 6 | project(Project) 7 | 8 | set(CATKIN_TOPLEVEL TRUE) 9 | 10 | # search for catkin within the workspace 11 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") 12 | execute_process(COMMAND ${_cmd} 13 | RESULT_VARIABLE _res 14 | OUTPUT_VARIABLE _out 15 | ERROR_VARIABLE _err 16 | OUTPUT_STRIP_TRAILING_WHITESPACE 17 | ERROR_STRIP_TRAILING_WHITESPACE 18 | ) 19 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) 20 | # searching fot catkin resulted in an error 21 | string(REPLACE ";" " " _cmd_str "${_cmd}") 22 | message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") 23 | endif() 24 | 25 | # include catkin from workspace or via find_package() 26 | if(_res EQUAL 0) 27 | set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") 28 | # include all.cmake without add_subdirectory to let it operate in same scope 29 | include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) 30 | add_subdirectory("${_out}") 31 | 32 | else() 33 | # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument 34 | # or CMAKE_PREFIX_PATH from the environment 35 | if(NOT DEFINED CMAKE_PREFIX_PATH) 36 | if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") 37 | if(NOT WIN32) 38 | string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 39 | else() 40 | set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 41 | endif() 42 | endif() 43 | endif() 44 | 45 | # list of catkin workspaces 46 | set(catkin_search_path "") 47 | foreach(path ${CMAKE_PREFIX_PATH}) 48 | if(EXISTS "${path}/.catkin") 49 | list(FIND catkin_search_path ${path} _index) 50 | if(_index EQUAL -1) 51 | list(APPEND catkin_search_path ${path}) 52 | endif() 53 | endif() 54 | endforeach() 55 | 56 | # search for catkin in all workspaces 57 | set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) 58 | find_package(catkin QUIET 59 | NO_POLICY_SCOPE 60 | PATHS ${catkin_search_path} 61 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 62 | unset(CATKIN_TOPLEVEL_FIND_PACKAGE) 63 | 64 | if(NOT catkin_FOUND) 65 | message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") 66 | endif() 67 | endif() 68 | 69 | catkin_workspace() 70 | -------------------------------------------------------------------------------- /src/ls_extractor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ls_extractor) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | add_compile_options(-Wall -Wno-sign-compare -g) 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | message_generation 13 | tf 14 | pcl_ros 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | find_package(Python3 COMPONENTS Development NumPy) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | add_message_files( 52 | FILES 53 | PointWithCov.msg 54 | PointCloudWithCov.msg 55 | LineSegment.msg 56 | LineSegmentList.msg 57 | ) 58 | 59 | ## Generate services in the 'srv' folder 60 | # add_service_files( 61 | # FILES 62 | # Service1.srv 63 | # Service2.srv 64 | # ) 65 | 66 | ## Generate actions in the 'action' folder 67 | # add_action_files( 68 | # FILES 69 | # Action1.action 70 | # Action2.action 71 | # ) 72 | 73 | ## Generate added messages and services with any dependencies listed here 74 | generate_messages( 75 | DEPENDENCIES 76 | std_msgs 77 | ) 78 | 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | INCLUDE_DIRS include 111 | # LIBRARIES ls_visualizer 112 | # CATKIN_DEPENDS roscpp 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | include 124 | ${catkin_INCLUDE_DIRS} 125 | ${Python3_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/ls_extractor.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/ls_extractor_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | # target_link_libraries(${PROJECT_NAME}_node 155 | # ${catkin_LIBRARIES} 156 | # ) 157 | 158 | add_library(smc src/impl/smc.cpp) 159 | add_library(smf src/impl/smf.cpp) 160 | 161 | add_library(ls_visualizer src/visualizer.cpp) 162 | target_link_libraries(ls_visualizer smc ${catkin_LIBRARIES}) 163 | 164 | add_executable(ls_test src/ls_test.cpp) 165 | target_link_libraries(ls_test smc ${catkin_LIBRARIES}) 166 | 167 | add_executable(laser_node src/laser_node.cpp) 168 | target_link_libraries(laser_node smc ls_visualizer ${catkin_LIBRARIES}) 169 | 170 | # add_executable(ls_extractor_test_real src/ls_extractor_test_real.cpp) 171 | # add_dependencies(ls_extractor_test_real ls_extractor_generate_messages_cpp) 172 | # target_link_libraries(ls_extractor_test_real ${catkin_LIBRARIES}) 173 | 174 | 175 | # add_executable(ls_extractor_cluster src/ls_extractor_cluster.cpp) 176 | # add_dependencies(ls_extractor_cluster ls_extractor_generate_messages_cpp) 177 | # target_link_libraries(ls_extractor_cluster ${catkin_LIBRARIES} ls_extractor) 178 | ############# 179 | ## Install ## 180 | ############# 181 | 182 | # all install targets should use catkin DESTINATION variables 183 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 184 | 185 | ## Mark executable scripts (Python etc.) for installation 186 | ## in contrast to setup.py, you can choose the destination 187 | # catkin_install_python(PROGRAMS 188 | # scripts/my_python_script 189 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 190 | # ) 191 | 192 | ## Mark executables for installation 193 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 194 | # install(TARGETS ${PROJECT_NAME}_node 195 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 196 | # ) 197 | 198 | ## Mark libraries for installation 199 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 200 | # install(TARGETS ${PROJECT_NAME} 201 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 202 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 203 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 204 | # ) 205 | 206 | ## Mark cpp header files for installation 207 | # install(DIRECTORY include/${PROJECT_NAME}/ 208 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 209 | # FILES_MATCHING PATTERN "*.h" 210 | # PATTERN ".svn" EXCLUDE 211 | # ) 212 | 213 | ## Mark other files for installation (e.g. launch and bag files, etc.) 214 | # install(FILES 215 | # # myfile1 216 | # # myfile2 217 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 218 | # ) 219 | 220 | ############# 221 | ## Testing ## 222 | ############# 223 | 224 | ## Add gtest based cpp test target and link libraries 225 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_ls_extractor.cpp) 226 | # if(TARGET ${PROJECT_NAME}-test) 227 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 228 | # endif() 229 | 230 | ## Add folders to be run by python nosetests 231 | # catkin_add_nosetests(test) 232 | -------------------------------------------------------------------------------- /src/ls_extractor/README.md: -------------------------------------------------------------------------------- 1 | # Line Segment Extractor 2 | 3 | Line segment extractor for 2D pointclouds/LiDARs data with error propagation. Similar to https://github.com/kam3k/laser_line_extraction, but with the following differences 4 | 5 | 1. Core implementation (in [include/impl](include/impl) and [src/impl](src/impl)) can be compiled without ros 6 | 2. Error propagation model is different: we allow users to specify difference covariance matrices for each point 7 | 3. Agglomerative clustering can be optionally enabled before line segment extraction 8 | 9 | By default, the split-and-merge with clustering (smc) algorithm is used to perform line segment extraction because of its efficiency and performance. However, an implementation of prototype based fuzzing (also named as split-and-merge fuzzing, smf) is provided. If you wish to test this implementation, change includes of "smc.h" to "smf.h" and link against "smf" instead. 10 | 11 | ## Configurations 12 | 13 | See [config/example.yaml](config/example.yaml) for all configurable parameters and their meaning 14 | 15 | ## Launch files 16 | 17 | An example ROS node that subscribes to LaserScan together with a launch file is provided. See (src/ls_extractor/launch/laser.launch)[src/ls_extractor/launch/laser.launch]. Note that it puts scaled identity matrices as the covariance for each point. You can adapt the code for your need. 18 | 19 | ```bash 20 | roslaunch ls_extractor laser.launch 21 | ``` -------------------------------------------------------------------------------- /src/ls_extractor/config/example.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /LaserScan1 10 | Splitter Ratio: 0.5 11 | Tree Height: 798 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: LaserScan 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/Marker 57 | Enabled: true 58 | Marker Topic: /ls/segment_markers 59 | Name: Marker 60 | Namespaces: 61 | base_link/segment_markers: true 62 | Queue Size: 100 63 | Value: true 64 | - Alpha: 1 65 | Autocompute Intensity Bounds: true 66 | Autocompute Value Bounds: 67 | Max Value: 10 68 | Min Value: -10 69 | Value: true 70 | Axis: Z 71 | Channel Name: intensity 72 | Class: rviz/LaserScan 73 | Color: 255; 255; 255 74 | Color Transformer: Intensity 75 | Decay Time: 0 76 | Enabled: true 77 | Invert Rainbow: false 78 | Max Color: 255; 255; 255 79 | Min Color: 0; 0; 0 80 | Name: LaserScan 81 | Position Transformer: XYZ 82 | Queue Size: 10 83 | Selectable: true 84 | Size (Pixels): 3 85 | Size (m): 0.029999999329447746 86 | Style: Flat Squares 87 | Topic: /full_scan 88 | Unreliable: false 89 | Use Fixed Frame: true 90 | Use rainbow: true 91 | Value: true 92 | Enabled: true 93 | Global Options: 94 | Background Color: 48; 48; 48 95 | Default Light: true 96 | Fixed Frame: map 97 | Frame Rate: 30 98 | Name: root 99 | Tools: 100 | - Class: rviz/Interact 101 | Hide Inactive Objects: true 102 | - Class: rviz/MoveCamera 103 | - Class: rviz/Select 104 | - Class: rviz/FocusCamera 105 | - Class: rviz/Measure 106 | - Class: rviz/SetInitialPose 107 | Theta std deviation: 0.2617993950843811 108 | Topic: /initialpose 109 | X std deviation: 0.5 110 | Y std deviation: 0.5 111 | - Class: rviz/SetGoal 112 | Topic: /move_base_simple/goal 113 | - Class: rviz/PublishPoint 114 | Single click: true 115 | Topic: /clicked_point 116 | Value: true 117 | Views: 118 | Current: 119 | Class: rviz/Orbit 120 | Distance: 16.406309127807617 121 | Enable Stereo Rendering: 122 | Stereo Eye Separation: 0.05999999865889549 123 | Stereo Focal Distance: 1 124 | Swap Stereo Eyes: false 125 | Value: false 126 | Field of View: 0.7853981852531433 127 | Focal Point: 128 | X: 6.6742095947265625 129 | Y: 1.502950668334961 130 | Z: -0.12503452599048615 131 | Focal Shape Fixed Size: true 132 | Focal Shape Size: 0.05000000074505806 133 | Invert Z Axis: false 134 | Name: Current View 135 | Near Clip Distance: 0.009999999776482582 136 | Pitch: 1.5697963237762451 137 | Target Frame: 138 | Yaw: 0.12039850652217865 139 | Saved: ~ 140 | Window Geometry: 141 | Displays: 142 | collapsed: false 143 | Height: 1089 144 | Hide Left Dock: false 145 | Hide Right Dock: false 146 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008000000003efc0100000002fb0000000800540069006d00650100000000000008000000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000058f000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 147 | Selection: 148 | collapsed: false 149 | Time: 150 | collapsed: false 151 | Tool Properties: 152 | collapsed: false 153 | Views: 154 | collapsed: false 155 | Width: 2048 156 | X: 0 157 | Y: 23 158 | -------------------------------------------------------------------------------- /src/ls_extractor/config/example.yaml: -------------------------------------------------------------------------------- 1 | # the maximum distance allowed between any 2 points that constitude a line segment 2 | max_line_gap: 0.3 3 | # minimum allowed line length 4 | min_line_length: 0.75 5 | # split a segment when the distance from any point on this segment to this segment is greater than this value 6 | min_split_dist: 0.1 7 | # a point is considered a outlier if the distance between it and any of its two neighbors is greater than this value 8 | outlier_dist: 0.1 9 | # minimum allowed number of points for a line segment 10 | min_line_points: 10 11 | # unused config. please ignore 12 | rmse_thresh: 0.1 13 | # the distance threshold between 2 clusters. Setting a large value effectively disable clustering 14 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/ls_extractor/include/ls_extractor/defs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace ls_extractor { 7 | using namespace std; 8 | 9 | // note that we store both the Cartesian and polar coordinate 10 | // because we use both form to perform computations 11 | struct Point { 12 | Eigen::Vector2f point; // (x, y) coordinate 13 | // (rho, theta) polar coordinate 14 | // equals to (sqrt(x^2+y^2), atan2(y, x)) 15 | Eigen::Vector2f rhotheta; 16 | // covariance matrix of the (x, y) coordinate 17 | Eigen::Matrix2f cov; 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 19 | }; 20 | 21 | struct Params { 22 | float outlier_dist = 0.1; 23 | float min_split_dist = 0.15; 24 | float max_line_gap = 0.25; 25 | float min_line_length = 0.5; 26 | float rmse_thresh = 0.1; 27 | float cluster_threshold = 0.5; 28 | int min_line_points = 10; 29 | }; 30 | 31 | struct _LineSegment { 32 | public: 33 | Eigen::Vector2f start_, end_; 34 | Eigen::Vector2f rhotheta; 35 | Eigen::Matrix2f cov; 36 | protected: 37 | inline float distToPoint(const Point& point) const { 38 | return abs(point.rhotheta[0] * cos(point.rhotheta[1] - rhotheta[1]) - rhotheta[0]); 39 | } 40 | }; 41 | 42 | using PointVector = vector> ; 43 | using pit = PointVector::const_iterator; 44 | using PPtrVector = vector; 45 | } -------------------------------------------------------------------------------- /src/ls_extractor/include/ls_extractor/impl/smc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "ls_extractor/defs.h" 5 | namespace ls_extractor { 6 | using namespace std; 7 | 8 | class LineSegment : public _LineSegment { 9 | public: 10 | LineSegment() {} 11 | LineSegment(pit start, pit end) : start(start), end(end) { 12 | } 13 | 14 | private: 15 | pit start, end; 16 | void endpointFit(); 17 | float gapBetween(const LineSegment& other) const; 18 | void leastSqFit(); 19 | void projectEndpoints(); 20 | inline float length() const { 21 | return (start->point - (end - 1)->point).norm(); 22 | } 23 | friend class LineSegmentExtractor; 24 | }; 25 | // TODO: in theory we need an aligned allocator here, but it will cause a segfault when the destructor is called. Not sure why. 26 | using SegmentVector = vector; 27 | class LineSegmentExtractor { 28 | public: 29 | SegmentVector segments; 30 | Params params; 31 | vector splits; 32 | 33 | void extract_lines(PointVector& points); 34 | 35 | private: 36 | void extract_lines_helper(PointVector& points); 37 | void split(pit start, pit end); 38 | void merge(); 39 | void merge_pairwise(); 40 | unordered_map cluster(const PointVector& points); 41 | 42 | vector ranks, parents; 43 | }; 44 | } // namespace ls_extractor -------------------------------------------------------------------------------- /src/ls_extractor/include/ls_extractor/impl/smf.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "ls_extractor/defs.h" 6 | 7 | namespace ls_extractor { 8 | using namespace std; 9 | 10 | float pairwise_closest(const vector& points1, const vector& points2); 11 | 12 | struct LineSegment : public _LineSegment { 13 | vector points; 14 | vector uj, dj; 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 16 | LineSegment() = default; 17 | LineSegment(const PointVector& points); 18 | LineSegment(const vector& points); 19 | 20 | private: 21 | float calc_xybar(Eigen::Vector2f& xybar, float m) const; 22 | void calc_dij(const Eigen::Vector2f& xybar); 23 | float calc_dispersion() const; 24 | 25 | void leastSqFit(const float m, bool calc_dij_flag = true, bool calc_cov = false); 26 | void projectEndpoints(); 27 | 28 | bool satisfy_param(float max_gap, float min_length) const; 29 | friend class LineSegmentExtractor; 30 | }; 31 | using SegmentVector = std::vector; 32 | class LineSegmentExtractor { 33 | public: 34 | SegmentVector segments; 35 | Params params; 36 | 37 | void extract_lines(PointVector& points); 38 | 39 | void merge(); 40 | void merge2(); 41 | 42 | // prototype based fuzzying 43 | void PBF(LineSegment& segment, float m = 2.0f); 44 | }; 45 | } // namespace ls_extractor -------------------------------------------------------------------------------- /src/ls_extractor/include/ls_extractor/ros_utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ls_extractor/defs.h" 3 | 4 | namespace ls_extractor { 5 | 6 | void parseParams(Params& params, const XmlRpc::XmlRpcValue& config) { 7 | params.max_line_gap = (double)config["max_line_gap"]; 8 | params.min_line_length = (double)config["min_line_length"]; 9 | params.min_line_points = config["min_line_points"]; 10 | params.cluster_threshold = (double)config["cluster_threshold"]; 11 | params.min_split_dist = (double)config["min_split_dist"]; 12 | params.outlier_dist = (double)config["outlier_dist"]; 13 | params.rmse_thresh = (double)config["rmse_thresh"]; 14 | } 15 | 16 | } -------------------------------------------------------------------------------- /src/ls_extractor/include/ls_extractor/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace ls_extractor { 6 | inline void eigenToCov2d(const Eigen::Matrix2f& cov, boost::array& lcov) { 7 | lcov[0] = cov(0, 0); 8 | lcov[1] = cov(0, 1); 9 | lcov[2] = cov(1, 0); 10 | lcov[3] = cov(1, 1); 11 | } 12 | 13 | inline void cov2dToEigen(const boost::array& lcov, Eigen::Matrix2f& cov) { 14 | cov(0, 0) = lcov[0]; 15 | cov(0, 1) = lcov[1]; 16 | cov(1, 0) = lcov[2]; 17 | cov(1, 1) = lcov[3]; 18 | } 19 | template 20 | using Vector2 = Eigen::Matrix; 21 | 22 | template 23 | inline void checkRhoTheta(Eigen::Ref> rhotheta) { 24 | if (rhotheta[0] < 0.0) { 25 | rhotheta[0] = -rhotheta[0]; 26 | rhotheta[1] += M_PI; 27 | if (rhotheta[1] > M_PI) 28 | rhotheta[1] -= 2 * M_PI; 29 | } 30 | } 31 | 32 | template 33 | Vector2 transform_line(const Vector2& rhotheta, const Vector2& trans, T angle) { 34 | Vector2 result = rhotheta; 35 | result[1] += angle; 36 | if (result[1] > M_PI) 37 | result[1] -= 2 * M_PI; 38 | if (result[1] < -M_PI) 39 | result[1] += 2 * M_PI; 40 | 41 | Vector2 normal(cos(result[1]), sin(result[1])); 42 | result[0] += trans.dot(normal); 43 | checkRhoTheta(result); 44 | return result; 45 | } 46 | 47 | template 48 | inline Vector2 topolar(const Eigen::Ref> start, const Eigen::Ref> end) { 49 | Vector2 result, d = start - end; 50 | result[1] = atan2(-d[0], d[1]); 51 | result[0] = start[0] * cos(result[1]) + start[1] * sin(result[1]); 52 | checkRhoTheta(result); 53 | return result; 54 | } 55 | 56 | template 57 | inline void calc_start_dir(const Eigen::Ref> rhotheta, Vector2& start, Vector2& dir) { 58 | Vector2 cos_sin(cos(rhotheta[1]), sin(rhotheta[1])); 59 | dir = {-cos_sin[1], cos_sin[0]}; 60 | start = rhotheta[0] * cos_sin; 61 | } 62 | 63 | template 64 | T ll_distance(const Eigen::Ref> rhotheta, 65 | const Eigen::Ref> p1, 66 | const Eigen::Ref> p2, 67 | Eigen::Ref> tout) { 68 | Vector2 start, dir; 69 | calc_start_dir(rhotheta, start, dir); 70 | 71 | Vector2 dvec1 = p1 - start; 72 | tout[0] = dvec1.dot(dir); 73 | Vector2 dvec2 = p2 - start; 74 | tout[1] = dvec2.dot(dir); 75 | 76 | T error = (dvec1 - tout[0] * dir).norm() + (dvec2 - tout[1] * dir).norm(); 77 | if (tout[1] < tout[0]) 78 | std::swap(tout[0], tout[1]); 79 | return error; 80 | } 81 | 82 | template 83 | inline void calc_endpoints(const Eigen::Ref> start, 84 | const Eigen::Ref> dir, 85 | const Eigen::Ref> p1, 86 | const Eigen::Ref> p2, 87 | Eigen::Ref> tout) { 88 | tout[0] = (p1 - start).dot(dir); 89 | tout[1] = (p2 - start).dot(dir); 90 | if (tout[1] < tout[0]) 91 | std::swap(tout[0], tout[1]); 92 | } 93 | 94 | template 95 | inline void calc_endpoints(const Eigen::Ref> rhotheta, 96 | const Eigen::Ref> p1, 97 | const Eigen::Ref> p2, 98 | Eigen::Ref> tout) { 99 | Vector2 start, dir; 100 | calc_start_dir(rhotheta, start, dir); 101 | calc_endpoints(start, dir, p1, p2, tout); 102 | } 103 | } // namespace ls_extractor -------------------------------------------------------------------------------- /src/ls_extractor/include/ls_extractor/visualizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "ls_extractor/impl/smc.h" 3 | #include 4 | #include 5 | #include 6 | 7 | namespace ls_extractor { 8 | 9 | class SegmentVisualizer { 10 | public: 11 | ros::Publisher seg_marker_pub, split_marker_pub; 12 | visualization_msgs::Marker seg_markers, split_markers; 13 | SegmentVisualizer(const string& frame_id, ros::NodeHandle& nh); 14 | void visualize(const SegmentVector& segments, const ros::Time& stamp); 15 | }; 16 | 17 | class PCVisualizer { 18 | std::string frame_id; 19 | ros::Publisher pub; 20 | visualization_msgs::MarkerArray markers; 21 | PCVisualizer(const string& frame_id, ros::NodeHandle& nh); 22 | void visualize(const PointVector& pcv, const ros::Time& stamp, double scale); 23 | }; 24 | 25 | } 26 | 27 | -------------------------------------------------------------------------------- /src/ls_extractor/launch/laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | baselink 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/ls_extractor/msg/LineSegment.msg: -------------------------------------------------------------------------------- 1 | float32 radius 2 | float32 angle 3 | float32[4] covariance 4 | float32[2] start 5 | float32[2] end 6 | -------------------------------------------------------------------------------- /src/ls_extractor/msg/LineSegmentList.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | ls_extractor/LineSegment[] line_segments 3 | -------------------------------------------------------------------------------- /src/ls_extractor/msg/PointCloudWithCov.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | ls_extractor/PointWithCov[] points -------------------------------------------------------------------------------- /src/ls_extractor/msg/PointWithCov.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32[4] cov -------------------------------------------------------------------------------- /src/ls_extractor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ls_extractor 4 | 0.0.0 5 | The ls_extractor package 6 | 7 | 8 | 9 | 10 | hanzhi713 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | roscpp 54 | roscpp 55 | message_generation 56 | message_runtime 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/ls_extractor/src/laser_node.cpp: -------------------------------------------------------------------------------- 1 | #include "ls_extractor/impl/smc.h" 2 | #include "ls_extractor/ros_utils.h" 3 | #include "ls_extractor/visualizer.h" 4 | #include 5 | #include 6 | 7 | using namespace ls_extractor; 8 | 9 | LineSegmentExtractor extractor; 10 | PointVector pcv; 11 | 12 | void callback(SegmentVisualizer& vis, sensor_msgs::LaserScanConstPtr _scan) { 13 | pcv.clear(); 14 | const auto& scan = *_scan; 15 | float angle = scan.angle_min; 16 | for (int i = 0; i < scan.ranges.size(); i++, angle += scan.angle_increment) { 17 | float range = scan.ranges[i]; 18 | if (range >= scan.range_min && range <= scan.range_max) { 19 | pcv.emplace_back(); 20 | auto& pt = pcv.back(); 21 | pt.point[0] = cos(angle) * range; 22 | pt.point[1] = sin(angle) * range; 23 | pt.rhotheta[0] = range; 24 | pt.rhotheta[1] = angle; 25 | pt.cov = Eigen::Matrix2f::Identity() * 0.01; 26 | } 27 | } 28 | extractor.extract_lines(pcv); 29 | vis.visualize(extractor.segments, scan.header.stamp); 30 | } 31 | 32 | int main(int argc, char** argv) { 33 | ros::init(argc, argv, "ls_extractor_laser_node"); 34 | ros::NodeHandle nh; 35 | XmlRpc::XmlRpcValue config; 36 | std::string frame_id; 37 | ros::param::get("~config", config); 38 | ros::param::get("~frame_id", frame_id); 39 | SegmentVisualizer vis(frame_id, nh); 40 | auto sub = nh.subscribe("scan", 1, boost::bind(callback, vis, _1)); 41 | ros::spin(); 42 | return 0; 43 | } -------------------------------------------------------------------------------- /src/ls_extractor/src/ls_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "ls_extractor/impl/smc.h" 5 | #include "ls_extractor/matplotlibcpp.h" 6 | 7 | namespace plt = matplotlibcpp; 8 | 9 | struct Line { 10 | double t1, t2; 11 | Eigen::Vector2d start, dir; 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 13 | 14 | inline double pl_distance(const Eigen::Vector2d& point, double& t) const { 15 | auto dvec = point - start; 16 | t = dvec.dot(dir); 17 | return (dvec - t * dir).norm(); 18 | } 19 | 20 | void normalize() { 21 | dir /= dir.norm(); 22 | } 23 | 24 | void plot(const std::string& key) const { 25 | Eigen::Vector2d start_p = start + t1 * dir; 26 | Eigen::Vector2d end_p = start + t2 * dir; 27 | // cout << start_p << "|" << end_p << endl; 28 | plt::plot({start_p[0], end_p[0]}, {start_p[1], end_p[1]}, key); 29 | } 30 | 31 | double length() const { 32 | return t2 - t1; 33 | } 34 | 35 | Eigen::Vector2d midpoint() const { 36 | return ((t1 + t2) * 0.5) * dir + start; 37 | } 38 | 39 | void recalc_start() { 40 | double t_start = start.dot(dir); 41 | t1 += t_start; 42 | t2 += t_start; 43 | start -= t_start * dir; 44 | } 45 | }; 46 | 47 | using LineVector = std::vector>; 48 | int main() { 49 | // plt::ion() 50 | 51 | plt::figure_size(720, 720); 52 | // plt::figure(); 53 | ls_extractor::LineSegmentExtractor extractor; 54 | LineVector lines; 55 | 56 | std::default_random_engine gen(std::chrono::system_clock::now().time_since_epoch().count()); 57 | std::normal_distribution dist(0.0, 0.03); 58 | std::normal_distribution dist2(0.0, 0.05); 59 | 60 | lines.push_back({0.0, 2.0, {-1.5, -1.5}, {0.0, 1.0}}); 61 | lines.push_back({0.0, 2.5, {0.5, -1.5}, {0.1, 0.9}}); 62 | lines.push_back({0.0, 1.0, {0.5, -1.5}, {0.9, -0.2}}); 63 | lines.push_back({0.0, 3.0, {-1.0, 1.0}, {0.9, 0.2}}); 64 | // lines.push_back({0.0, 3.0, {-1.0, 1.0}, {0.5, -0.5}}); 65 | for (auto& line : lines) { 66 | line.normalize(); 67 | line.plot("r-"); 68 | } 69 | 70 | Eigen::Matrix2f cov = (0.01 * 0.01) * Eigen::Matrix2f::Identity(); 71 | std::vector px, py; 72 | ls_extractor::PointVector pv; 73 | for (auto& line : lines) { 74 | for (double t = line.t1; t < line.t2; t += 0.05) { 75 | Eigen::Vector2f point = (line.start + t * line.dir).cast(); 76 | Eigen::Vector2f normal(-line.dir[1], line.dir[0]); 77 | point += normal * dist(gen); 78 | pv.push_back({point, {point.norm(), atan2(point[1], point[0])}, cov}); 79 | px.push_back(point[0]); 80 | py.push_back(point[1]); 81 | } 82 | } 83 | 84 | std::shuffle(pv.begin(), pv.end(), gen); 85 | extractor.extract_lines(pv); 86 | 87 | plt::scatter(px, py, 4.0); 88 | px.clear(); 89 | py.clear(); 90 | for (auto& seg : extractor.segments) { 91 | plt::plot({seg.start_[0], seg.end_[0]}, {seg.start_[1], seg.end_[1]}, "b-"); 92 | } 93 | 94 | plt::show(); 95 | return 0; 96 | } -------------------------------------------------------------------------------- /src/ls_extractor/src/visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "ls_extractor/visualizer.h" 2 | 3 | #include 4 | 5 | using namespace std; 6 | namespace ls_extractor { 7 | SegmentVisualizer::SegmentVisualizer( 8 | const string& frame_id, 9 | ros::NodeHandle& nh) : seg_marker_pub(nh.advertise("segment_markers", 2)) { 10 | seg_markers.header.frame_id = frame_id; 11 | seg_markers.ns = frame_id + "/segment_markers"; 12 | seg_markers.id = 1; 13 | seg_markers.type = visualization_msgs::Marker::LINE_LIST; 14 | seg_markers.scale.x = 0.075; 15 | seg_markers.color.r = 1.0; 16 | seg_markers.color.g = 0.0; 17 | seg_markers.color.b = 0.0; 18 | seg_markers.color.a = 1.0; 19 | seg_markers.pose.orientation.w = 1.0; 20 | } 21 | 22 | void SegmentVisualizer::visualize(const SegmentVector& segments, const ros::Time& stamp) { 23 | split_markers.header.stamp = seg_markers.header.stamp = stamp; 24 | // markers.id++; 25 | seg_markers.points.resize(segments.size() * 2); 26 | for (int i = 0; i < segments.size(); i++) { 27 | seg_markers.points[2 * i].x = segments[i].start_[0]; 28 | seg_markers.points[2 * i].y = segments[i].start_[1]; 29 | seg_markers.points[2 * i + 1].x = segments[i].end_[0]; 30 | seg_markers.points[2 * i + 1].y = segments[i].end_[1]; 31 | } 32 | seg_marker_pub.publish(seg_markers); 33 | } 34 | 35 | PCVisualizer::PCVisualizer(const string& frame_id, ros::NodeHandle& nh) : frame_id(frame_id), 36 | pub(nh.advertise("pcv", 2)) { 37 | } 38 | 39 | void PCVisualizer::visualize(const PointVector& pcv, const ros::Time& stamp, double scale) { 40 | markers.markers.resize(pcv.size()); 41 | for (int i = 0; i < pcv.size(); i++) { 42 | auto& pt = pcv[i]; 43 | auto& marker = markers.markers[i]; 44 | marker.header.frame_id = frame_id; 45 | marker.header.stamp = stamp; 46 | marker.ns = frame_id + "/pcv"; 47 | marker.id = i; 48 | marker.type = visualization_msgs::Marker::CYLINDER; 49 | 50 | const auto& cov = pt.cov; 51 | float apc2 = 0.5 * (cov(0, 0) + cov(1, 1)); // (a+c)/2 52 | float amc2 = 0.5 * (cov(0, 0) - cov(1, 1)); // (a-c)/2 53 | float temp = sqrt(amc2 * amc2 + cov(0, 1) * cov(0, 1)); 54 | float l1 = apc2 + temp, l2 = apc2 - temp; // 2 eigenvalues 55 | marker.scale.x = sqrt(l1) * scale; 56 | marker.scale.y = sqrt(l2) * scale; 57 | 58 | float theta; 59 | if (abs(cov(0, 1)) < 1e-8) { 60 | theta = cov(0, 0) >= cov(1, 1) ? 0.0 : M_PI / 2; 61 | } else { 62 | theta = atan2(l1 - cov(0, 0), cov(0, 1)); 63 | } 64 | 65 | tf::Matrix3x3 mat; 66 | tf::Quaternion q; 67 | mat.setRPY(0.0, 0.0, theta); 68 | mat.getRotation(q); 69 | tf::quaternionTFToMsg(q, marker.pose.orientation); 70 | 71 | marker.scale.z = 0.1; 72 | marker.color.r = 0.0; 73 | marker.color.g = 1.0; 74 | marker.color.b = 1.0; 75 | marker.color.a = 1.0; 76 | 77 | marker.pose.position.x = pt.point[0]; 78 | marker.pose.position.y = pt.point[1]; 79 | } 80 | pub.publish(markers); 81 | } 82 | } // namespace ls_extractor -------------------------------------------------------------------------------- /src/sparse_gslam/cmake/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) -------------------------------------------------------------------------------- /src/sparse_gslam/config/gamepad.yaml: -------------------------------------------------------------------------------- 1 | deadzone: 0.1 2 | coalesce_interval: 0.02 3 | autorepeat_rate: 50 -------------------------------------------------------------------------------- /src/sparse_gslam/config/swarm.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E703 2 | teleop: true 3 | hover_before_start: false 4 | gamepad: true 5 | height: 0.5 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.75 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-10.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 10 13 | multicloud_size: 160 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.575 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-11.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 11 13 | multicloud_size: 176 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-13.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 13 13 | multicloud_size: 169 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.575 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 6.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-30.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 0.5 4 | std_w: 0.5 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 30 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.8 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.08 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 1.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-4.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 4 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.5 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-5.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 5 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.5 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 12.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-6.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 6 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | 41 | match_interval: 10 42 | data_provider: carmen 43 | 44 | # visualization settings 45 | visualize_rate: 2 46 | map_resolution: 0.1 47 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-7.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 7 13 | multicloud_size: 126 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/aces/slam-8.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.5 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 8 13 | multicloud_size: 144 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.525 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 20 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/albertb/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/albertb/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.8 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 11 13 | multicloud_size: 176 14 | 15 | landmark_max_gap: 4.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | last_submap_not_match: 3 25 | branch_and_bound_depth: 5 26 | 27 | # settings for the occupancy grid 28 | hit_probability: 0.7 29 | miss_probability: 0.4 30 | 31 | # settings for the submap builder 32 | max_match_distance: 10.0 # meters 33 | submap_resolution: 0.1 # meters 34 | submap_trajectory_length: 6.0 # meters 35 | submap_overlap_poses: 2 # number of poses 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/calc_time.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | import sys 4 | import matplotlib.pyplot as plt 5 | import matplotlib 6 | 7 | datasets = ["aces", "intel-lab", "mit-killian"] 8 | # dataset = sys.argv[1] 9 | 10 | # dtimes = [] 11 | ftimes = [] 12 | btimes = [] 13 | for dataset in datasets: 14 | ftimes.append(np.loadtxt(os.path.join(dataset, f"{dataset}.ftime"))) 15 | btimes.append(np.loadtxt(os.path.join(dataset, f"{dataset}.btime"))) 16 | 17 | print( 18 | dataset, 19 | np.average(np.diff(np.loadtxt(os.path.join(dataset, f"{dataset}.dtime")))), 20 | np.max(ftimes[-1]), 21 | np.max(btimes[-1]), 22 | (np.sum(ftimes[-1]) + np.sum(btimes[-1])) / len(ftimes[-1]) 23 | ) 24 | 25 | 26 | 27 | fig1, ax1 = plt.subplots(figsize=(3.0, 2.2)) 28 | plt.subplots_adjust(left=0.2, bottom=0.2, right=0.99, top=0.99, wspace=0.3, hspace=None) 29 | ax1.hist(ftimes, np.logspace(-4, -1.5, 40), stacked=True) 30 | ax1.set_xscale("log") 31 | f_ticks =[0.0001, 0.001, 0.01, 0.03] 32 | ax1.set_xticks(f_ticks) 33 | ax1.set_xticklabels([str(x) for x in f_ticks]) 34 | ax1.legend(["Aces", "Intel Lab", "MIT Killian"]) 35 | ax1.set_xlabel("Processing time (s)") 36 | ax1.set_ylabel("Frequency") 37 | plt.savefig("frontend.png") 38 | 39 | fig1, ax1 = plt.subplots(figsize=(3.0, 2.2)) 40 | plt.subplots_adjust(left=0.2, bottom=0.2, right=0.99, top=0.99, wspace=0.3, hspace=None) 41 | ax1.hist(btimes, np.logspace(-3, 0, 40), stacked=True) 42 | ax1.set_xscale("log") 43 | b_ticks = [0.001, 0.01, 0.1, 0.7] 44 | ax1.set_xticks(b_ticks) 45 | ax1.set_xticklabels([str(x) for x in b_ticks]) 46 | # ax1.legend(["Aces", "Intel Lab", "MIT Killian"]) 47 | ax1.set_xlabel("Processing time (s)") 48 | ax1.set_ylabel("Frequency") 49 | plt.savefig("backend.png") 50 | plt.show() -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/download.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | wget -O aces/aces.log http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/aces.clf 3 | wget -O aces/aces.relations http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/aces.relations 4 | 5 | wget -O intel-lab/intel-lab.log http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/intel.clf 6 | wget -O intel-lab/intel-lab.relations http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/intel.relations 7 | 8 | wget -O mit-killian/mit-killian.log http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/mit-killian.clf 9 | wget -O mit-killian/mit-killian.relations http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/mit-killian.relations 10 | 11 | wget -O mit-csail/mit-csail.log http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/mit-csail.clf 12 | wget -O mit-csail/mit-csail.relations http://ais.informatik.uni-freiburg.de/slamevaluation/datasets/mit-csail.relations 13 | 14 | cd usc-sal 15 | wget -O usc-sal.log.gz https://dspace.mit.edu/bitstream/handle/1721.1/62281/run02.log.gz 16 | gzip -d -f usc-sal.log.gz 17 | cd .. 18 | 19 | cd nsh_level_a 20 | wget -O nsh_level_a.log.gz https://dspace.mit.edu/bitstream/handle/1721.1/62266/nsh_level_a.log.gz 21 | gzip -d -f nsh_level_a.log.gz 22 | cd .. 23 | 24 | cd stanford-gates 25 | wget -O stanford-gates.log.gz https://dspace.mit.edu/bitstream/handle/1721.1/62252/stanford-gates1.log.gz 26 | gzip -d stanford-gates.log.gz 27 | cd .. 28 | 29 | cd intel-oregon 30 | wget -O intel-oregon.log.gz https://dspace.mit.edu/bitstream/handle/1721.1/62253/intel-01-sorted.log.gz 31 | gzip -d intel-oregon.log.gz 32 | cd .. 33 | 34 | cd albertb 35 | wget -O albertb.log.gz https://dspace.mit.edu/bitstream/handle/1721.1/62265/albertb.sm.log.gz 36 | gzip -d albertb.log.gz 37 | cd .. 38 | 39 | rm -rf metricEvaluator 40 | git clone https://github.com/shiftlab-nanodrone/slam-metric-evaluator metricEvaluator 41 | cd metricEvaluator 42 | make -j4 43 | cd .. -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/eval.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ./metricEvaluator/metricEvaluator -s ./$1/$1.result -r ./$1/$1.relations -w "{1.0,1.0,1.0,0.0,0.0,0.0}" -e ./$1/$1-$2_trans_error.log -o ./$1/$1.log 3 | ./metricEvaluator/metricEvaluator -s ./$1/$1.result -r ./$1/$1.relations -w "{0.0,0.0,0.0,1.0,1.0,1.0}" -e ./$1/$1-$2_rot_error.log -o ./$1/$1.log 4 | # ./metricEvaluator/metricEvaluator -q -s ./$1/$1.result -r ./$1/$1.relations -w "{1.0,1.0,1.0,0.0,0.0,0.0}" -e ./$1/$1_sq_trans_error.log -o ./$1/$1.log 5 | # ./metricEvaluator/metricEvaluator -q -s ./$1/$1.result -r ./$1/$1.relations -w "{0.0,0.0,0.0,1.0,1.0,1.0}" -e ./$1/$1_sq_rot_error.log -o ./$1/$1.log 6 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/fixlog.py: -------------------------------------------------------------------------------- 1 | from spatialmath import SE2 2 | import sys 3 | import os 4 | 5 | def read_as_dict(f): 6 | r_lines = f.readlines() 7 | r_lines = [line.split(" ") for line in r_lines if line.startswith("FLASER")] 8 | t_dict = dict() 9 | for line in r_lines: 10 | time = round(float(line[-3]), 6) 11 | t_dict[time] = SE2( 12 | float(line[-6]), float(line[-5]), float(line[-4]) 13 | ) 14 | return t_dict 15 | 16 | dataset, in_f = os.path.split(sys.argv[1]) 17 | with open(sys.argv[1], "r") as f: 18 | r_dict = read_as_dict(f) 19 | 20 | with open(os.path.join(dataset, f"{dataset}.log"), "r") as f: 21 | l_dict = read_as_dict(f) 22 | 23 | for key in r_dict: 24 | assert key in l_dict 25 | 26 | r_dict = list(r_dict.items()) 27 | r_dict.sort(key=lambda x:x[0]) 28 | l_dict = list(l_dict.items()) 29 | l_dict.sort(key=lambda x:x[0]) 30 | 31 | r_time, r_se2 = r_dict[0] 32 | new_results = [(r_time, r_se2.xyt())] 33 | l_idx = 0 34 | r_idx = 0 35 | while r_dict[r_idx][0] != l_dict[l_idx][0]: 36 | l_idx += 1 37 | print(l_idx, r_idx) 38 | last_raw_odom: SE2 = l_dict[l_idx][1] 39 | r_idx += 1 40 | l_idx += 1 41 | r_time, r_se2 = r_dict[r_idx] 42 | while True: 43 | while r_time != l_dict[l_idx][0]: 44 | l_time, raw_odom = l_dict[l_idx] 45 | new_results.append(( 46 | l_time, (r_dict[r_idx - 1][1] * last_raw_odom.inv() * raw_odom).xyt() 47 | )) 48 | l_idx += 1 49 | last_raw_odom = l_dict[l_idx][1] 50 | new_results.append((r_time, r_se2.xyt())) 51 | r_idx += 1 52 | l_idx += 1 53 | if r_idx >= len(r_dict): 54 | break 55 | r_time, r_se2 = r_dict[r_idx] 56 | 57 | with open(os.path.join(dataset, f"{dataset}.result"), "w") as f: 58 | for row in new_results: 59 | time, (x, y, theta) = row 60 | f.write( 61 | f"FLASER 0 {x} {y} {theta} {x} {y} {theta} {time} myhost {time}\n" 62 | ) 63 | 64 | # print(r_dict.keys()) 65 | # new_lines = [] 66 | # for line in lines: 67 | # line = line.split(" ") 68 | # line = line[:2] + line[2:5] + line[2:5] + line[5:] 69 | # new_lines.append(" ".join(line)) 70 | # print(new_lines[0]) 71 | # with open("intel-lab.result", "w") as f: 72 | # f.writelines(new_lines) -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/gen_acc_table.py: -------------------------------------------------------------------------------- 1 | def get_error_std(fname): 2 | return list(map(lambda x: float(x), open(fname).readlines()[1].split(", ")[:2])) 3 | 4 | tb = """ 5 | \\begin{tabular}{@{}ccc@{}} 6 | \\toprule 7 | & \multicolumn{1}{c}{Ours} & \multicolumn{1}{c}{Gmapping} \\\\ \midrule 8 | \multicolumn{1}{l}{Aces} & \multicolumn{1}{c}{4pt} & \multicolumn{1}{c}{30pt} \\\\ 9 | \multicolumn{1}{l}{\quad Absolute translational} & \multicolumn{1}{c}{$%0.4f \pm %0.4f$} & \multicolumn{1}{c}{$%0.4f \pm %0.4f$} \\\\ 10 | \multicolumn{1}{l}{\quad Absolute rotational} & \multicolumn{1}{c}{$%0.3f \pm %0.3f$} & \multicolumn{1}{c}{$%0.3f \pm %0.3f$} \\\\ 11 | \midrule 12 | \multicolumn{1}{l}{Intel-Lab} & \multicolumn{1}{c}{13pt} & \multicolumn{1}{c}{30pt} \\\\ 13 | \multicolumn{1}{l}{\quad Absolute translational} & \multicolumn{1}{c}{$%0.4f \pm %0.4f$} & \multicolumn{1}{c}{$%0.4f \pm %0.4f$} \\\\ 14 | \multicolumn{1}{l}{\quad Absolute rotational} & \multicolumn{1}{c}{$%0.3f \pm %0.3f$} & \multicolumn{1}{c}{$%0.3f \pm %0.3f$} \\\\ 15 | \midrule 16 | \multicolumn{1}{l}{MIT-Killian} & \multicolumn{1}{c}{11pt} & \multirow{3}{*}{Fail to produce map} \\\\ 17 | \multicolumn{1}{l}{\quad Absolute translational} & \multicolumn{1}{c}{$%0.4f \pm %0.4f$} & \\\\ 18 | \multicolumn{1}{l}{\quad Absolute rotational} & \multicolumn{1}{c}{$%0.3f \pm %0.3f$} & \\\\ 19 | \\bottomrule 20 | \end{tabular} 21 | """ % ( 22 | *get_error_std("aces/aces-11_trans_error.log"), 23 | *get_error_std("aces/aces-gmapping-30_trans_error.log"), 24 | *get_error_std("aces/aces-11_rot_error.log"), 25 | *get_error_std("aces/aces-gmapping-30_rot_error.log"), 26 | 27 | *get_error_std("intel-lab/intel-lab-11_trans_error.log"), 28 | *get_error_std("intel-lab/intel-lab-gmapping-30_trans_error.log"), 29 | *get_error_std("intel-lab/intel-lab-11_rot_error.log"), 30 | *get_error_std("intel-lab/intel-lab-gmapping-30_rot_error.log"), 31 | 32 | *get_error_std("mit-killian/mit-killian-11_trans_error.log"), 33 | *get_error_std("mit-killian/mit-killian-11_rot_error.log") 34 | ) 35 | print(tb) -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/gen_time_table.py: -------------------------------------------------------------------------------- 1 | def get_our_time(fname): 2 | c = open(f"{fname}/{fname}.time").readlines()[1].split(",") 3 | return float(c[0]), float(c[1]) # , float(c[2]) * 1000 4 | 5 | table = """ 6 | \\begin{tabular}{@{}lllll@{}} 7 | \\toprule 8 | Dataset & Data duration & Wall clock & GMapping & Cartographer \\\\ \\midrule 9 | USC SAL & %0.1f & %0.1f & - & \\\\ 10 | CMU NSH & %0.1f & %0.1f & - & \\\\ 11 | Stanford Gates & %0.1f & %0.1f & - & \\\\ 12 | ACES & %0.1f & %0.1f & - & 41 \\\\ 13 | Intel Lab & %0.1f & %0.1f & - & 179 \\\\ 14 | MIT Killian & %0.1f & %0.1f & - & 190 \\\\ \\bottomrule 15 | \\end{tabular} 16 | """ % ( 17 | *get_our_time("usc-sal"), 18 | *get_our_time("nsh_level_a"), 19 | *get_our_time("stanford-gates"), 20 | *get_our_time("aces"), 21 | *get_our_time("intel-lab"), 22 | *get_our_time("mit-killian") 23 | ) 24 | print(table) -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-10.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 10 13 | multicloud_size: 160 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.65 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-11.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 11 13 | multicloud_size: 176 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-13.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 13 13 | multicloud_size: 182 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.65 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-30.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.2 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 30 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.08 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.08 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-4.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 4 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-45.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.25 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 45 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.075 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 1 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-5.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 5 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-6.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 6 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-60.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.25 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 60 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.55 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 6 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.8 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.05 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 1 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-7.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 7 13 | multicloud_size: 126 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-8.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 8 13 | multicloud_size: 144 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-lab/slam-9.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5533430342749532 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 9 13 | multicloud_size: 162 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.4 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 15.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 25 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 3 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-oregon/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/intel-oregon/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 7.0 12 | scan_size: 4 13 | multicloud_size: 120 14 | 15 | landmark_max_gap: 2.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 5.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 2 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 1.0 39 | 40 | match_interval: 10 41 | data_provider: oregon 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-csail/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-killian/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 1.0 3 | min_split_dist: 0.2 4 | outlier_dist: 0.2 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-killian/slam-10.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 10 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 5.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 75.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 2 # number of poses 35 | last_submap_not_match: 30 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 2.5 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 1 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-killian/slam-11.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 11 13 | multicloud_size: 176 14 | 15 | landmark_max_gap: 5.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.725 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 60.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 2 # number of poses 35 | last_submap_not_match: 30 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 2.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-killian/slam-30-2.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 30 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 5.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 30.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 2 # number of poses 35 | last_submap_not_match: 30 36 | loop_closing_threads: 10 37 | 38 | dcs_phi: 1.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-killian/slam-30.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 30 13 | multicloud_size: 180 14 | 15 | landmark_max_gap: 5.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 3.0 21 | loop_closure_min_score: 0.725 22 | angular_search_window: 1.5 # radians 23 | linear_search_window: 4.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.8 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 40.0 # meters 32 | submap_resolution: 0.075 # meters 33 | submap_trajectory_length: 4.5 # meters 34 | submap_overlap_poses: 1 # number of poses 35 | last_submap_not_match: 30 36 | loop_closing_threads: 10 37 | 38 | dcs_phi: 2.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/mit-killian/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 2.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 11 13 | multicloud_size: 176 14 | 15 | landmark_max_gap: 5.0 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.725 22 | angular_search_window: 1.25 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 40.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 2 # number of poses 35 | last_submap_not_match: 30 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 0.75 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/nsh_level_a/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/nsh_level_a/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.08 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 3.0 12 | scan_size: 5 13 | multicloud_size: 150 14 | 15 | landmark_max_gap: 3.0 16 | landmark_max_dist: 100.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 3.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 1000 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: carmen 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/olsson-3loop/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/olsson-3loop/olsson-3loop.log: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shiftlab-nanodrone/sparse-gslam/c772618f3470c73e17c728d19364e99464afb436/src/sparse_gslam/datasets/olsson-3loop/olsson-3loop.log -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/olsson-3loop/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.4 3 | std_y: 0.4 4 | std_w: 0.4 5 | std_r: 0.08 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 3.141592653589793 10 | range_min: 0.0 11 | range_max: 2.0 12 | scan_size: 4 13 | multicloud_size: 160 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.65 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 5.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 6 36 | loop_closing_threads: 6 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: drone_bag 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/olsson-demo/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.75 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/olsson-demo/olsson-demo.log: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shiftlab-nanodrone/sparse-gslam/c772618f3470c73e17c728d19364e99464afb436/src/sparse_gslam/datasets/olsson-demo/olsson-demo.log -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/olsson-demo/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.4 3 | std_y: 0.4 4 | std_w: 0.4 5 | std_r: 0.08 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 3.141592653589793 10 | range_min: 0.0 11 | range_max: 2.0 12 | scan_size: 4 13 | multicloud_size: 160 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.6 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 5.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 4 # number of poses 35 | last_submap_not_match: 6 36 | loop_closing_threads: 6 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: drone_bag 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/relation_stats.py: -------------------------------------------------------------------------------- 1 | import math 2 | from tabulate import tabulate 3 | 4 | 5 | def gen_stats(dname: str): 6 | f = open(f"{dname}/{dname}.relations").readlines() 7 | sx = 0.0 8 | sy = 0.0 9 | st = 0.0 10 | for line in f: 11 | line = line.split(" ") 12 | x = float(line[2]) 13 | y = float(line[3]) 14 | theta = float(line[7]) 15 | 16 | sx += abs(x) 17 | sy += abs(y) 18 | st += abs(theta) 19 | sx /= len(f) 20 | sy /= len(f) 21 | st /= len(f) 22 | return dname, sx, sy, math.hypot(sx, sy), math.degrees(st) 23 | 24 | print(tabulate([ 25 | gen_stats("intel-lab"), 26 | gen_stats("aces"), 27 | gen_stats("mit-killian") 28 | ], headers=["dataset", "mean x dist (m)", "mean y dist (m)", "mean dist (m)", "mean angle (degrees)"], floatfmt=".10f")) -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/rice/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.75 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/rice/rice.log: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shiftlab-nanodrone/sparse-gslam/c772618f3470c73e17c728d19364e99464afb436/src/sparse_gslam/datasets/rice/rice.log -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/rice/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.4 3 | std_y: 0.4 4 | std_w: 0.4 5 | std_r: 0.08 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 3.141592653589793 10 | range_min: 0.0 11 | range_max: 2.0 12 | scan_size: 4 13 | multicloud_size: 160 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.6 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 5.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 2 # number of poses 35 | last_submap_not_match: 6 36 | loop_closing_threads: 6 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 10 41 | data_provider: drone_bag 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/slam_config_example.yaml: -------------------------------------------------------------------------------- 1 | # radio address of crazyflie. Ignored for log runner 2 | address: radio://0/80/2M/E7E7E7E702 3 | # the standard deviation of odometry measurements, in meters. They also serve as a tool to put more or less weight on pose-pose (odometry) constraints. Larger error means less weight. 4 | std_x: 0.2 5 | std_y: 1.0 6 | std_w: 1.0 7 | # the standard deviation of range measurements, in meters. They also serve as a tool to put more or less weight on pose-landmark constraints. Larger error means less weight. 8 | std_r: 0.1 9 | # ignored 10 | std_rpcm: 0.0001 11 | 12 | # Configuration of the sparse lidar. Meaning of these parameters can be found here: http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/LaserScan.html 13 | angle_min: -1.5707963267948966 14 | angle_max: 1.5707963267948966 15 | range_min: 0.0 16 | range_max: 7.0 17 | # number of range readings per scan 18 | scan_size: 4 19 | # see Fig 5 of the paper 20 | multicloud_size: 120 21 | 22 | # maximum gap between a extracted segment and landmark in meters. If the gap between the if greater than distance, they will not be associated even if they are colinear. Usually not a very important parameter. 23 | landmark_max_gap: 2.0 24 | # disallowed pose landmark association if the robot has traveled this far after the landmark is created. This is to prevent reassociation of similar looking landmarks from far away. 25 | landmark_max_dist: 10.0 26 | # This is ε in formula (5) in the paper. Usually the default 0.5 is good. 27 | landmark_assoc_thresh: 0.5 28 | 29 | # To perform scan to map matching, we first build a multiscan from past trajectory. This parameter defines the length of the trajectory. 30 | # Longer trajectory = higher precision, lower recall, higher computation cost. 31 | last_traj_length: 5.0 32 | # loop closure match threshold. Higher threshold = higher precision, lower recall, lower computation cost. 33 | loop_closure_min_score: 0.7 34 | # Settings for the scan to map matcher. For more details, see the cartographer paper. 35 | # Basic tuning guide: enlarge the search window and increase depth for higher recall, but it will lead to more computation. 36 | # usually the default is good. 37 | angular_search_window: 1.0 # radians 38 | linear_search_window: 5.0 # meters 39 | branch_and_bound_depth: 5 40 | 41 | # settings for the occupancy grid. Usually no need to change. Higher hit probability basically puts a higher weight for each scan hit in the occupancy grid 42 | hit_probability: 0.7 43 | miss_probability: 0.4 44 | 45 | # settings for the submap builder 46 | # Do not match the current multiscan against a submap if the distance from the current pose to the submap is greater than this value. 47 | # This is mainly a performance optimization. Reduce max match distance reduces the total matchable number of submaps in the pool. 48 | max_match_distance: 5.0 # meters 49 | # Cell size of the occupancy grid. This parameter trade off between fuzziness of submaps to allow more matching candidate and matching accuracy. Higher the resolution (smaller cell size), higher matching accuracy but less match recall and more computation. For sparse sensing, usually 0.1 is good. 50 | submap_resolution: 0.1 # meters 51 | # Each submap is built from scans accumulated for a certain length of trajectory. For example, 6.0 here means that each submap is built from the scans during the past 6 meters of robot trajectory. Larger trajectory length means more information will present in the submap, hence allowing more matches, but will lead to more computation. usually the default 6.0 is good. 52 | submap_trajectory_length: 6.0 # meters 53 | submap_overlap_poses: 2 # number of poses 54 | # number of recently created submaps not to matched. Only tune this parameter when you see false matches between recently poses and recent submaps. This map happen when you have a lot of featureless hallways, which may lead to false matches between recent pose and submaps. 55 | last_submap_not_match: 3 56 | # number of threads to use to compute loop closures. 57 | loop_closing_threads: 8 58 | 59 | # phi for DCS. For more details, see the DCS paper. 60 | dcs_phi: 1.0 61 | 62 | # The interval of compute loop closures computation. 10 means we compute loop closure once every 10 scans/data frames 63 | match_interval: 10 64 | # the name of the data provider in data_provider.cpp 65 | data_provider: oregon 66 | 67 | # visualization settings 68 | visualize_rate: 2 # in Hz 69 | # resolution of the global map. Lower resolution will lead to faster visualizations. 70 | map_resolution: 0.1 71 | -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/stanford-gates/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.75 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/stanford-gates/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.1 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 5 13 | multicloud_size: 160 14 | 15 | landmark_max_gap: 0.5 16 | landmark_max_dist: 10.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 5.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 20.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 3 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 20 41 | data_provider: stanford 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/sweep.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import os 3 | 4 | def get_plots(): 5 | fig, axes = plt.subplots(1, 2, figsize=(6.75, 2.5)) 6 | plt.subplots_adjust(left=0.09, bottom=0.2, right=0.99, top=None, wspace=0.3, hspace=None) 7 | return fig, axes 8 | 9 | 10 | def read_errors(dname, xs): 11 | rot_ys = [] 12 | rot_stds = [] 13 | trans_ys = [] 14 | trans_stds = [] 15 | for i in xs: 16 | with open(os.path.join(dname, f"{dname}-{i}_rot_error.log")) as f: 17 | line = f.readlines()[1].split(", ")[:2] 18 | rot_ys.append(float(line[0])) 19 | rot_stds.append(float(line[1]) / 2) 20 | with open(os.path.join(dname, f"{dname}-{i}_trans_error.log")) as f: 21 | line = f.readlines()[1].split(", ")[:2] 22 | trans_ys.append(float(line[0])) 23 | trans_stds.append(float(line[1]) / 2) 24 | return rot_ys, rot_stds, trans_ys, trans_stds 25 | 26 | 27 | def plot_errors(dname: str): 28 | # xs = [4, 5, 6, 7, 8, 10, 11, 13, 30, 45, 60] 29 | xs = [4, 6, 9, 11, 13, 30, 45, 60] 30 | g_xs = [30, 45, 60] 31 | rot_ys, rot_stds, trans_ys, trans_stds = read_errors(dname, xs) 32 | g_rot_ys, g_rot_stds, g_trans_ys, g_trans_stds = read_errors(dname, [f"gmapping-{x}" for x in g_xs]) 33 | 34 | fig, axes = get_plots() 35 | axes[0].plot(xs, rot_ys) 36 | axes[0].plot(g_xs, g_rot_ys) 37 | axes[0].scatter(xs, rot_ys, marker="*", s=64) 38 | axes[0].scatter(g_xs, g_rot_ys, marker="*", s=64) 39 | axes[0].set_xlabel("Number of Range Measurements") 40 | axes[0].set_ylabel("Error (degrees)") 41 | axes[0].legend(["Ours", "GMapping"], loc="upper right") 42 | axes[0].set_title("Absolute Rotational Error", fontsize=11) 43 | 44 | axes[1].plot(xs, trans_ys) 45 | axes[1].plot(g_xs, g_trans_ys) 46 | axes[1].scatter(xs, trans_ys, marker="*", s=64) 47 | axes[1].scatter(g_xs, g_trans_ys, marker="*", s=64) 48 | axes[1].legend(["Ours", "GMapping"], loc="upper right") 49 | axes[1].set_xlabel("Number of Range Measurements") 50 | axes[1].set_ylabel("Error (meter)") 51 | axes[1].set_title("Absolute Translational Error", fontsize=11) 52 | plt.savefig("sweep.PNG") 53 | 54 | xs = ["4-k0", "4", "4-k5", "4-k7"] 55 | rot_ys, rot_stds, trans_ys, trans_stds = read_errors(dname, xs) 56 | 57 | fig, axes = get_plots() 58 | xs = ["no kernel", "3x3", "5x5", "7x7"] 59 | x = list(range(len(xs))) 60 | axes[0].plot(x, rot_ys) 61 | axes[0].scatter(x, rot_ys, marker="*", s=64) 62 | axes[0].set_xlabel("Kernel choices") 63 | axes[0].set_xticks(x) 64 | axes[0].set_xticklabels(xs) 65 | axes[0].set_ylabel("Error (degrees)") 66 | axes[0].set_title("Absolute Rotational Error", fontsize=11) 67 | 68 | axes[1].plot(x, trans_ys) 69 | axes[1].scatter(x, trans_ys, marker="*", s=64) 70 | axes[1].set_xlabel("Kernel choices") 71 | axes[1].set_xticks(x) 72 | axes[1].set_xticklabels(xs) 73 | axes[1].set_ylabel("Error (meter)") 74 | axes[1].set_title("Absolute Translational Error", fontsize=11) 75 | plt.savefig("kernel.PNG") 76 | 77 | xs = list(range(92, 136, 4)) 78 | xs.remove(124) 79 | rot_ys, rot_stds, trans_ys, trans_stds = read_errors(dname, [f"4-m-{x}" for x in xs]) 80 | 81 | fig, axes = get_plots() 82 | axes[0].plot(xs, rot_ys) 83 | axes[0].scatter(xs, rot_ys, marker="*", s=64) 84 | axes[0].set_xlabel("Multiscan size") 85 | axes[0].set_ylabel("Error (degrees)") 86 | axes[0].set_title("Absolute Rotational Error", fontsize=11) 87 | 88 | axes[1].plot(xs, trans_ys) 89 | axes[1].scatter(xs, trans_ys, marker="*", s=64) 90 | axes[1].set_xlabel("Multiscan size") 91 | axes[1].set_ylabel("Error (meter)") 92 | axes[1].set_title("Absolute Translational Error", fontsize=11) 93 | plt.savefig("multiscan.PNG") 94 | plt.show() 95 | 96 | # plot_errors("aces") 97 | plot_errors("intel-lab") -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/usc-sal/line_extractor.yaml: -------------------------------------------------------------------------------- 1 | max_line_gap: 0.25 2 | min_line_length: 0.5 3 | min_split_dist: 0.1 4 | outlier_dist: 0.1 5 | min_line_points: 10 6 | rmse_thresh: 0.1 7 | cluster_threshold: 100.0 -------------------------------------------------------------------------------- /src/sparse_gslam/datasets/usc-sal/slam.yaml: -------------------------------------------------------------------------------- 1 | address: radio://0/80/2M/E7E7E7E702 2 | std_x: 0.2 3 | std_y: 1.0 4 | std_w: 1.0 5 | std_r: 0.15 6 | std_rpcm: 0.0001 7 | 8 | angle_min: -1.5707963267948966 9 | angle_max: 1.5707963267948966 10 | range_min: 0.0 11 | range_max: 5.0 12 | scan_size: 5 13 | multicloud_size: 150 14 | 15 | landmark_max_gap: 2.0 16 | landmark_max_dist: 100.0 17 | landmark_assoc_thresh: 0.5 18 | 19 | # settings for the scan to map matcher 20 | last_traj_length: 5.0 21 | loop_closure_min_score: 0.7 22 | angular_search_window: 1.0 # radians 23 | linear_search_window: 4.0 # meters 24 | branch_and_bound_depth: 5 25 | 26 | # settings for the occupancy grid 27 | hit_probability: 0.7 28 | miss_probability: 0.4 29 | 30 | # settings for the submap builder 31 | max_match_distance: 10.0 # meters 32 | submap_resolution: 0.1 # meters 33 | submap_trajectory_length: 6.0 # meters 34 | submap_overlap_poses: 0 # number of poses 35 | last_submap_not_match: 1000 36 | loop_closing_threads: 8 37 | 38 | dcs_phi: 10.0 39 | 40 | match_interval: 20 41 | data_provider: usc 42 | 43 | # visualization settings 44 | visualize_rate: 2 45 | map_resolution: 0.1 46 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/cartographer_bindings/ceres_scan_matcher_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ 18 | #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "Eigen/Core" 24 | #include "cartographer/common/lua_parameter_dictionary.h" 25 | #include "cartographer/mapping/2d/grid_2d.h" 26 | #include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.pb.h" 27 | #include "cartographer/sensor/point_cloud.h" 28 | #include "ceres/ceres.h" 29 | 30 | namespace cartographer { 31 | namespace mapping { 32 | namespace scan_matching { 33 | 34 | proto::CeresScanMatcherOptions2D CreateCeresScanMatcherOptions2D( 35 | common::LuaParameterDictionary* parameter_dictionary); 36 | 37 | // Align scans with an existing map using Ceres. 38 | class CeresScanMatcher2D { 39 | public: 40 | explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options); 41 | virtual ~CeresScanMatcher2D(); 42 | 43 | CeresScanMatcher2D(const CeresScanMatcher2D&) = delete; 44 | CeresScanMatcher2D& operator=(const CeresScanMatcher2D&) = delete; 45 | 46 | // Aligns 'point_cloud' within the 'grid' given an 47 | // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver 48 | // 'summary'. 49 | void Match(const Eigen::Vector2d& target_translation, 50 | const transform::Rigid2d& initial_pose_estimate, 51 | const sensor::PointCloud& point_cloud, const Grid2D& grid, 52 | transform::Rigid2d* pose_estimate, 53 | ceres::Solver::Summary* summary) const; 54 | 55 | private: 56 | const proto::CeresScanMatcherOptions2D options_; 57 | ceres::Solver::Options ceres_solver_options_; 58 | }; 59 | 60 | } // namespace scan_matching 61 | } // namespace mapping 62 | } // namespace cartographer 63 | 64 | #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_2D_H_ 65 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/cartographer_bindings/correlative_scan_matcher_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ 18 | #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ 19 | 20 | #include 21 | 22 | #include "Eigen/Core" 23 | #include "cartographer/common/lua_parameter_dictionary.h" 24 | #include "cartographer/mapping/2d/map_limits.h" 25 | #include "cartographer/mapping/2d/xy_index.h" 26 | 27 | 28 | namespace cartographer { 29 | namespace mapping { 30 | namespace scan_matching { 31 | 32 | typedef std::vector DiscreteScan2D; 33 | 34 | // Describes the search space. 35 | struct SearchParameters { 36 | // Linear search window in pixel offsets; bounds are inclusive. 37 | struct LinearBounds { 38 | int min_x; 39 | int max_x; 40 | int min_y; 41 | int max_y; 42 | }; 43 | 44 | SearchParameters(double linear_search_window, double angular_search_window, 45 | const std::vector& point_cloud, double resolution); 46 | 47 | // For testing. 48 | SearchParameters(int num_linear_perturbations, int num_angular_perturbations, 49 | double angular_perturbation_step_size, double resolution); 50 | 51 | // Tightens the search window as much as possible. 52 | void ShrinkToFit(const std::vector& scans, 53 | const CellLimits& cell_limits); 54 | 55 | int num_angular_perturbations; 56 | double angular_perturbation_step_size; 57 | double resolution; 58 | int num_scans; 59 | std::vector linear_bounds; // Per rotated scans. 60 | }; 61 | 62 | // Generates a collection of rotated scans. 63 | std::vector GenerateRotatedScans( 64 | const std::vector& point_cloud, 65 | const SearchParameters& search_parameters, 66 | const transform::Rigid2d& initial_pose_estimate, 67 | const MapLimits& map_limits); 68 | 69 | // A possible solution. 70 | struct Candidate2D { 71 | Candidate2D(const int init_scan_index, const int init_x_index_offset, 72 | const int init_y_index_offset, 73 | const SearchParameters& search_parameters) 74 | : scan_index(init_scan_index), 75 | x_index_offset(init_x_index_offset), 76 | y_index_offset(init_y_index_offset) {} 77 | 78 | static inline Eigen::Vector3d ToPose(int x_index_offset, int y_index_offset, int scan_index, const SearchParameters& search_parameters) { 79 | return {-y_index_offset * search_parameters.resolution, 80 | -x_index_offset * search_parameters.resolution, 81 | (scan_index - search_parameters.num_angular_perturbations) * search_parameters.angular_perturbation_step_size}; 82 | } 83 | 84 | inline Eigen::Vector3d ToPose(const SearchParameters& search_parameters) const { 85 | return Candidate2D::ToPose(x_index_offset, y_index_offset, scan_index, search_parameters); 86 | } 87 | 88 | // Index into the rotated scans vector. 89 | int scan_index = 0; 90 | 91 | // Linear offset from the initial pose. 92 | int x_index_offset = 0; 93 | int y_index_offset = 0; 94 | 95 | // Score, higher is better. 96 | float score = 0.f; 97 | 98 | bool operator<(const Candidate2D& other) const { return score < other.score; } 99 | bool operator>(const Candidate2D& other) const { return score > other.score; } 100 | }; 101 | 102 | } // namespace scan_matching 103 | } // namespace mapping 104 | } // namespace cartographer 105 | 106 | #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_2D_H_ -------------------------------------------------------------------------------- /src/sparse_gslam/include/cartographer_bindings/fast_correlative_scan_matcher_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | // This is an implementation of the algorithm described in "Real-Time 18 | // Correlative Scan Matching" by Olson. 19 | // 20 | // It is similar to the RealTimeCorrelativeScanMatcher but has a different 21 | // trade-off: Scan matching is faster because more effort is put into the 22 | // precomputation done for a given map. However, this map is immutable after 23 | // construction. 24 | 25 | #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ 26 | #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include "Eigen/Core" 32 | #include "cartographer/common/port.h" 33 | #include "cartographer/mapping/2d/grid_2d.h" 34 | #include "cartographer_bindings/correlative_scan_matcher_2d.h" 35 | #include "cartographer_bindings/range_data_2d.h" 36 | #include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h" 37 | 38 | namespace cartographer { 39 | namespace mapping { 40 | namespace scan_matching { 41 | 42 | proto::FastCorrelativeScanMatcherOptions2D 43 | CreateFastCorrelativeScanMatcherOptions2D( 44 | common::LuaParameterDictionary* parameter_dictionary); 45 | 46 | // A precomputed grid that contains in each cell (x0, y0) the maximum 47 | // probability in the width x width area defined by x0 <= x < x0 + width and 48 | // y0 <= y < y0. 49 | class PrecomputationGrid2D { 50 | public: 51 | PrecomputationGrid2D(const Grid2D& grid, const CellLimits& limits, int width, 52 | std::vector* reusable_intermediate_grid); 53 | 54 | // Returns a value between 0 and 255 to represent probabilities between 55 | // min_score and max_score. 56 | int GetValue(const Eigen::Array2i& xy_index) const { 57 | const Eigen::Array2i local_xy_index = xy_index - offset_; 58 | // The static_cast is for performance to check with 2 comparisons 59 | // xy_index.x() < offset_.x() || xy_index.y() < offset_.y() || 60 | // local_xy_index.x() >= wide_limits_.num_x_cells || 61 | // local_xy_index.y() >= wide_limits_.num_y_cells 62 | // instead of using 4 comparisons. 63 | if (static_cast(local_xy_index.x()) >= 64 | static_cast(wide_limits_.num_x_cells) || 65 | static_cast(local_xy_index.y()) >= 66 | static_cast(wide_limits_.num_y_cells)) { 67 | return 0; 68 | } 69 | const int stride = wide_limits_.num_x_cells; 70 | return cells_[local_xy_index.x() + local_xy_index.y() * stride]; 71 | } 72 | 73 | // Maps values from [0, 255] to [min_score, max_score]. 74 | float ToScore(float value) const { 75 | return min_score_ + value * ((max_score_ - min_score_) / 255.f); 76 | } 77 | 78 | private: 79 | uint8 ComputeCellValue(float probability) const; 80 | 81 | // Offset of the precomputation grid in relation to the 'grid' 82 | // including the additional 'width' - 1 cells. 83 | const Eigen::Array2i offset_; 84 | 85 | // Size of the precomputation grid. 86 | const CellLimits wide_limits_; 87 | 88 | const float min_score_; 89 | const float max_score_; 90 | 91 | // Probabilites mapped to 0 to 255. 92 | std::vector cells_; 93 | }; 94 | 95 | class PrecomputationGridStack2D { 96 | public: 97 | PrecomputationGridStack2D( 98 | const Grid2D& grid, 99 | const proto::FastCorrelativeScanMatcherOptions2D& options); 100 | 101 | const PrecomputationGrid2D& Get(int index) { 102 | return precomputation_grids_[index]; 103 | } 104 | 105 | int max_depth() const { return precomputation_grids_.size() - 1; } 106 | 107 | private: 108 | std::vector precomputation_grids_; 109 | }; 110 | 111 | // An implementation of "Real-Time Correlative Scan Matching" by Olson. 112 | class FastCorrelativeScanMatcher2D { 113 | public: 114 | FastCorrelativeScanMatcher2D( 115 | const Grid2D& grid, 116 | const proto::FastCorrelativeScanMatcherOptions2D& options); 117 | ~FastCorrelativeScanMatcher2D(); 118 | 119 | FastCorrelativeScanMatcher2D(const FastCorrelativeScanMatcher2D&) = delete; 120 | FastCorrelativeScanMatcher2D& operator=(const FastCorrelativeScanMatcher2D&) = 121 | delete; 122 | 123 | // Aligns 'point_cloud' within the 'grid' given an 124 | // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) 125 | // is possible, true is returned, and 'score' and 'pose_estimate' are updated 126 | // with the result. 127 | bool Match(const transform::Rigid2d& initial_pose_estimate, 128 | const std::vector& point_cloud, float min_score, 129 | float* score, transform::Rigid2d* pose_estimate, Eigen::Matrix3d& covariance) const; 130 | 131 | // Aligns 'point_cloud' within the full 'grid', i.e., not 132 | // restricted to the configured search window. If a score above 'min_score' 133 | // (excluding equality) is possible, true is returned, and 'score' and 134 | // 'pose_estimate' are updated with the result. 135 | // bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score, 136 | // float* score, transform::Rigid2d* pose_estimate) const; 137 | 138 | private: 139 | // The actual implementation of the scan matcher, called by Match() and 140 | // MatchFullSubmap() with appropriate 'initial_pose_estimate' and 141 | // 'search_parameters'. 142 | bool MatchWithSearchParameters( 143 | SearchParameters search_parameters, 144 | const transform::Rigid2d& initial_pose_estimate, 145 | const std::vector& point_cloud, float min_score, float* score, 146 | transform::Rigid2d* pose_estimate, Eigen::Matrix3d& covariance) const; 147 | std::vector ComputeLowestResolutionCandidates( 148 | const std::vector& discrete_scans, 149 | const SearchParameters& search_parameters) const; 150 | std::vector GenerateLowestResolutionCandidates( 151 | const SearchParameters& search_parameters) const; 152 | void ScoreCandidates(const PrecomputationGrid2D& precomputation_grid, 153 | const std::vector& discrete_scans, 154 | const SearchParameters& search_parameters, 155 | std::vector* const candidates) const; 156 | Candidate2D BranchAndBound(const std::vector& discrete_scans, 157 | const SearchParameters& search_parameters, 158 | const std::vector& candidates, 159 | int candidate_depth, float min_score) const; 160 | 161 | const proto::FastCorrelativeScanMatcherOptions2D options_; 162 | MapLimits limits_; 163 | std::unique_ptr precomputation_grid_stack_; 164 | }; 165 | 166 | } // namespace scan_matching 167 | } // namespace mapping 168 | } // namespace cartographer 169 | 170 | #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_2D_H_ 171 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/cartographer_bindings/range_data_2d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | namespace sensor_msgs { 7 | ROS_DECLARE_MESSAGE(LaserScan) 8 | } 9 | namespace cartographer { 10 | namespace sensor { 11 | // multi-range data 2d 12 | class RangeData2D { 13 | public: 14 | struct MetaData { 15 | int return_end, end; 16 | Eigen::Vector2f origin; 17 | inline void shift_idx(int idx) { 18 | return_end += idx; 19 | end += idx; 20 | } 21 | }; 22 | std::vector points_; 23 | std::vector meta_data_; 24 | 25 | void insert_data(const sensor_msgs::LaserScan& scan, const std::vector& table, const Eigen::Vector2f& trans, const Eigen::Rotation2Df& rot); 26 | void transform_to(const Eigen::Vector2f& trans, const Eigen::Rotation2Df& rot, RangeData2D& out) const; 27 | void get_returns_pointcloud(std::vector& out) const; 28 | void clear(); 29 | }; 30 | 31 | } // namespace sensor 32 | } // namespace cartographer -------------------------------------------------------------------------------- /src/sparse_gslam/include/cartographer_bindings/range_data_inserter_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include "cartographer/common/lua_parameter_dictionary.h" 23 | #include "cartographer/common/port.h" 24 | #include "cartographer/mapping/2d/probability_grid.h" 25 | #include "cartographer/mapping/2d/xy_index.h" 26 | #include "cartographer/sensor/point_cloud.h" 27 | #include "cartographer_bindings/range_data_2d.h" 28 | 29 | namespace cartographer { 30 | namespace mapping { 31 | 32 | class MultirangeDataInserter { 33 | public: 34 | explicit MultirangeDataInserter(double hit_prob, double miss_prob); 35 | 36 | MultirangeDataInserter( 37 | const MultirangeDataInserter&) = delete; 38 | MultirangeDataInserter& operator=( 39 | const MultirangeDataInserter&) = delete; 40 | 41 | // Inserts 'range_data' into 'probability_grid'. 42 | void Insert(const sensor::RangeData2D& range_data, GridInterface* grid) const; 43 | 44 | private: 45 | const std::vector hit_table_; 46 | const std::vector miss_table_; 47 | }; 48 | 49 | } // namespace mapping 50 | } // namespace cartographer 51 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/cartographer_bindings/ray_to_pixel_mask.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ 18 | #define CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/transform/transform.h" 23 | 24 | namespace cartographer { 25 | namespace mapping { 26 | 27 | // Compute all pixels that contain some part of the line segment connecting 28 | // 'scaled_begin' and 'scaled_end'. 'scaled_begin' and 'scaled_end' are scaled 29 | // by 'subpixel_scale'. 'scaled_begin' and 'scaled_end' are expected to be 30 | // greater than zero. Return values are in pixels and not scaled. 31 | std::vector RayToPixelMask(const Eigen::Array2i& scaled_begin, 32 | const Eigen::Array2i& scaled_end, 33 | int subpixel_scale); 34 | 35 | } // namespace mapping 36 | } // namespace cartographer 37 | 38 | #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_RAY_TO_PIXEL_MASK_H_ 39 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/data_provider.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | class DataProvider { 7 | public: 8 | virtual bool get_data(double& time, g2o::SE2& pose, std::vector& full_range) = 0; 9 | virtual ~DataProvider() {} 10 | }; 11 | 12 | std::unique_ptr create_data_provider(const std::string& name, const std::string& fpath); -------------------------------------------------------------------------------- /src/sparse_gslam/include/delta_vector.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include 5 | struct Delta { 6 | double dt; 7 | g2o::SE2 dpose; 8 | }; 9 | 10 | typedef std::vector> DeltaVector; -------------------------------------------------------------------------------- /src/sparse_gslam/include/drone.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "loop_closer/submap_loop_closer.h" 7 | #include "odom_error_propagator.h" 8 | #include "visualizer.h" 9 | #include "graphs.h" 10 | 11 | namespace nav_msgs { 12 | ROS_DECLARE_MESSAGE(Odometry) 13 | } 14 | namespace sensor_msgs { 15 | ROS_DECLARE_MESSAGE(LaserScan) 16 | } 17 | namespace ls_extractor { 18 | class LineSegment; 19 | using SegmentVector = std::vector; 20 | } 21 | 22 | constexpr int LANDMARK_BASE_ID = 10000000; 23 | class Drone { 24 | public: 25 | tf2_ros::TransformBroadcaster br; 26 | tf::Transform odom_map_tf; 27 | geometry_msgs::TransformStamped odom_bl_tf; 28 | 29 | OdomErrorPropagator odom_prop; 30 | geometry_msgs::PoseStamped cor_pose_msg; 31 | ros::Publisher cor_pose_pub; 32 | 33 | std::vector table; 34 | Delta prev_pose; 35 | 36 | LandmarkGraph lm_graph; 37 | PoseGraph pose_graph; 38 | SubmapLoopCloser loop_closer; 39 | 40 | bool need_reinit = true; 41 | int vertex_id = 0; 42 | int landmark_id = LANDMARK_BASE_ID; 43 | const double landmark_assoc_thresh; 44 | const double landmark_max_gap; 45 | const double landmark_max_dist; 46 | double chi2_before = 0.0; 47 | double traveled_dist = 0.0; 48 | Drone(ros::NodeHandle& nh, const XmlRpc::XmlRpcValue& config); 49 | 50 | void msgCallback(const ls_extractor::SegmentVector& segments_in, const nav_msgs::Odometry& odom, const sensor_msgs::LaserScan& pc); 51 | 52 | void addLandmarkObservations(g2o::VertexRhoTheta* landmark, const ls_extractor::LineSegment& bl_line, g2o::VertexSE2* new_vertex); 53 | 54 | g2o::VertexRhoTheta* mergeLine(const Eigen::Vector2f& start, const Eigen::Vector2f& end); 55 | void submap_matching(); 56 | }; 57 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/g2o_bindings/edge_se2_rhotheta.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "g2o/core/base_binary_edge.h" 3 | #include "g2o/types/slam2d/vertex_se2.h" 4 | #include "g2o_bindings/vertex_rhotheta.h" 5 | 6 | namespace g2o { 7 | 8 | class EdgeSE2RhoTheta : public BaseBinaryEdge<2, Eigen::Vector2d, VertexSE2, VertexRhoTheta> { 9 | public: 10 | Eigen::Vector2f start, end; 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 12 | EdgeSE2RhoTheta() = default; 13 | void computeError() override; 14 | 15 | virtual bool read(std::istream& is) override; 16 | virtual bool write(std::ostream& os) const override; 17 | }; 18 | 19 | } // namespace g2o -------------------------------------------------------------------------------- /src/sparse_gslam/include/g2o_bindings/vertex_rhotheta.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include "g2o/core/base_vertex.h" 6 | #include "g2o/core/hyper_graph_action.h" 7 | #include "g2o/stuff/misc.h" 8 | 9 | namespace g2o { 10 | class EdgeSE2RhoTheta; 11 | class VertexRhoTheta : public BaseVertex<2, Eigen::Vector2d> { 12 | public: 13 | Eigen::Vector2f start, end; 14 | double dist; 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 16 | VertexRhoTheta() = default; 17 | void updateEndpoints(); 18 | virtual void setToOriginImpl() override; 19 | virtual void oplusImpl(const double* update) override; 20 | virtual bool read(std::istream& is) override; 21 | virtual bool write(std::ostream& os) const override; 22 | }; 23 | } // namespace g2o -------------------------------------------------------------------------------- /src/sparse_gslam/include/graphs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "g2o/core/sparse_optimizer.h" 3 | #include "g2o/types/slam2d/edge_se2.h" 4 | #include "g2o/types/slam2d/vertex_se2.h" 5 | #include "g2o_bindings/edge_se2_rhotheta.h" 6 | #include "g2o_bindings/vertex_rhotheta.h" 7 | #include "pose_with_observation.h" 8 | #include 9 | #include 10 | #include 11 | 12 | void setup_lm_opt(g2o::SparseOptimizer& opt); 13 | void setup_pose_opt(g2o::SparseOptimizer& opt); 14 | 15 | struct PoseChain { 16 | g2o::VertexSE2 pose; 17 | g2o::EdgeSE2 edge; 18 | }; 19 | struct LandmarkGraph { 20 | int last_landmark_edge = 0; 21 | boost::shared_mutex mu; 22 | std::deque> landmarks; 23 | std::deque> pose_landmarks; 24 | std::deque> poses; 25 | g2o::HyperGraph::VertexSet new_vset; 26 | g2o::HyperGraph::EdgeSet new_eset; 27 | g2o::SparseOptimizer opt; 28 | LandmarkGraph(); 29 | ~LandmarkGraph(); 30 | }; 31 | struct PoseGraph { 32 | boost::shared_mutex mu; 33 | std::deque> poses; 34 | std::deque> all_closures; 35 | std::unordered_set closures, false_closures; 36 | std::vector false_candidates; 37 | g2o::SparseOptimizer opt; 38 | PoseGraph(); 39 | ~PoseGraph(); 40 | }; -------------------------------------------------------------------------------- /src/sparse_gslam/include/loop_closer/submap_loop_closer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cartographer_bindings/fast_correlative_scan_matcher_2d.h" 3 | #include "cartographer_bindings/ceres_scan_matcher_2d.h" 4 | #include "cartographer_bindings/range_data_inserter_2d.h" 5 | #include "ctpl_stl.h" 6 | #include "submap.h" 7 | 8 | class Drone; 9 | class SubmapLoopCloser { 10 | public: 11 | Drone& drone; 12 | 13 | double loop_closure_min_score; 14 | const double max_match_distance; 15 | const double last_traj_length; 16 | const double submap_resolution; 17 | const double submap_trajectory_length; 18 | const int last_submap_not_match; 19 | const int submap_overlap_poses; 20 | int last_pose_idx = 0; 21 | int last_opt_pose_index = 1; 22 | 23 | struct SubmapInfo { 24 | float x, y, theta; 25 | Submap* submap; 26 | }; 27 | std::vector submaps_info; 28 | std::deque submaps; 29 | std::vector multi_scan; 30 | 31 | mapping::scan_matching::proto::FastCorrelativeScanMatcherOptions2D matcher_options; 32 | mapping::MultirangeDataInserter range_data_inserter; 33 | mapping::scan_matching::CeresScanMatcher2D ceres_matcher; 34 | 35 | SubmapLoopCloser(Drone& visualizer, const XmlRpc::XmlRpcValue& config); 36 | void precompute(); 37 | bool match(); 38 | 39 | ctpl::thread_pool pool; 40 | }; -------------------------------------------------------------------------------- /src/sparse_gslam/include/multicloud2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "ls_extractor/defs.h" 9 | #include "odom_error_propagator.h" 10 | namespace tf { 11 | class Transform; 12 | } 13 | class MulticloudConverter { 14 | public: 15 | const int scan_size, mc_window_size; 16 | const float var_r; 17 | 18 | ros::Publisher scan_pub, mc_pub, mc_cov_pub; 19 | sensor_msgs::PointCloud2 mc_msg; 20 | pcl::PointCloud multicloud, temp_cloud, temp_bl_cloud; 21 | OdomErrorPropagator odom_prop; 22 | Eigen::Matrix Jl = Eigen::Matrix::Zero(); 23 | ls_extractor::PointVector pcv; 24 | 25 | sensor_msgs::LaserScan scan; 26 | struct CosSin { 27 | float cos_v, sin_v; 28 | }; 29 | std::vector table; 30 | MulticloudConverter(ros::NodeHandle& nh, const XmlRpc::XmlRpcValue& config); 31 | 32 | bool update(const ros::Time& stamp, const DeltaVector& deltas, const tf::Transform& current_scan_tf); 33 | }; -------------------------------------------------------------------------------- /src/sparse_gslam/include/odom_error_propagator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "delta_vector.h" 5 | 6 | template 7 | inline void updateJacobian(Eigen::Matrix& J, T dx, T dy, T theta) { 8 | T ct = cos(theta), st = sin(theta); 9 | J(0, 2) = dy * ct - dx * st; 10 | J(0, 3) = ct; 11 | J(0, 4) = st; 12 | J(1, 2) = -dx * ct - dy * st; 13 | J(1, 3) = -st; 14 | J(1, 4) = ct; 15 | } 16 | 17 | template 18 | class OdomErrorPropagator { 19 | public: 20 | g2o::SE2 pose; 21 | const T var_x, var_y, var_w; 22 | Eigen::Matrix cov; // x, y, theta 23 | OdomErrorPropagator(T std_x, T std_y, T std_w, const g2o::SE2& pose = g2o::SE2()) : pose(pose), 24 | var_x(std_x * std_x), 25 | var_y(std_y * std_y), 26 | var_w(std_w * std_w), 27 | cov(Eigen::Matrix::Identity() * 1e-6) { 28 | J << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 29 | 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 30 | 0.0, 0.0, 1.0, 0.0, 0.0, 1.0; 31 | } 32 | 33 | void reset() { 34 | cov = Eigen::Matrix::Identity() * 1e-6; 35 | pose = g2o::SE2(); 36 | } 37 | 38 | void step(const Delta& delta) { 39 | updateJacobian(J, (T)delta.dpose[0], (T)delta.dpose[1], (T)pose[2]); 40 | covu.diagonal() << std::abs(delta.dpose[0] * delta.dpose[0]) * var_x, 41 | std::abs(delta.dpose[1] * delta.dpose[0]) * var_y, 42 | std::abs(delta.dpose[2] * delta.dpose[0]) * var_w; 43 | cov.template block<3, 3>(0, 0) = J.template block<3, 3>(0, 0) * cov * J.template block<3, 3>(0, 0).transpose() + 44 | J.template block<3, 3>(0, 3) * covu * J.template block<3, 3>(0, 3).transpose(); 45 | pose *= delta.dpose; 46 | } 47 | 48 | private: 49 | Eigen::DiagonalMatrix covu; // vx, vy, vtheta 50 | Eigen::Matrix J; 51 | }; 52 | -------------------------------------------------------------------------------- /src/sparse_gslam/include/pose_with_observation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "cartographer_bindings/range_data_2d.h" 3 | #include "g2o/types/slam2d/edge_se2.h" 4 | #include "g2o/types/slam2d/vertex_se2.h" 5 | #include "delta_vector.h" 6 | using namespace cartographer; 7 | class PoseWithObservation; 8 | using PoseWithObservations = std::deque>; 9 | class PoseWithObservation { 10 | public: 11 | g2o::VertexSE2 pose; 12 | g2o::EdgeSE2 edge; 13 | sensor::RangeData2D data; 14 | DeltaVector odom; 15 | PoseWithObservation(const sensor_msgs::LaserScan& scan, const std::vector& table, const g2o::SE2& raw_odom); 16 | void addPoints(const sensor_msgs::LaserScan& scan, const g2o::SE2& transform, const std::vector& table); 17 | static void construct_multicloud_with_correct( 18 | const PoseWithObservations& poses, 19 | int start, int mid, int end, sensor::RangeData2D& result); 20 | static void construct_multicloud_with_correct_returns_only( 21 | const PoseWithObservations& poses, 22 | int start, int mid, int end, std::vector& result); 23 | }; -------------------------------------------------------------------------------- /src/sparse_gslam/include/submap.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "cartographer/mapping/2d/probability_grid.h" 9 | #include "cartographer_bindings/fast_correlative_scan_matcher_2d.h" 10 | #include "cartographer_bindings/range_data_2d.h" 11 | 12 | namespace g2o { 13 | class VertexSE2; 14 | class VertexRhoTheta; 15 | class SE2; 16 | }; 17 | using namespace cartographer; 18 | struct Submap { 19 | static mapping::ValueConversionTables conversion_tables; 20 | static Eigen::AlignedBox2f probability_grid_to_occupancy_grid( 21 | const g2o::SE2& trans, const mapping::ProbabilityGrid& probability_grid, nav_msgs::OccupancyGrid& grid, float padding); 22 | static void probability_grid_to_occupancy_grid_fixed_size( 23 | const mapping::ProbabilityGrid& probability_grid, nav_msgs::OccupancyGrid& grid); 24 | 25 | g2o::VertexSE2* pose; 26 | mapping::ProbabilityGrid probability_grid; 27 | mapping::ProbabilityGrid high_res_grid; 28 | // delayed initialization for matcher. it will only be constructed after the submap is finalized 29 | boost::optional matcher; 30 | Submap(const Submap& a) = delete; 31 | Submap(Submap&& a) = default; 32 | Submap(float res); 33 | void fix_submap(g2o::VertexSE2* pose, const mapping::scan_matching::proto::FastCorrelativeScanMatcherOptions2D& options, float padding); 34 | 35 | #if defined(VISUALIZE_SUBMAP) || defined(SHOW_MATCH) 36 | // for visualization purposes only 37 | Eigen::AlignedBox2f box; 38 | nav_msgs::OccupancyGrid grid; 39 | void publish_without_compute(ros::Publisher& pub, const g2o::SE2& pose); 40 | #endif 41 | }; -------------------------------------------------------------------------------- /src/sparse_gslam/include/visualizer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | namespace ros { 5 | class NodeHandle; 6 | class Time; 7 | } 8 | namespace XmlRpc { 9 | class XmlRpcValue; 10 | } 11 | class Drone; 12 | 13 | class Visualizer { 14 | public: 15 | Visualizer(Drone& drone, ros::NodeHandle& nh, const XmlRpc::XmlRpcValue& config) noexcept; 16 | ~Visualizer(); 17 | void visualize_landmarks(const ros::Time& stamp); 18 | void start(int rate); 19 | void stop(); 20 | 21 | private: 22 | struct Impl; 23 | std::unique_ptr impl; 24 | }; -------------------------------------------------------------------------------- /src/sparse_gslam/include/wallfollowing_multirange_onboard.h: -------------------------------------------------------------------------------- 1 | /* 2 | * wallfollowing_multirange_onboard.h 3 | * 4 | * Created on: Aug 7, 2018 5 | * Author: knmcguire 6 | */ 7 | #pragma once 8 | #include 9 | #include 10 | int wall_follower(float *vel_x, float *vel_y, float *vel_w, float front_range, float side_range, float current_heading, 11 | int direction_turn); 12 | 13 | void adjustDistanceWall(float distance_wall_new); 14 | 15 | void wall_follower_init(float new_ref_distance_from_wall, float max_speed_ref, int init_state); -------------------------------------------------------------------------------- /src/sparse_gslam/launch/controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 11 | -------------------------------------------------------------------------------- /src/sparse_gslam/launch/log_runner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | $(arg realtime) 13 | $(arg rate) 14 | $(find sparse_gslam)/datasets/$(arg dataset)/ 15 | $(arg dataset) 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/sparse_gslam/launch/physical.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 31 | -------------------------------------------------------------------------------- /src/sparse_gslam/launch/takeoff.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/sparse_gslam/launch/virtual.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 24 | 25 | 33 | 34 | 42 | -------------------------------------------------------------------------------- /src/sparse_gslam/msg/RawData.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32[] raw -------------------------------------------------------------------------------- /src/sparse_gslam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sparse_gslam 4 | 0.0.0 5 | The sparse_gslam package 6 | 7 | 8 | 9 | 10 | hanzhi713 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | message_generation 63 | ls_extractor 64 | message_runtime 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /src/sparse_gslam/rviz_config/gmapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Axes1 10 | Splitter Ratio: 0.5 11 | Tree Height: 719 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Class: rviz/Axes 58 | Enabled: true 59 | Length: 1 60 | Name: Axes 61 | Radius: 0.10000000149011612 62 | Reference Frame: base_link 63 | Value: true 64 | - Alpha: 0.699999988079071 65 | Class: rviz/Map 66 | Color Scheme: map 67 | Draw Behind: false 68 | Enabled: true 69 | Name: Map 70 | Topic: /map 71 | Unreliable: false 72 | Use Timestamp: false 73 | Value: true 74 | Enabled: true 75 | Global Options: 76 | Background Color: 48; 48; 48 77 | Default Light: true 78 | Fixed Frame: map 79 | Frame Rate: 30 80 | Name: root 81 | Tools: 82 | - Class: rviz/Interact 83 | Hide Inactive Objects: true 84 | - Class: rviz/MoveCamera 85 | - Class: rviz/Select 86 | - Class: rviz/FocusCamera 87 | - Class: rviz/Measure 88 | - Class: rviz/SetInitialPose 89 | Theta std deviation: 0.2617993950843811 90 | Topic: /initialpose 91 | X std deviation: 0.5 92 | Y std deviation: 0.5 93 | - Class: rviz/SetGoal 94 | Topic: /move_base_simple/goal 95 | - Class: rviz/PublishPoint 96 | Single click: true 97 | Topic: /clicked_point 98 | Value: true 99 | Views: 100 | Current: 101 | Class: rviz/Orbit 102 | Distance: 82.46022033691406 103 | Enable Stereo Rendering: 104 | Stereo Eye Separation: 0.05999999865889549 105 | Stereo Focal Distance: 1 106 | Swap Stereo Eyes: false 107 | Value: false 108 | Field of View: 0.7853981852531433 109 | Focal Point: 110 | X: 1.2574245929718018 111 | Y: 1.725799798965454 112 | Z: -2.109459161758423 113 | Focal Shape Fixed Size: true 114 | Focal Shape Size: 0.05000000074505806 115 | Invert Z Axis: false 116 | Name: Current View 117 | Near Clip Distance: 0.009999999776482582 118 | Pitch: 1.544796347618103 119 | Target Frame: 120 | Yaw: 1.3303974866867065 121 | Saved: ~ 122 | Window Geometry: 123 | Displays: 124 | collapsed: false 125 | Height: 1016 126 | Hide Left Dock: false 127 | Hide Right Dock: false 128 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 129 | Selection: 130 | collapsed: false 131 | Time: 132 | collapsed: false 133 | Tool Properties: 134 | collapsed: false 135 | Views: 136 | collapsed: false 137 | Width: 1848 138 | X: 72 139 | Y: 27 140 | -------------------------------------------------------------------------------- /src/sparse_gslam/rviz_config/test_bag.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 85 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 703 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: LaserScan 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 10 59 | Min Value: -10 60 | Value: true 61 | Axis: Z 62 | Channel Name: intensity 63 | Class: rviz/LaserScan 64 | Color: 255; 255; 255 65 | Color Transformer: Intensity 66 | Decay Time: 0 67 | Enabled: true 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 4096 71 | Min Color: 0; 0; 0 72 | Min Intensity: 0 73 | Name: LaserScan 74 | Position Transformer: XYZ 75 | Queue Size: 10 76 | Selectable: true 77 | Size (Pixels): 3 78 | Size (m): 0.009999999776482582 79 | Style: Flat Squares 80 | Topic: /scan 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | Enabled: true 86 | Global Options: 87 | Background Color: 48; 48; 48 88 | Default Light: true 89 | Fixed Frame: odom 90 | Frame Rate: 30 91 | Name: root 92 | Tools: 93 | - Class: rviz/Interact 94 | Hide Inactive Objects: true 95 | - Class: rviz/MoveCamera 96 | - Class: rviz/Select 97 | - Class: rviz/FocusCamera 98 | - Class: rviz/Measure 99 | - Class: rviz/SetInitialPose 100 | Theta std deviation: 0.2617993950843811 101 | Topic: /initialpose 102 | X std deviation: 0.5 103 | Y std deviation: 0.5 104 | - Class: rviz/SetGoal 105 | Topic: /move_base_simple/goal 106 | - Class: rviz/PublishPoint 107 | Single click: true 108 | Topic: /clicked_point 109 | Value: true 110 | Views: 111 | Current: 112 | Class: rviz/Orbit 113 | Distance: 24.5716552734375 114 | Enable Stereo Rendering: 115 | Stereo Eye Separation: 0.05999999865889549 116 | Stereo Focal Distance: 1 117 | Swap Stereo Eyes: false 118 | Value: false 119 | Focal Point: 120 | X: 0 121 | Y: 0 122 | Z: 0 123 | Focal Shape Fixed Size: true 124 | Focal Shape Size: 0.05000000074505806 125 | Invert Z Axis: false 126 | Name: Current View 127 | Near Clip Distance: 0.009999999776482582 128 | Pitch: 0.9853978157043457 129 | Target Frame: 130 | Value: Orbit (rviz) 131 | Yaw: 0.6403981447219849 132 | Saved: ~ 133 | Window Geometry: 134 | Displays: 135 | collapsed: false 136 | Height: 1020 137 | Hide Left Dock: false 138 | Hide Right Dock: false 139 | QMainWindow State: 000000ff00000000fd00000004000000000000015a00000355fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000355000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000355fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000355000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002c500fffffffb0000000800540069006d00650100000000000004500000000000000000000004c60000035500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 140 | Selection: 141 | collapsed: false 142 | Time: 143 | collapsed: false 144 | Tool Properties: 145 | collapsed: false 146 | Views: 147 | collapsed: false 148 | Width: 1853 149 | X: 67 150 | Y: 32 151 | -------------------------------------------------------------------------------- /src/sparse_gslam/scripts/takeoff.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from std_msgs.msg import String 3 | from std_srvs.srv import SetBool 4 | import rospy 5 | 6 | if __name__ == "__main__": 7 | rospy.init_node("takeoff_commander") 8 | rospy.wait_for_service('takeoff') 9 | takeoff = rospy.ServiceProxy('takeoff', SetBool) 10 | 11 | takeoff(True) 12 | 13 | rospy.spin() 14 | 15 | takeoff(False) -------------------------------------------------------------------------------- /src/sparse_gslam/src/cartographer_bindings/correlative_scan_matcher_2d.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer_bindings/correlative_scan_matcher_2d.h" 18 | 19 | #include 20 | 21 | #include "cartographer/common/math.h" 22 | 23 | namespace cartographer { 24 | namespace mapping { 25 | namespace scan_matching { 26 | 27 | SearchParameters::SearchParameters(const double linear_search_window, 28 | const double angular_search_window, 29 | const std::vector& point_cloud, 30 | const double resolution) 31 | : resolution(resolution) { 32 | // We set this value to something on the order of resolution to make sure that 33 | // the std::acos() below is defined. 34 | float max_scan_range = 3.f * resolution; 35 | for (const auto& point : point_cloud) { 36 | max_scan_range = std::max(point.norm(), max_scan_range); 37 | } 38 | const double kSafetyMargin = 1. - 1e-3; 39 | angular_perturbation_step_size = 40 | kSafetyMargin * std::acos(1. - common::Pow2(resolution) / 41 | (2. * common::Pow2(max_scan_range))); 42 | num_angular_perturbations = 43 | std::ceil(angular_search_window / angular_perturbation_step_size); 44 | num_scans = 2 * num_angular_perturbations + 1; 45 | 46 | const int num_linear_perturbations = 47 | std::ceil(linear_search_window / resolution); 48 | linear_bounds.reserve(num_scans); 49 | for (int i = 0; i != num_scans; ++i) { 50 | linear_bounds.push_back( 51 | LinearBounds{-num_linear_perturbations, num_linear_perturbations, 52 | -num_linear_perturbations, num_linear_perturbations}); 53 | } 54 | } 55 | 56 | SearchParameters::SearchParameters(const int num_linear_perturbations, 57 | const int num_angular_perturbations, 58 | const double angular_perturbation_step_size, 59 | const double resolution) 60 | : num_angular_perturbations(num_angular_perturbations), 61 | angular_perturbation_step_size(angular_perturbation_step_size), 62 | resolution(resolution), 63 | num_scans(2 * num_angular_perturbations + 1) { 64 | linear_bounds.reserve(num_scans); 65 | for (int i = 0; i != num_scans; ++i) { 66 | linear_bounds.push_back( 67 | LinearBounds{-num_linear_perturbations, num_linear_perturbations, 68 | -num_linear_perturbations, num_linear_perturbations}); 69 | } 70 | } 71 | 72 | void SearchParameters::ShrinkToFit(const std::vector& scans, 73 | const CellLimits& cell_limits) { 74 | CHECK_EQ(scans.size(), num_scans); 75 | CHECK_EQ(linear_bounds.size(), num_scans); 76 | for (int i = 0; i != num_scans; ++i) { 77 | Eigen::Array2i min_bound = Eigen::Array2i::Zero(); 78 | Eigen::Array2i max_bound = Eigen::Array2i::Zero(); 79 | for (const Eigen::Array2i& xy_index : scans[i]) { 80 | min_bound = min_bound.min(-xy_index); 81 | max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1, 82 | cell_limits.num_y_cells - 1) - 83 | xy_index); 84 | } 85 | linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x()); 86 | linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x()); 87 | linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y()); 88 | linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y()); 89 | } 90 | } 91 | 92 | std::vector GenerateRotatedScans( 93 | const std::vector& point_cloud, 94 | const SearchParameters& search_parameters, 95 | const transform::Rigid2d& initial_pose_estimate, 96 | const MapLimits& map_limits) { 97 | std::vector discrete_scans; 98 | discrete_scans.reserve(search_parameters.num_scans); 99 | 100 | double delta_theta = -search_parameters.num_angular_perturbations * 101 | search_parameters.angular_perturbation_step_size + 102 | initial_pose_estimate.rotation().angle(); 103 | Eigen::Vector2f initial_translation = initial_pose_estimate.translation().cast(); 104 | for (int scan_index = 0; scan_index < search_parameters.num_scans; 105 | ++scan_index, 106 | delta_theta += search_parameters.angular_perturbation_step_size) { 107 | discrete_scans.emplace_back(); 108 | discrete_scans.back().reserve(point_cloud.size()); 109 | Eigen::Matrix2f rot = Eigen::Rotation2Df(delta_theta).toRotationMatrix(); 110 | for (const auto& point : point_cloud) { 111 | const Eigen::Vector2f translated_point = rot * point + initial_translation; 112 | discrete_scans.back().push_back( 113 | map_limits.GetCellIndex(translated_point)); 114 | } 115 | } 116 | return discrete_scans; 117 | } 118 | 119 | } // namespace scan_matching 120 | } // namespace mapping 121 | } // namespace cartographer 122 | -------------------------------------------------------------------------------- /src/sparse_gslam/src/cartographer_bindings/range_data_2d.cc: -------------------------------------------------------------------------------- 1 | #include "cartographer_bindings/range_data_2d.h" 2 | 3 | #include 4 | 5 | namespace cartographer { 6 | namespace sensor { 7 | 8 | void RangeData2D::insert_data(const sensor_msgs::LaserScan& scan, const std::vector& table, const Eigen::Vector2f& trans, const Eigen::Rotation2Df& rot) { 9 | meta_data_.emplace_back(); 10 | auto& meta = meta_data_.back(); 11 | meta.origin = trans; 12 | int scan_size = scan.ranges.size(); 13 | for (int i = 0; i < scan_size; i++) { 14 | float dist = scan.ranges[i]; 15 | if (dist < scan.range_max) { 16 | points_.push_back(trans + rot * (table[i] * dist)); 17 | } 18 | } 19 | meta.return_end = points_.size(); 20 | for (int i = 0; i < scan_size; i++) { 21 | float dist = scan.ranges[i]; 22 | if (!std::isnan(dist) && dist >= scan.range_max) { 23 | points_.push_back(trans + rot * (table[i] * scan.range_max)); 24 | } 25 | } 26 | meta.end = points_.size(); 27 | } 28 | 29 | void RangeData2D::transform_to(const Eigen::Vector2f& trans, const Eigen::Rotation2Df& rot, RangeData2D& out) const { 30 | int p_start_idx = out.points_.size(); 31 | int m_start_idx = out.meta_data_.size(); 32 | out.points_.resize(p_start_idx + points_.size()); 33 | out.meta_data_.resize(m_start_idx + meta_data_.size()); 34 | 35 | std::transform(points_.begin(), points_.end(), out.points_.begin() + p_start_idx, [&rot, &trans](const auto& pt) { return (rot * pt + trans).eval(); }); 36 | std::transform(meta_data_.begin(), meta_data_.end(), out.meta_data_.begin() + m_start_idx, [&trans, p_start_idx](MetaData meta) { 37 | meta.shift_idx(p_start_idx); 38 | meta.origin += trans; 39 | return meta; 40 | }); 41 | } 42 | 43 | void RangeData2D::get_returns_pointcloud(std::vector& out) const { 44 | out.reserve(points_.size()); 45 | int i = 0; 46 | for (const auto& meta : meta_data_) { 47 | for (; i < meta.return_end; i++) { 48 | out.push_back(points_[i]); 49 | } 50 | i = meta.end; 51 | } 52 | } 53 | 54 | void RangeData2D::clear() { 55 | points_.clear(); 56 | meta_data_.clear(); 57 | } 58 | } // namespace sensor 59 | } // namespace cartographer -------------------------------------------------------------------------------- /src/sparse_gslam/src/cartographer_bindings/range_data_inserter_2d.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer_bindings/range_data_inserter_2d.h" 18 | 19 | #include 20 | 21 | #include "Eigen/Core" 22 | #include "Eigen/Geometry" 23 | #include "cartographer/mapping/2d/xy_index.h" 24 | #include "cartographer/mapping/probability_values.h" 25 | #include "cartographer_bindings/ray_to_pixel_mask.h" 26 | #include "glog/logging.h" 27 | 28 | namespace cartographer { 29 | namespace mapping { 30 | namespace { 31 | 32 | // Factor for subpixel accuracy of start and end point for ray casts. 33 | constexpr int kSubpixelScale = 1000; 34 | 35 | void GrowAsNeeded(const sensor::RangeData2D& range_data, 36 | ProbabilityGrid* const probability_grid) { 37 | Eigen::AlignedBox2f bounding_box; 38 | // Padding around bounding box to avoid numerical issues at cell boundaries. 39 | constexpr float kPadding = 1e-6f; 40 | for (const auto& point : range_data.points_) { 41 | bounding_box.extend(point); 42 | } 43 | probability_grid->GrowLimits(bounding_box.min() - 44 | kPadding * Eigen::Vector2f::Ones()); 45 | probability_grid->GrowLimits(bounding_box.max() + 46 | kPadding * Eigen::Vector2f::Ones()); 47 | } 48 | 49 | } // namespace 50 | 51 | MultirangeDataInserter::MultirangeDataInserter(double hit_prob, double miss_prob) 52 | : hit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(hit_prob))), 53 | miss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(miss_prob))) {} 54 | 55 | void MultirangeDataInserter::Insert( 56 | const sensor::RangeData2D& range_data, GridInterface* const grid) const { 57 | ProbabilityGrid* const probability_grid = static_cast(grid); 58 | CHECK(probability_grid != nullptr); 59 | 60 | GrowAsNeeded(range_data, probability_grid); 61 | 62 | const MapLimits& limits = probability_grid->limits(); 63 | const double superscaled_resolution = limits.resolution() / kSubpixelScale; 64 | const MapLimits superscaled_limits( 65 | superscaled_resolution, limits.max(), 66 | CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, 67 | limits.cell_limits().num_y_cells * kSubpixelScale)); 68 | 69 | int i = 0; 70 | for (const auto& meta_data : range_data.meta_data_) { 71 | const Eigen::Array2i begin = superscaled_limits.GetCellIndex(meta_data.origin); 72 | 73 | for (; i < meta_data.return_end; i++) { 74 | auto end = superscaled_limits.GetCellIndex(range_data.points_[i]); 75 | probability_grid->ApplyLookupTable(end / kSubpixelScale, hit_table_); 76 | 77 | // trace the miss space 78 | std::vector ray = RayToPixelMask(begin, end, kSubpixelScale); 79 | for (const Eigen::Array2i& cell_index : ray) { 80 | probability_grid->ApplyLookupTable(cell_index, miss_table_); 81 | } 82 | } 83 | for (; i < meta_data.end; i++) { 84 | auto end = superscaled_limits.GetCellIndex(range_data.points_[i]); 85 | std::vector ray = RayToPixelMask(begin, end, kSubpixelScale); 86 | for (const Eigen::Array2i& cell_index : ray) { 87 | probability_grid->ApplyLookupTable(cell_index, miss_table_); 88 | } 89 | } 90 | probability_grid->FinishUpdate(); 91 | } 92 | // By not finishing the update after hits are inserted, we give hits priority 93 | // (i.e. no hits will be ignored because of a miss in the same cell). 94 | } 95 | 96 | } // namespace mapping 97 | } // namespace cartographer 98 | -------------------------------------------------------------------------------- /src/sparse_gslam/src/converter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include "sparse_gslam/RawData.h" 15 | #include "multicloud2.h" 16 | using sparse_gslam::RawData; 17 | 18 | typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; 19 | typedef message_filters::Synchronizer ATS; 20 | constexpr float NaN = std::numeric_limits().quiet_NaN(); 21 | class RawDataConverter { 22 | ros::CallbackQueue cb_queue; 23 | ros::AsyncSpinner spinner; 24 | 25 | std::string ns; 26 | DeltaVector deltas; 27 | g2o::SE2 last_pose; 28 | ros::Time last_stamp; 29 | 30 | nav_msgs::Odometry odometry; 31 | geometry_msgs::TransformStamped odom_pc_tf; 32 | 33 | MulticloudConverter pc_converter; 34 | ros::Publisher odom_pub, rssi_pub, battery_pub; 35 | tf2_ros::TransformBroadcaster br; 36 | message_filters::Subscriber sub1, sub2; 37 | ATS ats; 38 | 39 | public: 40 | RawDataConverter(const std::string& ns, ros::NodeHandle& nh, const XmlRpc::XmlRpcValue& config) 41 | : spinner(1, &cb_queue), 42 | ns(ns), 43 | last_pose(NaN, NaN, 0.0), 44 | pc_converter(nh, ns, config), 45 | 46 | odom_pub(nh.advertise(ns + "/odom", 1)), 47 | rssi_pub(nh.advertise(ns + "/rssi", 1)), 48 | battery_pub(nh.advertise(ns + "/battery_voltage", 1)), 49 | 50 | sub1(nh, ns + "/state_xyzv", 1, ros::TransportHints(), &cb_queue), 51 | sub2(nh, ns + "/state_ranger_qxyzw", 1, ros::TransportHints(), &cb_queue), 52 | ats(MySyncPolicy(5), sub1, sub2) { 53 | odom_pc_tf.header.frame_id = odometry.header.frame_id = ns + "/odom"; 54 | odom_pc_tf.child_frame_id = odometry.child_frame_id = ns + "/base_link"; 55 | 56 | ats.registerCallback(&RawDataConverter::process_raw_data, this); 57 | spinner.start(); 58 | std::cout << ns << std::endl; 59 | } 60 | 61 | ~RawDataConverter() { 62 | spinner.stop(); 63 | } 64 | 65 | void process_raw_data(const RawData& state1, const RawData& state2) { 66 | auto stamp = ros::Time::now(); // note: may need to be changed to use the timestamp on crazyflies 67 | odometry.header.stamp = stamp; 68 | 69 | auto& position = odometry.pose.pose.position; 70 | position.x = state1.raw[0]; 71 | position.y = state1.raw[1]; 72 | position.z = state1.raw[2]; 73 | 74 | auto& linear = odometry.twist.twist.linear; 75 | linear.x = state1.raw[3]; 76 | linear.y = state1.raw[4]; 77 | linear.z = state1.raw[5]; 78 | auto& angular = odometry.twist.twist.angular; 79 | angular.x = state1.raw[6]; 80 | angular.y = state1.raw[7]; 81 | angular.z = state1.raw[8]; 82 | 83 | auto& rotation = odometry.pose.pose.orientation; 84 | rotation.x = state2.raw[5]; 85 | rotation.y = state2.raw[6]; 86 | rotation.z = state2.raw[7]; 87 | rotation.w = state2.raw[8]; 88 | 89 | odom_pub.publish(odometry); 90 | 91 | std_msgs::Float32 temp; 92 | temp.data = state1.raw[9]; 93 | battery_pub.publish(temp); 94 | 95 | // maybe add median filter? 96 | temp.data = state1.raw[10]; 97 | rssi_pub.publish(temp); 98 | 99 | tf::Transform current_scan_tf; 100 | tf::poseMsgToTF(odometry.pose.pose, current_scan_tf); 101 | 102 | double _r, _p, _y; 103 | current_scan_tf.getBasis().getRPY(_r, _p, _y); 104 | 105 | if (std::isnan(last_pose[0])) { // first pose ever? 106 | last_pose = g2o::SE2(position.x, position.y, _y); 107 | odom_pc_tf.header.stamp = stamp; 108 | odom_pc_tf.transform.translation.x = position.x; 109 | odom_pc_tf.transform.translation.y = position.y; 110 | odom_pc_tf.transform.translation.z = position.z; 111 | odom_pc_tf.transform.rotation = rotation; 112 | 113 | br.sendTransform(odom_pc_tf); 114 | } else { 115 | g2o::SE2 cur_pose(position.x, position.y, _y); 116 | deltas.push_back({stamp.toSec() - last_stamp.toSec(), last_pose.inverse() * cur_pose}); 117 | last_pose = cur_pose; 118 | } 119 | last_stamp = stamp; 120 | pc_converter.update(stamp, deltas, current_scan_tf, state2.raw.data()); 121 | } 122 | }; 123 | 124 | int main(int argc, char** argv) { 125 | ros::init(argc, argv, "multiscan_converter"); 126 | ros::NodeHandle nh; 127 | 128 | XmlRpc::XmlRpcValue value; 129 | nh.getParam("swarm/crazyflies", value); 130 | 131 | std::vector dv; 132 | for (const auto& p : value) 133 | dv.push_back(new RawDataConverter(p.first, nh, p.second)); 134 | 135 | ros::waitForShutdown(); 136 | 137 | for (auto ptr : dv) 138 | delete ptr; 139 | } -------------------------------------------------------------------------------- /src/sparse_gslam/src/g2o_bindings/edge_se2_rhotheta.cpp: -------------------------------------------------------------------------------- 1 | #include "g2o_bindings/edge_se2_rhotheta.h" 2 | 3 | #include "g2o/core/factory.h" 4 | #include "g2o/stuff/macros.h" 5 | #include "ls_extractor/utils.h" 6 | 7 | namespace g2o { 8 | 9 | void EdgeSE2RhoTheta::computeError() { 10 | const auto* pose = static_cast(_vertices[0]); 11 | const auto* line = static_cast(_vertices[1]); 12 | 13 | auto poseInv = pose->estimate().inverse(); 14 | _error.noalias() = _measurement - ls_extractor::transform_line(line->estimate(), poseInv.translation(), poseInv.rotation().angle()); 15 | _error[1] = normalize_theta(_error[1]); 16 | } 17 | 18 | bool EdgeSE2RhoTheta::read(std::istream& is) { 19 | return true; 20 | } 21 | bool EdgeSE2RhoTheta::write(std::ostream& os) const { 22 | return true; 23 | } 24 | G2O_REGISTER_TYPE(EDGE_SE2_RHOTHETA, EdgeSE2RhoTheta); 25 | } // namespace g2o -------------------------------------------------------------------------------- /src/sparse_gslam/src/g2o_bindings/vertex_rhotheta.cpp: -------------------------------------------------------------------------------- 1 | #include "g2o_bindings/vertex_rhotheta.h" 2 | #include "g2o_bindings/edge_se2_rhotheta.h" 3 | #include "g2o/types/slam2d/vertex_se2.h" 4 | #include "ls_extractor/utils.h" 5 | #include "g2o/core/factory.h" 6 | #include "g2o/stuff/macros.h" 7 | namespace g2o { 8 | 9 | void VertexRhoTheta::updateEndpoints() { 10 | if (_edges.size() == 0) 11 | return; 12 | Eigen::Vector2d start, dir; 13 | ls_extractor::calc_start_dir(_estimate, start, dir); 14 | Eigen::Vector2d t{1e10, -1e10}; 15 | // ls_extractor::calc_endpoints(start, dir, this->start.cast(), this->end.cast(), t); 16 | for (auto* e : _edges) { 17 | auto* edge = static_cast(e); 18 | auto* pose = static_cast(edge->vertex(0)); 19 | Eigen::Vector2d tout; 20 | ls_extractor::calc_endpoints(start, dir, pose->estimate() * edge->start.cast(), pose->estimate() * edge->end.cast(), tout); 21 | t[0] = std::min(t[0], tout[0]); 22 | t[1] = std::max(t[1], tout[1]); 23 | } 24 | this->start = (start + t[0] * dir).cast(); 25 | this->end = (start + t[1] * dir).cast(); 26 | } 27 | 28 | void VertexRhoTheta::setToOriginImpl() { _estimate.setZero(); } 29 | 30 | void VertexRhoTheta::oplusImpl(const double* update) { 31 | _estimate[0] += update[0]; 32 | _estimate[1] += update[1]; 33 | normalize_theta(_estimate[1]); 34 | } 35 | 36 | bool VertexRhoTheta::read(std::istream& is) { 37 | return true; 38 | } 39 | 40 | bool VertexRhoTheta::write(std::ostream& os) const { 41 | return os.good(); 42 | } 43 | G2O_REGISTER_TYPE(VERTEX_RHOTHETA, VertexRhoTheta); 44 | } // namespace g2o -------------------------------------------------------------------------------- /src/sparse_gslam/src/graphs.cpp: -------------------------------------------------------------------------------- 1 | #include "graphs.h" 2 | 3 | #include "g2o/core/block_solver.h" 4 | #include "g2o/core/optimization_algorithm_factory.h" 5 | #include "g2o/core/optimization_algorithm_gauss_newton.h" 6 | #include "g2o/core/optimization_algorithm_levenberg.h" 7 | #include "g2o/solvers/eigen/linear_solver_eigen.h" 8 | 9 | void setup_lm_opt(g2o::SparseOptimizer& opt) { 10 | using SlamBlockSolver = g2o::BlockSolver>; 11 | using SlamLinearSolver = g2o::LinearSolverEigen; 12 | opt.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique(g2o::make_unique()))); 13 | opt.setVerbose(false); 14 | opt.setComputeBatchStatistics(false); 15 | } 16 | 17 | void setup_pose_opt(g2o::SparseOptimizer& opt) { 18 | using SlamBlockSolver = g2o::BlockSolver>; 19 | using SlamLinearSolver = g2o::LinearSolverEigen; 20 | opt.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique(g2o::make_unique()))); 21 | opt.setVerbose(false); 22 | opt.setComputeBatchStatistics(false); 23 | } 24 | 25 | LandmarkGraph::LandmarkGraph() { 26 | setup_lm_opt(opt); 27 | } 28 | LandmarkGraph::~LandmarkGraph() { 29 | delete opt.algorithm(); 30 | } 31 | 32 | PoseGraph::PoseGraph() { 33 | setup_pose_opt(opt); 34 | } 35 | PoseGraph::~PoseGraph() { 36 | delete opt.algorithm(); 37 | } -------------------------------------------------------------------------------- /src/sparse_gslam/src/multicloud2.cpp: -------------------------------------------------------------------------------- 1 | #include "multicloud2.h" 2 | 3 | #include 4 | #include 5 | 6 | inline float thresh(float v, float thresh) { 7 | return v >= thresh ? std::numeric_limits().infinity() : v; 8 | } 9 | 10 | MulticloudConverter::MulticloudConverter(ros::NodeHandle& nh, 11 | const XmlRpc::XmlRpcValue& config) : scan_size(config["scan_size"]), 12 | mc_window_size(config["multicloud_size"]), 13 | var_r(std::pow(config["std_r"], 2.0)), 14 | scan_pub(nh.advertise("scan", 2)), 15 | mc_pub(nh.advertise("multicloud", 2)), 16 | odom_prop((double)config["std_x"], (double)config["std_y"], (double)config["std_w"]), 17 | table(scan_size) { 18 | temp_bl_cloud.header.frame_id = multicloud.header.frame_id 19 | = scan.header.frame_id = "base_link"; 20 | scan.angle_min = (double)config["angle_min"]; 21 | scan.angle_max = (double)config["angle_max"]; 22 | scan.range_min = (double)config["range_min"]; 23 | scan.range_max = (double)config["range_max"]; 24 | scan.ranges.resize(scan_size); 25 | scan.angle_increment = (scan.angle_max - scan.angle_min) / (scan_size - 1); 26 | 27 | temp_cloud.resize(scan_size); 28 | temp_bl_cloud.resize(mc_window_size); 29 | multicloud.reserve(mc_window_size + scan_size * 2); 30 | 31 | pcv.reserve(mc_window_size); 32 | Jl(0, 0) = Jl(1, 1) = 1.0f; 33 | } 34 | 35 | bool MulticloudConverter::update(const ros::Time& stamp, const DeltaVector& deltas, const tf::Transform& current_scan_tf) { 36 | scan.header.stamp = stamp; 37 | scan_pub.publish(scan); 38 | 39 | for (int i = 0; i < scan_size; i++) { 40 | temp_cloud[i].x = table[i].cos_v * thresh(scan.ranges[i], scan.range_max); 41 | temp_cloud[i].y = table[i].sin_v * thresh(scan.ranges[i], scan.range_max); 42 | temp_cloud[i].z = 0.0; 43 | } 44 | // transform to odom frame 45 | pcl_ros::transformPointCloud(temp_cloud, temp_cloud, current_scan_tf); // rotation then translation 46 | multicloud.insert(multicloud.end(), temp_cloud.begin(), temp_cloud.end()); 47 | 48 | if (multicloud.size() >= mc_window_size) { 49 | memmove(multicloud.points.data(), multicloud.points.data() + multicloud.size() - mc_window_size, mc_window_size * sizeof(pcl::PointXYZ)); 50 | multicloud.points.resize(mc_window_size); 51 | pcl_ros::transformPointCloud(multicloud, temp_bl_cloud, current_scan_tf.inverse()); 52 | 53 | pcv.clear(); 54 | int delta_offset = mc_window_size / scan_size - 1; 55 | for (int i = 0; i < mc_window_size / scan_size; i++) { 56 | odom_prop.reset(); 57 | for (int j = i; j < delta_offset; j++) { // FIXME: boundary 58 | int k = deltas.size() + j - delta_offset; 59 | odom_prop.step(deltas[k]); 60 | } 61 | float ct = cos((float)odom_prop.pose[2]), st = sin((float)odom_prop.pose[2]); 62 | Eigen::Matrix3f Juk; // jacobian of the inverse 63 | Juk << -ct, st, odom_prop.pose[1] * ct + odom_prop.pose[0] * st, 64 | -st, -ct, odom_prop.pose[1] * st - odom_prop.pose[0] * ct, 65 | 0.0, 0.0, -1.0; 66 | odom_prop.cov = Juk * odom_prop.cov * Juk.transpose(); 67 | odom_prop.pose = odom_prop.pose.inverse(); 68 | updateJacobian(Jl, (float)odom_prop.pose[0], (float)odom_prop.pose[1], (float)odom_prop.pose[2]); 69 | for (int j = 0; j < scan_size; j++) { 70 | int base = scan_size * i; 71 | if (std::isfinite(temp_bl_cloud[base + j].x) && std::isfinite(temp_bl_cloud[base + j].y)) { 72 | pcv.emplace_back(); 73 | auto& pt = pcv.back(); 74 | pt.point[0] = temp_bl_cloud[base + j].x; 75 | pt.point[1] = temp_bl_cloud[base + j].y; 76 | pt.rhotheta[0] = pt.point.norm(); 77 | pt.rhotheta[1] = std::atan2(pt.point[1], pt.point[0]); 78 | Eigen::Matrix2f covp; // covariance of the point 79 | const float c = table[j].cos_v * table[j].sin_v; 80 | covp << table[j].cos_v * table[j].cos_v, c, c, table[j].sin_v * table[j].sin_v; 81 | covp *= var_r; 82 | pt.cov.noalias() = Jl.block<2, 3>(0, 0) * odom_prop.cov * Jl.block<2, 3>(0, 0).transpose() + 83 | Jl.block<2, 2>(0, 3) * covp * Jl.block<2, 2>(0, 3).transpose(); 84 | } 85 | } 86 | } 87 | // make pcl happy 88 | temp_bl_cloud.width = mc_window_size; 89 | temp_bl_cloud.height = 1; 90 | pcl::toROSMsg(temp_bl_cloud, mc_msg); 91 | mc_msg.header.stamp = stamp; 92 | mc_pub.publish(mc_msg); 93 | return true; 94 | } 95 | return false; 96 | } -------------------------------------------------------------------------------- /src/sparse_gslam/src/pose_with_observation.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_with_observation.h" 2 | #include 3 | using namespace cartographer; 4 | 5 | PoseWithObservation::PoseWithObservation(const sensor_msgs::LaserScan& scan, const std::vector& table, const g2o::SE2& raw_odom) { 6 | data.insert_data(scan, table, Eigen::Vector2f::Zero(), Eigen::Rotation2Df::Identity()); 7 | odom.push_back({scan.header.stamp.toSec(), raw_odom}); 8 | } 9 | void PoseWithObservation::addPoints(const sensor_msgs::LaserScan& scan, const g2o::SE2& transform, const std::vector& table) { 10 | data.insert_data(scan, table, transform.translation().cast(), transform.rotation().cast()); 11 | odom.push_back({scan.header.stamp.toSec(), transform}); 12 | } 13 | 14 | void PoseWithObservation::construct_multicloud_with_correct(const PoseWithObservations& poses, int start, int mid, int end, sensor::RangeData2D& result) { 15 | auto pose_inv = poses[mid].pose.estimate().inverse(); 16 | result.clear(); 17 | for (auto it = poses.begin() + start, eit = poses.begin() + end; it != eit; it++) { 18 | auto trans = pose_inv * it->pose.estimate(); 19 | it->data.transform_to(trans.translation().cast(), trans.rotation().cast(), result); 20 | } 21 | } 22 | 23 | void PoseWithObservation::construct_multicloud_with_correct_returns_only(const PoseWithObservations& poses, int start, int mid, int end, std::vector& result) { 24 | auto pose_inv = poses[mid].pose.estimate().inverse(); 25 | result.clear(); 26 | for (auto it = poses.begin() + start, eit = poses.begin() + end; it != eit; it++) { 27 | auto transf = pose_inv * it->pose.estimate(); 28 | Eigen::Vector2f trans = transf.translation().cast(); 29 | Eigen::Matrix2f rot = transf.rotation().cast().toRotationMatrix(); 30 | int i = 0; 31 | for (const auto& meta : it->data.meta_data_) { 32 | for (; i < meta.return_end; i++) { 33 | result.push_back(rot * it->data.points_[i] + trans); 34 | } 35 | i = meta.end; 36 | } 37 | } 38 | } 39 | -------------------------------------------------------------------------------- /src/sparse_gslam/src/submap.cpp: -------------------------------------------------------------------------------- 1 | #include "submap.h" 2 | 3 | #include 4 | 5 | #include "g2o/types/slam2d/vertex_se2.h" 6 | #include "g2o_bindings/vertex_rhotheta.h" 7 | 8 | mapping::ValueConversionTables Submap::conversion_tables; 9 | 10 | Submap::Submap(float res): probability_grid(mapping::MapLimits(res, Eigen::Vector2d(4., 4.), mapping::CellLimits(160, 160)), &conversion_tables), 11 | high_res_grid(mapping::MapLimits(0.05f, Eigen::Vector2d(4., 4.), mapping::CellLimits(160, 160)), &conversion_tables) { 12 | #if defined(VISUALIZE_SUBMAP) || defined(SHOW_MATCH) 13 | grid.header.frame_id = "map"; 14 | grid.info.resolution = res; 15 | #endif 16 | } 17 | 18 | void Submap::fix_submap(g2o::VertexSE2* pose, const mapping::scan_matching::proto::FastCorrelativeScanMatcherOptions2D& options, float padding) { 19 | this->pose = pose; 20 | matcher.emplace(static_cast(probability_grid), options); 21 | #if defined(VISUALIZE_SUBMAP) || defined(SHOW_MATCH) 22 | box = probability_grid_to_occupancy_grid(pose->estimate(), probability_grid, grid, padding); 23 | #endif 24 | } 25 | 26 | struct Cell { 27 | Eigen::Vector2f disp; 28 | float prob; 29 | }; 30 | 31 | Eigen::AlignedBox2f Submap::probability_grid_to_occupancy_grid(const g2o::SE2& trans, const mapping::ProbabilityGrid& probability_grid, nav_msgs::OccupancyGrid& grid, float padding) { 32 | Eigen::AlignedBox2f box; 33 | std::vector cell_array; 34 | for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(probability_grid.limits().cell_limits())) { 35 | if (probability_grid.IsKnown(xy_index)) { 36 | Eigen::Vector2f disp = probability_grid.limits().GetCellCenter(xy_index); 37 | box.extend(disp); 38 | cell_array.push_back({disp, probability_grid.GetProbability(xy_index)}); 39 | } 40 | } 41 | float scale = 1.0 / grid.info.resolution; 42 | box.min() -= Eigen::Vector2f::Constant(padding); 43 | box.max() += Eigen::Vector2f::Constant(padding); 44 | Eigen::Vector2d origin = trans * box.min().cast(); 45 | Eigen::Vector2i sz = ((box.max() - box.min()) * scale).array().floor().cast(); 46 | tf::Matrix3x3 orit; 47 | orit.setRPY(0.0, 0.0, trans[2]); 48 | tf::Quaternion q; 49 | orit.getRotation(q); 50 | 51 | grid.info.width = sz[0]; 52 | grid.info.height = sz[1]; 53 | grid.info.origin.position.x = origin[0]; 54 | grid.info.origin.position.y = origin[1]; 55 | tf::quaternionTFToMsg(q, grid.info.origin.orientation); 56 | 57 | grid.data.resize(grid.info.width * grid.info.height); 58 | memset(grid.data.data(), 127, grid.data.size()); 59 | auto* ptr = reinterpret_cast(grid.data.data()); 60 | for (auto& cell : cell_array) { 61 | Eigen::Vector2i pt = ((cell.disp - box.min()) * scale).array().round().cast(); 62 | ptr[pt[1] * grid.info.width + pt[0]] = 255.0f - cell.prob * 255.0f; 63 | } 64 | return box; 65 | } 66 | 67 | void Submap::probability_grid_to_occupancy_grid_fixed_size(const mapping::ProbabilityGrid& probability_grid, nav_msgs::OccupancyGrid& grid) { 68 | Eigen::Vector2f box_min(grid.info.origin.position.x, grid.info.origin.position.y); 69 | 70 | grid.data.resize(grid.info.width * grid.info.height); 71 | memset(grid.data.data(), 127, grid.data.size()); 72 | auto* ptr = reinterpret_cast(grid.data.data()); 73 | const float scale = 1.0 / grid.info.resolution; 74 | for (const Eigen::Array2i& xy_index : mapping::XYIndexRangeIterator(probability_grid.limits().cell_limits())) { 75 | if (probability_grid.IsKnown(xy_index)) { 76 | Eigen::Vector2i pt = ((probability_grid.limits().GetCellCenter(xy_index) - box_min) * scale).array().cast(); 77 | ptr[pt[1] * grid.info.width + pt[0]] = 255.0f - probability_grid.GetProbability(xy_index) * 255.0f; 78 | } 79 | } 80 | } 81 | 82 | #if defined(VISUALIZE_SUBMAP) || defined(SHOW_MATCH) 83 | void Submap::publish_without_compute(ros::Publisher& pub, const g2o::SE2& pose) { 84 | if (!pub.getNumSubscribers()) 85 | return; 86 | Eigen::Vector2d origin = pose.translation() + Eigen::Rotation2Dd(pose[2]) * box.min().cast(); 87 | tf::Matrix3x3 orit; 88 | orit.setRPY(0.0, 0.0, pose[2]); 89 | tf::Quaternion q; 90 | orit.getRotation(q); 91 | 92 | grid.info.origin.position.x = origin[0]; 93 | grid.info.origin.position.y = origin[1]; 94 | tf::quaternionTFToMsg(q, grid.info.origin.orientation); 95 | pub.publish(grid); 96 | } 97 | #endif -------------------------------------------------------------------------------- /src/sparse_gslam/src/submap_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "cartographer_bindings/range_data_inserter_2d.h" 6 | #include "submap.h" 7 | #include "ros/ros.h" 8 | #include "g2o/types/slam2d/vertex_se2.h" 9 | #include "ls_extractor/matplotlibcpp.h" 10 | 11 | //!pwd/pwd/18.csv, pwd/pwd/match44_18.csv, true, score 0.420748, new score 0.49039 12 | // pwd/pwd/11.csv, pwd/pwd/match11_11.csv, false, score 0.412863, new score 0.708178 13 | //!pwd/pwd/94.csv, pwd/pwd/match36_94.csv, true, score 0.428638, new score 0.573387 14 | // pwd/pwd/44.csv, pwd/pwd/match33_44.csv, false, score 0.463311, new score 0.721469 15 | // pwd/pwd/15.csv, pwd/pwd/match13_15.csv, false, score 0.400475, new score 0.660784 16 | // pwd/pwd/5.csv, pwd/pwd/match20_5.csv, false, score 0.400475, new score 0.63683 17 | 18 | std::string fn = "pwd/pwd/18.csv"; 19 | std::string fn2 = "pwd/pwd/match44_18.csv"; 20 | bool reverse = true; 21 | 22 | namespace plt = matplotlibcpp; 23 | void construct_submap(){ 24 | std::fstream reader; 25 | reader.open(fn, std::ios::in); 26 | std::string line, word; 27 | sensor::RangeData2D temp_range; 28 | 29 | std::getline(reader, line); 30 | std::stringstream s(line); 31 | std::getline(s, word, ','); 32 | int mid = std::stoi(word); 33 | 34 | std::vector pose_est; 35 | std::getline(s, word, ','); 36 | pose_est.push_back(std::stod(word)); 37 | std::getline(s, word, ','); 38 | pose_est.push_back(std::stod(word)); 39 | std::getline(s, word, ','); 40 | pose_est.push_back(std::stod(word)); 41 | g2o::VertexSE2 pose; 42 | pose.setEstimateData(pose_est); 43 | 44 | 45 | while(!reader.eof()){ 46 | std::getline(reader, line); 47 | std::stringstream s(line); 48 | 49 | std::getline(s, word, ','); 50 | if(word == ""){ 51 | break; 52 | } 53 | float x = std::stof(word); 54 | std::getline(s, word, ','); 55 | float y = std::stof(word); 56 | 57 | temp_range.points_.push_back(Eigen::Vector2f(x,y)); 58 | } 59 | while(!reader.eof()){ 60 | std::getline(reader, line); 61 | std::stringstream s(line); 62 | 63 | std::getline(s, word, ','); 64 | if(word == ""){ 65 | break; 66 | } 67 | int start = std::stoi(word); 68 | std::getline(s, word, ','); 69 | int end = std::stoi(word); 70 | std::getline(s, word, ','); 71 | int return_end = std::stoi(word); 72 | std::getline(s, word, ','); 73 | float originx = std::stof(word); 74 | std::getline(s, word, ','); 75 | float originy = std::stof(word); 76 | 77 | temp_range.meta_data_.push_back({start, return_end, end, Eigen::Vector2f(originx,originy)}); 78 | } 79 | std::cout << temp_range.points_.size() << std::endl; 80 | std::cout << temp_range.meta_data_.size() << std::endl; 81 | Submap submap(1, 0.1); 82 | mapping::MultirangeDataInserter range_data_inserter(0.8,0.2); 83 | range_data_inserter.Insert(temp_range, &submap.probability_grid); 84 | submap.probability_grid = std::move(*static_cast(submap.probability_grid.ComputeCroppedGrid().release())); 85 | mapping::scan_matching::proto::FastCorrelativeScanMatcherOptions2D matcher_options; 86 | matcher_options.set_angular_search_window(0.7); 87 | matcher_options.set_linear_search_window(5.0); 88 | matcher_options.set_branch_and_bound_depth(6); 89 | submap.fix_submap(&pose, matcher_options, 1.0f); 90 | 91 | submap.range = std::move(temp_range); 92 | 93 | // get scan 94 | 95 | std::fstream reader2; 96 | reader2.open(fn2, std::ios::in); 97 | 98 | sensor::PointCloud c_pc; 99 | while(!reader2.eof()){ 100 | std::getline(reader2, line); 101 | std::stringstream s(line); 102 | 103 | std::getline(s, word, ','); 104 | if(word == ""){ 105 | break; 106 | } 107 | float x = std::stof(word); 108 | std::getline(s, word, ','); 109 | float y = std::stof(word); 110 | std::getline(s, word, ','); 111 | 112 | if(reverse){ 113 | c_pc.push_back({{-x,-y,0.0f}}); 114 | }else{ 115 | c_pc.push_back({{x,y,0.0f}}); 116 | } 117 | } 118 | 119 | // match 120 | float score = 0; 121 | transform::Rigid2d pose_estimate; 122 | submap.matcher.get().Match(transform::Rigid2d::Identity(), c_pc, 0.4, &score, &pose_estimate); 123 | std::cout << "score" << score << std::endl; 124 | 125 | std::cout << pose.estimate().toVector() << std::endl; 126 | 127 | // analysis 128 | std::cout << "c_pc size: " << c_pc.size() << std::endl; 129 | // for(auto it = submap.matcher.get().m.begin(); it != submap.matcher.get().m.end(); it++){ 130 | // std::cout << it->first << " : " << it->second << std::endl; 131 | // } 132 | 133 | // visualize 134 | std::vector x, y; 135 | x.reserve(c_pc.size()); 136 | y.reserve(c_pc.size()); 137 | auto _pef = pose_estimate.cast(); 138 | for (auto& p : c_pc) { 139 | Eigen::Vector2f pi = (_pef * p.position.head<2>() - submap.box.min()) / submap.grid.info.resolution; 140 | x.push_back(pi[0]); 141 | y.push_back(pi[1]); 142 | } 143 | auto& grid = submap.grid; 144 | plt::clf(); 145 | plt::imshow((unsigned char*)grid.data.data(), grid.info.height, grid.info.width, 1, {{"cmap", "gray"}}); 146 | plt::scatter(x, y, 5.0); 147 | plt::axis("equal"); 148 | plt::show(); 149 | } 150 | 151 | 152 | int main(int argc, char **argv){ 153 | construct_submap(); 154 | return 0; 155 | } --------------------------------------------------------------------------------