├── .gitignore ├── 1.png ├── 2.png ├── CMakeLists.txt ├── CMakeLists1.txt ├── LICENSE ├── README.md ├── __init__.py ├── config ├── convert_tf_to_trt.py ├── convert_to_trt_with_less_class_labels.py ├── gen_cam2lidar_from_l2c.py ├── generate_cam2lidar.py ├── infer_optimize.py ├── infer_optimize_mobilenetv2.py ├── lidar2realsense.py ├── mobilenet_const_nclt_2019_06_11.pb ├── mobilenet_nclt │ ├── optimized_graph_mobilenet_const_2019-06-15.pb │ ├── optimized_graph_mobilenet_trt32_2019-06-15.pb │ └── optimized_graph_mobilenet_trt_2019-06-15.pb ├── nclt_cam2lidar.py ├── nclt_cams │ ├── cam2lidar_1.npy │ ├── cam2lidar_4.npy │ ├── cam2lidar_5.npy │ ├── nclt_intrinsic_1.npy │ ├── nclt_intrinsic_4.npy │ ├── nclt_intrinsic_5.npy │ ├── x_body_lb3.csv │ ├── x_lb3_c0.csv │ ├── x_lb3_c1.csv │ ├── x_lb3_c2.csv │ ├── x_lb3_c3.csv │ ├── x_lb3_c4.csv │ └── x_lb3_c5.csv ├── optimize_graph.bash ├── optimize_graph_working.bash ├── optimized_mobilenet_const_nclt_2019_06_11.pb ├── realsense2lidar.npy ├── realsense_intrinsic.npy └── remove_is_train_in_graph.py ├── data ├── 2019-01-08-00-59-59.bag └── nclt_04_29 │ └── label2color.yaml ├── include └── SegmentationMapping │ ├── PointSegmentedDistribution.hpp │ ├── labeled_pc_map.hpp │ ├── lidar_camera_projection.hpp │ ├── stereo_segmentation.hpp │ └── tf_inference.hpp ├── launch ├── belltower.launch ├── cassie_lidar_py.launch ├── cassie_lidar_semanticbki.launch ├── cassie_stereo_cpp.launch ├── cassie_stereo_map_only.launch ├── cassie_stereo_py.launch ├── cassie_stereo_py_multi_camera.launch ├── cassie_stereo_py_two_camera.launch ├── nclt_classification.launch ├── nclt_distribution.launch ├── nclt_distribution_deeplab.launch ├── nclt_label_octomap.launch ├── nclt_label_voxblox_map.launch ├── nclt_labeled_pc_painting_node.launch ├── offline_cassie_stereo_py.launch ├── realsense.launch ├── rvsm_nclt_semantic_octmap.launch ├── segmentation_only.launch └── traversability_segmentation_only.launch ├── msg ├── ImageLabelDistribution.msg └── ImageLabelDistribution.msg~ ├── octomap.png ├── package.xml ├── scripts ├── NeuralNetConfigs.py ├── __init__.py ├── distort.py ├── gen_nclt_gt_pc.bash ├── get_projected_gt.py ├── gt_pose2bag.py ├── helper.py ├── label2color.py ├── label2traversability.py ├── project_vel_to_cam.py ├── projection.py ├── rgbd2pc.py ├── segmentation_node ├── segmentation_node_two_camera ├── segmentation_projection_node ├── segmentation_visual_node └── traversability_segmentation_node ├── setup.py ├── src ├── labeled_pc_map_node.cpp ├── lidar_projection_node.cpp ├── query_semantic_tree.cpp ├── rvsm_nclt_node.cpp └── stereo_segmentation_node.cpp └── youtube.png /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | .hypothesis/ 50 | .pytest_cache/ 51 | 52 | # Translations 53 | *.mo 54 | *.pot 55 | 56 | # Django stuff: 57 | *.log 58 | local_settings.py 59 | db.sqlite3 60 | 61 | # Flask stuff: 62 | instance/ 63 | .webassets-cache 64 | 65 | # Scrapy stuff: 66 | .scrapy 67 | 68 | # Sphinx documentation 69 | docs/_build/ 70 | 71 | # PyBuilder 72 | target/ 73 | 74 | # Jupyter Notebook 75 | .ipynb_checkpoints 76 | 77 | # IPython 78 | profile_default/ 79 | ipython_config.py 80 | 81 | # pyenv 82 | .python-version 83 | 84 | # celery beat schedule file 85 | celerybeat-schedule 86 | 87 | # SageMath parsed files 88 | *.sage.py 89 | 90 | # Environments 91 | .env 92 | .venv 93 | env/ 94 | venv/ 95 | ENV/ 96 | env.bak/ 97 | venv.bak/ 98 | 99 | # Spyder project settings 100 | .spyderproject 101 | .spyproject 102 | 103 | # Rope project settings 104 | .ropeproject 105 | 106 | # mkdocs documentation 107 | /site 108 | 109 | # mypy 110 | .mypy_cache/ 111 | .dmypy.json 112 | dmypy.json 113 | 114 | # Pyre type checker 115 | .pyre/ 116 | 117 | # emacs 118 | *.*~ 119 | *.*# 120 | -------------------------------------------------------------------------------- /1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/1.png -------------------------------------------------------------------------------- /2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/2.png -------------------------------------------------------------------------------- /CMakeLists1.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(SegmentationMapping) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | SET(CMAKE_BUILD_TYPE Release) 7 | 8 | #SET(EIGEN_INCLUDE_DIR /usr/local/include/eigen3) 9 | #SET(ENV{TENSORFLOW_SOURCE_DIR} "/home/biped/thirdparty/tensorflow/") 10 | #SET(ENV{TENSORFLOW_BUILD_DIR} "/home/biped/thirdparty/tensorflow/build_docker/") 11 | #list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/Modules) 12 | 13 | 14 | #find_package(TensorFlow REQUIRED) 15 | #TensorFlow_REQUIRE_C_LIBRARY() 16 | #TensorFlow_REQUIRE_SOURCE() 17 | #message(${TensorFlow_SOURCE_DIR}) 18 | 19 | 20 | ## Find catkin macros and libraries 21 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 22 | ## is used, also find other catkin packages 23 | find_package(catkin REQUIRED COMPONENTS 24 | message_filters 25 | rospy 26 | octomap 27 | sensor_msgs 28 | geometry_msgs 29 | nav_msgs 30 | tf2_ros 31 | #cassie_msgs 32 | cv_bridge 33 | pcl_ros 34 | tf_conversions 35 | eigen_conversions 36 | image_geometry 37 | message_generation 38 | ) 39 | 40 | 41 | SET(EIGEN_INCLUDE_DIR /usr/include/eigen3) 42 | #SET(EIGEN_INCLUDE_DIR /usr/local/include/eigen3) 43 | #find_package (Eigen3 REQUIRED ) 44 | #IF( NOT EIGEN3_INCLUDE_DIR ) 45 | # MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.") 46 | #ENDIF() 47 | #message(${EIGEN3_INCLUDE_DIR}) 48 | 49 | 50 | ## System dependencies are found with CMake's conventions 51 | # find_package(Boost REQUIRED COMPONENTS system) 52 | find_package(PCL REQUIRED) 53 | link_directories(${PCL_LIBRARY_DIRS}) 54 | add_definitions(${PCL_DEFINITIONS}) 55 | message("PCL library path is ") 56 | message(${PCL_INCLUDE_DIRS}) 57 | message(${PCL_LIBRARY_DIRS}) 58 | 59 | 60 | 61 | 62 | find_package(octomap REQUIRED) 63 | include_directories(${OCTOMAP_INCLUDE_DIRS}) 64 | link_directories(${OCTOMAP_LIBRARY_DIRS}) 65 | message(${OCTOMAP_INCLUDE_DIRS}) 66 | 67 | 68 | find_package(OpenCV REQUIRED) 69 | 70 | 71 | ## Uncomment this if the package has a setup.py. This macro ensures 72 | ## modules and global scripts declared therein get installed 73 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 74 | catkin_python_setup() 75 | 76 | ################################################ 77 | ## Declare ROS messages, services and actions ## 78 | ################################################ 79 | 80 | ## To declare and build messages, services or actions from within this 81 | ## package, follow these steps: 82 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 83 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 84 | ## * In the file package.xml: 85 | ## * add a build_depend tag for "message_generation" 86 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 87 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 88 | ## but can be declared for certainty nonetheless: 89 | ## * add a exec_depend tag for "message_runtime" 90 | ## * In this file (CMakeLists.txt): 91 | ## * add "message_generation" and every package in MSG_DEP_SET to 92 | ## find_package(catkin REQUIRED COMPONENTS ...) 93 | ## * add "message_runtime" and every package in MSG_DEP_SET to 94 | ## catkin_package(CATKIN_DEPENDS ...) 95 | ## * uncomment the add_*_files sections below as needed 96 | ## and list every .msg/.srv/.action file to be processed 97 | ## * uncomment the generate_messages entry below 98 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 99 | 100 | ## Generate messages in the 'msg' folder 101 | add_message_files( 102 | FILES 103 | ImageLabelDistribution.msg 104 | ) 105 | 106 | ## Generate services in the 'srv' folder 107 | # add_service_files( 108 | # FILES 109 | # Service1.srv 110 | # Service2.srv 111 | # ) 112 | 113 | ## Generate actions in the 'action' folder 114 | # add_action_files( 115 | # FILES 116 | # Action1.action 117 | # Action2.action 118 | # ) 119 | 120 | ## Generate added messages and services with any dependencies listed here 121 | generate_messages( 122 | DEPENDENCIES 123 | std_msgs 124 | ) 125 | 126 | ################################################ 127 | ## Declare ROS dynamic reconfigure parameters ## 128 | ################################################ 129 | 130 | ## To declare and build dynamic reconfigure parameters within this 131 | ## package, follow these steps: 132 | ## * In the file package.xml: 133 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 134 | ## * In this file (CMakeLists.txt): 135 | ## * add "dynamic_reconfigure" to 136 | ## find_package(catkin REQUIRED COMPONENTS ...) 137 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 138 | ## and list every .cfg file to be processed 139 | 140 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 141 | # generate_dynamic_reconfigure_options( 142 | # cfg/DynReconf1.cfg 143 | # cfg/DynReconf2.cfg 144 | # ) 145 | 146 | ################################### 147 | ## catkin specific configuration ## 148 | ################################### 149 | ## The catkin_package macro generates cmake config files for your package 150 | ## Declare things to be passed to dependent projects 151 | ## INCLUDE_DIRS: uncomment this if your package contains header files 152 | ## LIBRARIES: libraries you create in this project that dependent projects also need 153 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 154 | ## DEPENDS: system dependencies of this project that dependent projects also need 155 | catkin_package( 156 | INCLUDE_DIRS include/SegmentationMapping 157 | # LIBRARIES segmentation_projection 158 | CATKIN_DEPENDS message_filters rospy sensor_msgs message_runtime 159 | # DEPENDS system_lib 160 | ) 161 | 162 | ########### 163 | ## Build ## 164 | ########### 165 | 166 | ## Specify additional locations of header files 167 | ## Your package locations should be listed before other locations 168 | include_directories( 169 | include/SegmentationMapping 170 | ${catkin_INCLUDE_DIRS} 171 | ${PCL_INCLUDE_DIRS} 172 | ${EIGEN_INCLUDE_DIR} 173 | ${OCTOMAP_INCLUDE_DIRS} 174 | 175 | ) 176 | 177 | 178 | 179 | ## Declare a C++ library 180 | # add_library(${PROJECT_NAME} 181 | # src/${PROJECT_NAME}/segmentation_projection.cpp 182 | # ) 183 | 184 | ## Add cmake target dependencies of the library 185 | ## as an example, code may need to be generated before libraries 186 | ## either from message generation or dynamic reconfigure 187 | #add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 188 | 189 | ## Declare a C++ executable 190 | ## With catkin_make all packages are built within a single CMake context 191 | ## The recommended prefix ensures that target names across packages don't collide 192 | add_executable(labeled_pc_map_node src/labeled_pc_map_node.cpp) 193 | #add_executable(nclt_map_node src/rvsm_nclt_node.cpp) 194 | #add_executable(query_semantic_tree src/query_semantic_tree.cpp) 195 | add_executable(stereo_segmentation_node src/stereo_segmentation_node.cpp) 196 | 197 | ## Rename C++ executable without prefix 198 | ## The above recommended prefix causes long target names, the following renames the 199 | ## target back to the shorter version for ease of user use 200 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 201 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 202 | 203 | ## Add cmake target dependencies of the executable 204 | ## same as for the library above 205 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 206 | 207 | add_dependencies(stereo_segmentation_node ${catkin_EXPORTED_TARGETS}) 208 | ## Specify libraries to link a library or executable target against 209 | target_link_libraries(labeled_pc_map_node 210 | ${catkin_LIBRARIES} 211 | ${PCL_COMMON_LIBRARIES} 212 | ${PCL_IO_LIBRARIES} 213 | ${OCTOMAP_LIBRARIES} 214 | ) 215 | #target_link_libraries(nclt_map_node 216 | # ${catkin_LIBRARIES} 217 | # ${PCL_COMMON_LIBRARIES} 218 | # ${PCL_IO_LIBRARIES} 219 | # ${OCTOMAP_LIBRARIES} 220 | #) 221 | #target_link_libraries(query_semantic_tree 222 | # ${catkin_LIBRARIES} 223 | # ${PCL_COMMON_LIBRARIES} 224 | # ${PCL_IO_LIBRARIES} 225 | # ${OCTOMAP_LIBRARIES} 226 | # ) 227 | #target_include_directories(stereo_segmentation_node PRIVATE ${TensorFlow_SOURCE_DIR}) 228 | target_link_libraries(stereo_segmentation_node # PRIVATE TensorFlow_DEP 229 | ${EIGEN_INCLUDE_DIR} 230 | ${catkin_LIBRARIES} 231 | ${OpenCV_LIBS} 232 | ) 233 | 234 | ############# 235 | ## Install ## 236 | ############# 237 | 238 | # all install targets should use catkin DESTINATION variables 239 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 240 | 241 | ## Mark executable scripts (Python etc.) for installation 242 | ## in contrast to setup.py, you can choose the destination 243 | install(PROGRAMS 244 | scripts/segmentation_projection_node scripts/projection.py scripts/gt_pose2bag.py scripts/helper.py scripts/segmentation_node 245 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 246 | ) 247 | 248 | ## Mark executables and/or libraries for installation 249 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 250 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 251 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 252 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 253 | # ) 254 | 255 | ## Mark cpp header files for installation 256 | # install(DIRECTORY include/${PROJECT_NAME}/ 257 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 258 | # FILES_MATCHING PATTERN "*.h" 259 | # PATTERN ".svn" EXCLUDE 260 | # ) 261 | 262 | ## Mark other files for installation (e.g. launch and bag files, etc.) 263 | # install(FILES 264 | # # myfile1 265 | # # myfile2 266 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 267 | # ) 268 | 269 | ############# 270 | ## Testing ## 271 | ############# 272 | 273 | ## Add gtest based cpp test target and link libraries 274 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_segmentation_projection.cpp) 275 | # if(TARGET ${PROJECT_NAME}-test) 276 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 277 | # endif() 278 | 279 | ## Add folders to be run by python nosetests 280 | # catkin_add_nosetests(test) 281 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 University of Michigan Dynamic Legged Locomotion Robotics Lab 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 | # SegmentationMapping 2 | * Label pointcloud from stereo cameras or lidar with Tensorflow trained neural nets graph.pb 3 | * Build 3D Semantic voxel maps, 2D occupancy maps, 2D obstacle distance maps 4 | 5 | The [Youtube video of the Cassie Bipedal robot](https://youtu.be/uFyT8zCg1Kk)'s perception and mapping system is powered by this library. 6 | [![Cassie goes autonomous on Umich's wavefield](https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/jetson/youtube.png)](https://youtu.be/uFyT8zCg1Kk) 7 | 8 | Here is a semantic map built on NCLT dataset 9 | ![The Semantic Map build on NCLT dataset](https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/multi-camera/octomap.png "NCLT octomap") 10 | 11 | 12 | ## Pipeline 13 | Two types of accepted inputs (from ROS topics): 14 | * Raw RGBD images from depth cameras, or 15 | * Lidar scans + RGB images. 16 | 17 | C++: `pcl`, `eigen`, `xmlrpcpp` 18 | 19 | 20 | Outputs (in ROS messages): 21 | * Labeled images, each pixel with a distribution across classes 22 | * Labeled PointCloud ROS message 23 | * Semantic Octomap 24 | * 2D occupancy maps 25 | * 2D cost map, cost computed from distance to closest obstacles 26 | 27 | ![alt text](https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/multi-camera/1.png "Pipeline_1") 28 | ![alt text](https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/multi-camera/2.png "Pipeline_2") 29 | 30 | ## Performance 31 | Tested on 32 | * AlienWare Laptop (running in realtime for the Cassie Blue experiement): 3FPS on i7-6820, 32GB RAM, GTX1070 33 | * Dell XPS: 5FPS on i7-7700, 32GB RAM, GTX1070. 34 | * Jetson Xavier: Around 4Hz 35 | 36 | ## Dependencies 37 | ROS: Tested on Kinetic + Ubuntu 16.04, Melodic + Ubuntu 18.04 38 | 39 | Neural Network: 40 | * `tensorflow-gpu-1.8.0` 41 | * `tensorrt4` 42 | * `cuda-9.0`, 43 | * `cudnn-7` 44 | 45 | Python thirdparty: 46 | * [`python-pcl`](https://github.com/strawlab/python-pcl) 47 | * `python-opencv` 48 | 49 | C++: `pcl`, `eigen`, `OpenCV` 50 | 51 | ros thirdparty: 52 | * [`semantic octomap`](https://github.com/UMich-BipedLab/octomap.git): Modified octomap, supporting Bayesian updates for semantic label fusion 53 | * [`octomap_ros`](https://github.com/OctoMap/octomap_ros): Put it in the same catkin workspace as the SegmentationMappping repo. 54 | * [`octomap_msgs`](https://github.com/OctoMap/octomap_msgs): Put it in the same catkin workspace as the SegmentationMappping repo. 55 | * [`ros_numpy`](https://github.com/eric-wieser/ros_numpy): Put it in the same catkin workspace as the SegmentationMappping repo. 56 | 57 | ## Compiling 58 | Make sure that your semantic octomap is build. Make sure that you haven't install the native `octomap` and `octomap_ros` from `apt-get`. Then run 59 | `catkin_make install -DCMAKE_BUILD_TYPE=Release -Doctomap_DIR=${you_octomap_source_directory}/lib/cmake/octomap/ --pkg SegmentationMapping` 60 | 61 | ## launchfiles 62 | * run on Cassie with a single stereo camera (Intel Realsense): `roslaunch SegmentationMapping cassie_stereo_py.launch` 63 | * run nclt dataset: `roslaunch SegmentationMapping nclt_distribution_deeplab.launch` 64 | * run on mulitple cameras with multiple segmentations at the same time: `roslaunch SegmentationMapping cassie_stereo_py_multi_camera.launch` 65 | 66 | 85 | 86 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/__init__.py -------------------------------------------------------------------------------- /config/convert_tf_to_trt.py: -------------------------------------------------------------------------------- 1 | # Import TensorFlow and TensorRT 2 | import tensorflow as tf 3 | import tensorflow.contrib.tensorrt as trt 4 | # Inference with TF-TRT frozen graph workflow: 5 | 6 | import sys, os 7 | from tqdm import tqdm 8 | import numpy as np 9 | graph_name = sys.argv[1] 10 | 11 | graph = tf.Graph() 12 | with graph.as_default(): 13 | with tf.Session(graph=graph) as sess: 14 | # First deserialize your frozen graph: 15 | with tf.gfile.GFile(sys.argv[1], 'rb') as f: 16 | graph_def = tf.GraphDef() 17 | graph_def.ParseFromString(f.read()) 18 | # Now you can create a TensorRT inference graph from your 19 | # frozen graph: 20 | #for n in graph_def.node: 21 | # print(n.name) 22 | trt_graph = trt.create_inference_graph( 23 | input_graph_def=graph_def, 24 | outputs=[ 'network/upscore_8s/upscore8/upscore8/BiasAdd'], 25 | max_batch_size=1, 26 | max_workspace_size_bytes=2500000000, 27 | precision_mode='INT8') 28 | print('\n\nFinish tensorrt creation, now import to tensorflow') 29 | # Import the TensorRT graph into a new graph and run: 30 | output_node = tf.import_graph_def( 31 | trt_graph, 32 | return_elements=[ 'network/upscore_8s/upscore8/upscore8/BiasAdd:0' ]) 33 | print(output_node) 34 | img = np.ones((1, 480, 640, 3), dtype=np.uint8) 35 | #for n in graph_def.node: 36 | # print(n.name) 37 | x = graph.get_tensor_by_name('import/network/input/Placeholder:0') 38 | for _ in tqdm(range(1000)): 39 | sess.run([output_node], feed_dict={x: img}) 40 | -------------------------------------------------------------------------------- /config/convert_to_trt_with_less_class_labels.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import numpy as np 3 | import time 4 | import argparse 5 | from tqdm import tqdm 6 | import tensorflow.contrib.tensorrt as trt 7 | import pdb 8 | parser = argparse.ArgumentParser(description="Inference test") 9 | parser.add_argument('--graph', type=str) 10 | parser.add_argument('--new_num_class', type=int, default=14) 11 | parser.add_argument('--precision', type=str, default='FP32') 12 | parser.add_argument('--trt_optimized_graph', type=str) 13 | # Parse the arguments 14 | args = parser.parse_args() 15 | 16 | new_num_class = int(args.new_num_class) 17 | 18 | if args.graph is not None: 19 | with tf.gfile.GFile(args.graph, 'rb') as f: 20 | graph_def = tf.GraphDef() 21 | graph_def.ParseFromString(f.read()) 22 | else: 23 | raise ValueError("--graph should point to the input graph file.") 24 | 25 | G = tf.Graph() 26 | config = tf.ConfigProto() 27 | config.gpu_options.allow_growth = True 28 | 29 | with tf.Session(graph=G,config=config) as sess: 30 | # The inference is done till the argmax layer on the logits, as the softmax layer is not important. 31 | y, = tf.import_graph_def(graph_def, return_elements=['network/output/ArgMax:0'], name='') 32 | #y, = tf.import_graph_def(graph_def, return_elements=['SemanticPredictions:0']) 33 | #print('Operations in Graph:') 34 | #print([op.name for op in G.get_operations()]) 35 | x = G.get_tensor_by_name('network/input/Placeholder:0') 36 | #b = G.get_tensor_by_name('import/network/input/Placeholder_2:0') 37 | d = G.get_tensor_by_name("network/upscore_8s/upscore8/upscore8/BiasAdd:0") 38 | 39 | distribution_sliced = d[:, :, :, :new_num_class] 40 | class_prob_tensor = tf.nn.softmax(distribution_sliced, name='network/output/ClassDistribution') 41 | label_tensor = tf.argmax(class_prob_tensor, axis=-1, name='network/output/ClassIndexPrediction', output_type=tf.int32) 42 | 43 | 44 | print("tensorrt graph conversion...") 45 | trt_graph = trt.create_inference_graph( 46 | input_graph_def=tf.get_default_graph().as_graph_def(), 47 | outputs=[ 'network/output/ClassDistribution', 'network/output/ClassIndexPrediction' ], 48 | max_batch_size=1, 49 | max_workspace_size_bytes=2500000000, 50 | precision_mode=args.precision) 51 | print('\n\nFinish tensorrt creation, now import to tensorflow') 52 | 53 | # Import the TensorRT graph into a new graph and run: 54 | #print(distribution_sliced.shape) 55 | #x = G.get_tensor_by_name('import/ImageTensor:0') 56 | #d = G.get_tensor_by_name('import/ResizeBilinear_2:0') 57 | #tf.global_variables_initializer().run() 58 | g_new = tf.Graph() 59 | with tf.Session(graph=g_new,config=config) as sess2: 60 | output_node = tf.import_graph_def( 61 | trt_graph, 62 | #G.as_graph_def(), 63 | return_elements=[ 'network/output/ClassDistribution', 'network/output/ClassIndexPrediction' ], name='') 64 | 65 | 66 | x = g_new.get_tensor_by_name('network/input/Placeholder:0') 67 | class_prob_tensor = g_new.get_tensor_by_name('network/output/ClassDistribution:0') 68 | label_tensor = g_new.get_tensor_by_name('network/output/ClassIndexPrediction:0') 69 | print('class_prob_tensor.shape is ',class_prob_tensor.shape) 70 | 71 | img = np.ones((1, 640, 480, 3), dtype=np.uint8) 72 | 73 | # Experiment should be repeated in order to get an accurate value for the inference time and FPS. 74 | for _ in tqdm(range(100)): 75 | start = time.time() 76 | #out = sess.run(y, feed_dict={x: img, b:False}) 77 | out = sess2.run([label_tensor,class_prob_tensor], feed_dict={x: img}) 78 | 79 | tf.io.write_graph(g_new, './', args.trt_optimized_graph, as_text=False) 80 | -------------------------------------------------------------------------------- /config/gen_cam2lidar_from_l2c.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import cv2, sys 4 | 5 | cam2lidar_path = sys.argv[1] 6 | 7 | # roll 8 | def Rx(radian): 9 | M = np.zeros((4,4)) 10 | M[0,0] = 1; 11 | M[1,1] = np.cos(radian) 12 | M[1,2] = -np.sin(radian) 13 | M[2,1] = np.sin(radian) 14 | M[2,2] = np.cos(radian) 15 | M[3,3] = 1 16 | return M 17 | 18 | # pitch 19 | def Ry( radian): 20 | M = np.zeros((4,4)) 21 | M[0,0] = np.cos(radian) 22 | M[1,1] = 1 23 | M[0,2] = np.sin(radian) 24 | M[2,0] = -np.sin(radian) 25 | M[2,2] = np.cos(radian) 26 | M[3,3] = 1 27 | 28 | return M 29 | 30 | # yawn 31 | def Rz( radian): 32 | M = np.identity(4) 33 | M[0,0] = np.cos(radian) 34 | M[0,1] = -np.sin(radian) 35 | M[1,0] = np.sin(radian) 36 | M[1,1] = np.cos(radian) 37 | M[2,2] = 1 38 | M[3,3] = 1 39 | return M 40 | 41 | 42 | 43 | 44 | # The one that we measured for realsense2lidar 45 | #c2l_vec = np.array([0.19, 0.04, 0.2, -0.0124 * np.pi / 180, -89.5533* np.pi/180, 91.2846*np.pi/180]) 46 | # The one we actually used in this projection package: I tuned the numbers based on 47 | # my eyes' judgement of projection quality! 48 | 49 | 50 | l2c_v = np.array([0.041862, -0.001905, -0.000212, 160.868615* np.pi / 180, 89.914152* np.pi / 180, 160.619894* np.pi / 180]) 51 | rot = np.matmul(np.matmul(Rz(l2c_v[5]), Ry(l2c_v[4]) ), Rx(l2c_v[3]) ) 52 | T_l2c = np.identity(4) 53 | T_l2c[:3,:3] = rot[:3, :3] 54 | T_l2c[:3, 3] = np.transpose(l2c_v[:3]) 55 | np.save(cam2lidar_path, np.linalg.inv(T_l2c)) 56 | 57 | -------------------------------------------------------------------------------- /config/generate_cam2lidar.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import cv2 4 | 5 | 6 | 7 | cam2lidar_path = sys.argv[1] 8 | 9 | # roll 10 | def Rx(radian): 11 | M = np.zeros((4,4)) 12 | M[0,0] = 1; 13 | M[1,1] = np.cos(radian) 14 | M[1,2] = -np.sin(radian) 15 | M[2,1] = np.sin(radian) 16 | M[2,2] = np.cos(radian) 17 | M[3,3] = 1 18 | return M 19 | 20 | # pitch 21 | def Ry( radian): 22 | M = np.zeros((4,4)) 23 | M[0,0] = np.cos(radian) 24 | M[1,1] = 1 25 | M[0,2] = np.sin(radian) 26 | M[2,0] = -np.sin(radian) 27 | M[2,2] = np.cos(radian) 28 | M[3,3] = 1 29 | 30 | return M 31 | 32 | # yawn 33 | def Rz( radian): 34 | M = np.identity(4) 35 | M[0,0] = np.cos(radian) 36 | M[0,1] = -np.sin(radian) 37 | M[1,0] = np.sin(radian) 38 | M[1,1] = np.cos(radian) 39 | M[2,2] = 1 40 | M[3,3] = 1 41 | return M 42 | 43 | 44 | 45 | 46 | # The one that we measured for realsense2lidar 47 | #c2l_vec = np.array([0.19, 0.04, 0.2, -0.0124 * np.pi / 180, -89.5533* np.pi/180, 91.2846*np.pi/180]) 48 | # The one we actually used in this projection package: I tuned the numbers based on 49 | # my eyes' judgement of projection quality! 50 | c2l_vec = np.array([0.19, 0.04, 0.2, -1.5 * np.pi / 180, -81* np.pi/180, 93.2846*np.pi/180]) 51 | 52 | 53 | rot = np.matmul(np.matmul(Rz(c2l_vec[5]), Ry(c2l_vec[4]) ), Rx(c2l_vec[3]) ) 54 | T_c2l = np.identity(4) 55 | T_c2l[:3,:3] = rot[:3, :3] 56 | T_c2l[:3, 3] = np.transpose(c2l_vec[:3]) 57 | np.save(cam2lidar_path, T_c2l) 58 | 59 | -------------------------------------------------------------------------------- /config/infer_optimize.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import numpy as np 3 | import time 4 | import argparse 5 | from tqdm import tqdm 6 | import pdb 7 | parser = argparse.ArgumentParser(description="Inference test") 8 | parser.add_argument('--graph', type=str) 9 | parser.add_argument('--iterations', default=1000, type=int) 10 | 11 | # Parse the arguments 12 | args = parser.parse_args() 13 | 14 | if args.graph is not None: 15 | with tf.gfile.GFile(args.graph, 'rb') as f: 16 | graph_def = tf.GraphDef() 17 | graph_def.ParseFromString(f.read()) 18 | else: 19 | raise ValueError("--graph should point to the input graph file.") 20 | 21 | G = tf.Graph() 22 | config = tf.ConfigProto() 23 | config.gpu_options.allow_growth = True 24 | 25 | with tf.Session(graph=G,config=config) as sess: 26 | # The inference is done till the argmax layer on the logits, as the softmax layer is not important. 27 | #y, = tf.import_graph_def(graph_def, return_elements=['network/output/ArgMax:0']) 28 | y, = tf.import_graph_def(graph_def, return_elements=['network/output/ClassIndexPrediction:0'], name='') 29 | #y, = tf.import_graph_def(graph_def, return_elements=['SemanticPredictions:0']) 30 | #print('Operations in Graph:') 31 | #print([op.name for op in G.get_operations()]) 32 | x = G.get_tensor_by_name('network/input/Placeholder:0') 33 | #b = G.get_tensor_by_name('import/network/input/Placeholder_2:0') 34 | #d = G.get_tensor_by_name("import/network/upscore_8s/upscore8/upscore8/BiasAdd:0") 35 | d = G.get_tensor_by_name("network/output/ClassDistribution:0") 36 | print(d.shape) 37 | #x = G.get_tensor_by_name('import/ImageTensor:0') 38 | #d = G.get_tensor_by_name('import/ResizeBilinear_2:0') 39 | #tf.global_variables_initializer().run() 40 | 41 | img = np.ones((1, 640, 480, 3), dtype=np.uint8) 42 | 43 | # Experiment should be repeated in order to get an accurate value for the inference time and FPS. 44 | for _ in tqdm(range(args.iterations)): 45 | start = time.time() 46 | #out = sess.run(y, feed_dict={x: img, b:False}) 47 | out = sess.run([y,d], feed_dict={x: img}) 48 | 49 | 50 | -------------------------------------------------------------------------------- /config/infer_optimize_mobilenetv2.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import numpy as np 3 | import time 4 | import argparse 5 | from tqdm import tqdm 6 | import pdb 7 | parser = argparse.ArgumentParser(description="Inference test") 8 | parser.add_argument('--graph', type=str) 9 | parser.add_argument('--iterations', default=1000, type=int) 10 | 11 | # Parse the arguments 12 | args = parser.parse_args() 13 | 14 | if args.graph is not None: 15 | with tf.gfile.GFile(args.graph, 'rb') as f: 16 | graph_def = tf.GraphDef() 17 | graph_def.ParseFromString(f.read()) 18 | else: 19 | raise ValueError("--graph should point to the input graph file.") 20 | 21 | G = tf.Graph() 22 | config = tf.ConfigProto() 23 | config.gpu_options.allow_growth = True 24 | 25 | with tf.Session(graph=G,config=config) as sess: 26 | # The inference is done till the argmax layer on the logits, as the softmax layer is not important. 27 | y, = tf.import_graph_def(graph_def, return_elements=['SemanticPredictions:0']) 28 | #y, = tf.import_graph_def(graph_def, return_elements=['SemanticPredictions:0']) 29 | #print('Operations in Graph:') 30 | #print([op.name for op in G.get_operations()]) 31 | x = G.get_tensor_by_name('import/ImageTensor:0') 32 | #b = G.get_tensor_by_name('import/network/input/Placeholder_2:0') 33 | #d = G.get_tensor_by_name("import/network/upscore_8s/upscore8/upscore8/BiasAdd:0") 34 | print(x.shape) 35 | #x = G.get_tensor_by_name('import/ImageTensor:0') 36 | #d = G.get_tensor_by_name('import/ResizeBilinear_2:0') 37 | tf.global_variables_initializer().run() 38 | 39 | img = np.ones((1, 512, 1024, 3), dtype=np.uint8) 40 | 41 | # Experiment should be repeated in order to get an accurate value for the inference time and FPS. 42 | for _ in tqdm(range(args.iterations)): 43 | start = time.time() 44 | out = sess.run(y, feed_dict={x: img}) 45 | 46 | 47 | -------------------------------------------------------------------------------- /config/lidar2realsense.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/lidar2realsense.py -------------------------------------------------------------------------------- /config/mobilenet_const_nclt_2019_06_11.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/mobilenet_const_nclt_2019_06_11.pb -------------------------------------------------------------------------------- /config/mobilenet_nclt/optimized_graph_mobilenet_const_2019-06-15.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/mobilenet_nclt/optimized_graph_mobilenet_const_2019-06-15.pb -------------------------------------------------------------------------------- /config/mobilenet_nclt/optimized_graph_mobilenet_trt32_2019-06-15.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/mobilenet_nclt/optimized_graph_mobilenet_trt32_2019-06-15.pb -------------------------------------------------------------------------------- /config/mobilenet_nclt/optimized_graph_mobilenet_trt_2019-06-15.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/mobilenet_nclt/optimized_graph_mobilenet_trt_2019-06-15.pb -------------------------------------------------------------------------------- /config/nclt_cam2lidar.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import cv2, sys 4 | 5 | 6 | 7 | # roll 8 | def Rx(radian): 9 | M = np.zeros((4,4)) 10 | M[0,0] = 1; 11 | M[1,1] = np.cos(radian) 12 | M[1,2] = -np.sin(radian) 13 | M[2,1] = np.sin(radian) 14 | M[2,2] = np.cos(radian) 15 | M[3,3] = 1 16 | return M 17 | 18 | # pitch 19 | def Ry( radian): 20 | M = np.zeros((4,4)) 21 | M[0,0] = np.cos(radian) 22 | M[1,1] = 1 23 | M[0,2] = np.sin(radian) 24 | M[2,0] = -np.sin(radian) 25 | M[2,2] = np.cos(radian) 26 | M[3,3] = 1 27 | 28 | return M 29 | 30 | # yawn 31 | def Rz( radian): 32 | M = np.identity(4) 33 | M[0,0] = np.cos(radian) 34 | M[0,1] = -np.sin(radian) 35 | M[1,0] = np.sin(radian) 36 | M[1,1] = np.cos(radian) 37 | M[2,2] = 1 38 | M[3,3] = 1 39 | return M 40 | 41 | def ssc_to_homo(ssc): 42 | 43 | # Convert 6-DOF ssc coordinate transformation to 4x4 homogeneous matrix 44 | # transformation 45 | 46 | sr = np.sin(np.pi/180.0 * ssc[3]) 47 | cr = np.cos(np.pi/180.0 * ssc[3]) 48 | 49 | sp = np.sin(np.pi/180.0 * ssc[4]) 50 | cp = np.cos(np.pi/180.0 * ssc[4]) 51 | 52 | sh = np.sin(np.pi/180.0 * ssc[5]) 53 | ch = np.cos(np.pi/180.0 * ssc[5]) 54 | 55 | H = np.zeros((4, 4)) 56 | 57 | H[0, 0] = ch*cp 58 | H[0, 1] = -sh*cr + ch*sp*sr 59 | H[0, 2] = sh*sr + ch*sp*cr 60 | H[1, 0] = sh*cp 61 | H[1, 1] = ch*cr + sh*sp*sr 62 | H[1, 2] = -ch*sr + sh*sp*cr 63 | H[2, 0] = -sp 64 | H[2, 1] = cp*sr 65 | H[2, 2] = cp*cr 66 | 67 | H[0, 3] = ssc[0] 68 | H[1, 3] = ssc[1] 69 | H[2, 3] = ssc[2] 70 | 71 | H[3, 3] = 1 72 | 73 | return H 74 | 75 | # r, p, y is in degree 76 | def sixdof_to_transformation(sixdof): 77 | x,y,z, r,p,y = sixdof 78 | vec = np.array([x,y,z, r * np.pi/180, p * np.pi/180, y * np.pi / 180]) 79 | rot = np.matmul(np.matmul(Rz(vec[5]), Ry(vec[4]) ), Rx(vec[3]) ) 80 | T = np.identity(4) 81 | T[:3,:3] = rot[:3, :3] 82 | T[:3, 3] = np.transpose(vec[:3]) 83 | return T 84 | 85 | # The one that we measured for realsense2lidar 86 | #c2l_vec = np.array([0.19, 0.04, 0.2, -0.0124 * np.pi / 180, -89.5533* np.pi/180, 91.2846*np.pi/180]) 87 | # The one we actually used in this projection package: I tuned the numbers based on 88 | # my eyes' judgement of projection quality! 89 | 90 | if __name__ == "__main__": 91 | 92 | x_lb3_c = np.genfromtxt(sys.argv[1], delimiter=',') 93 | 94 | x_body_lb3 = np.genfromtxt ("nclt_cams/x_body_lb3.csv", delimiter=",") 95 | 96 | 97 | 98 | T_lb3_c = ssc_to_homo(x_lb3_c ) 99 | T_body_lb3 = ssc_to_homo(x_body_lb3) 100 | 101 | T_lb3_body = np.linalg.inv(T_body_lb3) 102 | T_c_lb3 = np.linalg.inv(T_lb3_c) 103 | # because nclt lidar are in the body frame ......... 104 | T_c_body = np.matmul(T_c_lb3, T_lb3_body) 105 | print(T_c_body) 106 | np.save(sys.argv[2], (T_c_body)) 107 | 108 | -------------------------------------------------------------------------------- /config/nclt_cams/cam2lidar_1.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/nclt_cams/cam2lidar_1.npy -------------------------------------------------------------------------------- /config/nclt_cams/cam2lidar_4.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/nclt_cams/cam2lidar_4.npy -------------------------------------------------------------------------------- /config/nclt_cams/cam2lidar_5.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/nclt_cams/cam2lidar_5.npy -------------------------------------------------------------------------------- /config/nclt_cams/nclt_intrinsic_1.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/nclt_cams/nclt_intrinsic_1.npy -------------------------------------------------------------------------------- /config/nclt_cams/nclt_intrinsic_4.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/nclt_cams/nclt_intrinsic_4.npy -------------------------------------------------------------------------------- /config/nclt_cams/nclt_intrinsic_5.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/nclt_cams/nclt_intrinsic_5.npy -------------------------------------------------------------------------------- /config/nclt_cams/x_body_lb3.csv: -------------------------------------------------------------------------------- 1 | 0.035, 0.002, -1.23, -179.93, -0.23, 0.50 -------------------------------------------------------------------------------- /config/nclt_cams/x_lb3_c0.csv: -------------------------------------------------------------------------------- 1 | 0.000920, -0.000569, 0.062413, -0.028132, 0.196467, 0.248664 2 | -------------------------------------------------------------------------------- /config/nclt_cams/x_lb3_c1.csv: -------------------------------------------------------------------------------- 1 | 0.014543, 0.039337, 0.000398, -138.449751, 89.703877, -66.518051 2 | -------------------------------------------------------------------------------- /config/nclt_cams/x_lb3_c2.csv: -------------------------------------------------------------------------------- 1 | -0.032674, 0.025928, 0.000176, 160.101024, 89.836345, -56.101163 2 | -------------------------------------------------------------------------------- /config/nclt_cams/x_lb3_c3.csv: -------------------------------------------------------------------------------- 1 | -0.034969, -0.022993, 0.000030, 95.603967, 89.724274, -48.640335 2 | -------------------------------------------------------------------------------- /config/nclt_cams/x_lb3_c4.csv: -------------------------------------------------------------------------------- 1 | 0.011238, -0.040367, -0.000393, -160.239278, 89.812338, 127.472911 2 | -------------------------------------------------------------------------------- /config/nclt_cams/x_lb3_c5.csv: -------------------------------------------------------------------------------- 1 | 0.041862, -0.001905, -0.000212, 160.868615, 89.914152, 160.619894 2 | -------------------------------------------------------------------------------- /config/optimize_graph.bash: -------------------------------------------------------------------------------- 1 | ~/thirdparty/tensorflow/bazel-bin/tensorflow/tools/graph_transforms/transform_graph \ 2 | --in_graph='mobilenet_nclt_2018_06_10.pb' \ 3 | --out_graph='optimized_mobilenet_nclt_2019_06_10.pb' \ 4 | --inputs='network/input/Placeholder' \ 5 | --outputs='network/output/ArgMax,network/upscore_8s/upscore8/upscore8/BiasAdd' \ 6 | --transforms=' 7 | strip_unused_nodes 8 | fold_constants(ignore_errors=true) 9 | fold_batch_norms 10 | fold_old_batch_norms 11 | quantize_weights' 12 | -------------------------------------------------------------------------------- /config/optimize_graph_working.bash: -------------------------------------------------------------------------------- 1 | ~/thirdparty/tensorflow/bazel-bin/tensorflow/tools/graph_transforms/transform_graph \ 2 | --in_graph='mobilenet_nclt/optimized_mobilenet_frozen_nclt_bn_480.pb' \ 3 | --out_graph='mobilenet_nclt/optimized_mobilenet_nclt_bn_2019_06_14.pb' \ 4 | --inputs='network/input/Placeholder' \ 5 | --outputs='network/output/ArgMax,network/upscore_8s/upscore8/upscore8/BiasAdd' \ 6 | --transforms=' 7 | fold_constants(ignore_errors=true) 8 | fold_old_batch_norms 9 | strip_unused_nodes 10 | ' 11 | -------------------------------------------------------------------------------- /config/optimized_mobilenet_const_nclt_2019_06_11.pb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/optimized_mobilenet_const_nclt_2019_06_11.pb -------------------------------------------------------------------------------- /config/realsense2lidar.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/realsense2lidar.npy -------------------------------------------------------------------------------- /config/realsense_intrinsic.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/config/realsense_intrinsic.npy -------------------------------------------------------------------------------- /config/remove_is_train_in_graph.py: -------------------------------------------------------------------------------- 1 | import tensorflow as tf 2 | import sys 3 | from tensorflow.core.framework import graph_pb2 4 | import copy 5 | 6 | 7 | INPUT_GRAPH_DEF_FILE = sys.argv[1] 8 | OUTPUT_GRAPH_DEF_FILE = sys.argv[2] 9 | 10 | # load our graph 11 | def load_graph(filename): 12 | graph_def = tf.GraphDef() 13 | with tf.gfile.FastGFile(filename, 'rb') as f: 14 | graph_def.ParseFromString(f.read()) 15 | return graph_def 16 | graph_def = load_graph(INPUT_GRAPH_DEF_FILE) 17 | 18 | target_node_name = sys.argv[3] 19 | c = tf.constant(False, dtype=bool, shape=[], name=target_node_name) 20 | 21 | # Create new graph, and rebuild it from original one 22 | # replacing phase train node def with constant 23 | new_graph_def = graph_pb2.GraphDef() 24 | for node in graph_def.node: 25 | if node.name == target_node_name: 26 | new_graph_def.node.extend([c.op.node_def]) 27 | else: 28 | new_graph_def.node.extend([copy.deepcopy(node)]) 29 | 30 | # save new graph 31 | with tf.gfile.GFile(OUTPUT_GRAPH_DEF_FILE, "wb") as f: 32 | f.write(new_graph_def.SerializeToString()) 33 | -------------------------------------------------------------------------------- /data/2019-01-08-00-59-59.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/data/2019-01-08-00-59-59.bag -------------------------------------------------------------------------------- /data/nclt_04_29/label2color.yaml: -------------------------------------------------------------------------------- 1 | label2color: 2 | 2: "128, 64, 128" # road 3 | 3: [128, 64,128] # sidewalk 4 | 5: [ 192, 192, 192] # building 5 | 10: [192,192,192] # pole 6 | 12: [255,255,0 ] # traffic sign 7 | 6: [107,142, 35] # vegetation 8 | 4: [107,142, 35] # terrain 9 | 0: [ 135, 206, 235 ] # sky 10 | 1: [ 30, 144, 255 ] # water 11 | 8: [220, 20, 60] # person 12 | 7: [ 0, 0,142] # car 13 | 9: [119, 11, 32] # bike 14 | 11 : [123, 104, 238] # stair 15 | -------------------------------------------------------------------------------- /include/SegmentationMapping/PointSegmentedDistribution.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // for the newly defined pointtype 4 | #define PCL_NO_PRECOMPILE 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | 14 | 15 | namespace pcl { 16 | template 17 | struct PointSegmentedDistribution 18 | { 19 | PCL_ADD_POINT4D; 20 | PCL_ADD_RGB; 21 | int label; 22 | float label_distribution[NUM_CLASS]; // templated on any number of classes. TODO 23 | //float label_distribution[14]; 24 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 25 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 26 | 27 | } 28 | 29 | namespace SegmentationMapping { 30 | 31 | template 32 | void PointSeg_to_PointXYZRGB(const pcl::PointCloud> & pc_seg, 33 | typename pcl::PointCloud & pc_rgb) { 34 | pc_rgb.resize(pc_seg.size()); 35 | for (int i = 0; i < pc_rgb.size(); i++) { 36 | auto & p_rgb = pc_rgb[i]; 37 | auto & p_seg = pc_seg[i]; 38 | 39 | p_rgb.x = p_seg.x; 40 | p_rgb.y = p_seg.y; 41 | p_rgb.z = p_seg.z; 42 | p_rgb.r = p_seg.r; 43 | p_rgb.g = p_seg.g; 44 | p_rgb.b = p_seg.b; 45 | 46 | } 47 | pc_rgb.header = pc_seg.header; 48 | } 49 | 50 | 51 | 52 | 53 | 54 | } 55 | -------------------------------------------------------------------------------- /include/SegmentationMapping/tf_inference.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace SegmentationMapping { 16 | class tfInference { 17 | public: 18 | tfInference(const std::string & frozen_tf_graph, 19 | const std::string & input_tensor_name, 20 | const std::string & output_label_tensor_name, 21 | const std::string & output_distribution_tensor_name); 22 | 23 | ~tfInference() { 24 | TF_CloseSession(tf_sess, status); 25 | TF_DeleteSession(tf_sess, status); 26 | TF_DeleteGraph(graph_def); 27 | TF_DeleteStatus(status); 28 | 29 | } 30 | 31 | /* 32 | input: 33 | a cv Mat rgb image 34 | 35 | returns: 36 | cv::Mat the labeled image, 1 channel 37 | Eigen::MatrixXf the 3D eigen matrix, each pixel is a distribution 38 | 39 | */ 40 | void segmentation(const cv::Mat & rgb, int num_class, 41 | cv::Mat & label_output, cv::Mat & distribution_output, 42 | bool & is_successful); 43 | 44 | 45 | 46 | private: 47 | // for tensorflow 48 | TF_Status * status; 49 | TF_Session * tf_sess; 50 | TF_Graph * graph_def; 51 | TF_Operation * input_op; 52 | TF_Operation * output_label_op; 53 | TF_Operation * output_distribution_op; 54 | 55 | 56 | std::vector raw_network_output_shape; 57 | std::vector get_tensor_dims(TF_Output output) ; 58 | }; 59 | 60 | 61 | 62 | 63 | 64 | /******************************************************* 65 | * 66 | * 67 | * Implementations start here 68 | * 69 | * 70 | ****************************************************8**/ 71 | 72 | std::vector tfInference::get_tensor_dims(TF_Output output) { 73 | const TF_DataType type = TF_OperationOutputType(output); 74 | const int num_dims = TF_GraphGetTensorNumDims(graph_def, output, status); 75 | std::vector dims(num_dims); 76 | TF_GraphGetTensorShape(graph_def, output, dims.data(), num_dims, status); 77 | std::cout << " ["; 78 | for (int d = 0; d < num_dims; ++d) { 79 | std::cout << dims[d]; 80 | if (d < num_dims - 1) { 81 | std::cout << ", "; 82 | } 83 | } 84 | std::cout << "]" << std::endl; 85 | return dims; 86 | 87 | } 88 | 89 | 90 | static TF_Buffer* ReadBufferFromFile(const char* file) { 91 | 92 | auto f = std::fopen(file, "rb"); 93 | if (f == nullptr) { 94 | return nullptr; 95 | } 96 | 97 | std::fseek(f, 0, SEEK_END); 98 | auto fsize = ftell(f); 99 | std::fseek(f, 0, SEEK_SET); 100 | 101 | if (fsize < 1) { 102 | std::fclose(f); 103 | return nullptr; 104 | } 105 | 106 | const auto data = std::malloc(fsize); 107 | std::fread(data, fsize, 1, f); 108 | std::fclose(f); 109 | 110 | TF_Buffer* buf = TF_NewBuffer(); 111 | buf->data = data; 112 | buf->length = fsize; 113 | buf->data_deallocator = [](void * data_to_free, long unsigned int){ std::free(data_to_free);}; 114 | std::cout<<"Read graph buffer "<node_size(); i++) { 173 | std::cout<node(i).name()<node(i).name() == input_tensor_name ) { 175 | auto shape = graph_def.node(i).Get(0).attr().at("shape").shape(); 176 | std::cout<<"Find input tensor with shape "<(num_dims), len); 201 | if (tensor == nullptr) { 202 | return nullptr; 203 | } 204 | 205 | void* tensor_data = TF_TensorData(tensor); 206 | if (tensor_data == nullptr) { 207 | TF_DeleteTensor(tensor); 208 | return nullptr; 209 | } 210 | 211 | if (data != nullptr) { 212 | std::memcpy(tensor_data, data, std::min(len, TF_TensorByteSize(tensor))); 213 | } 214 | 215 | return tensor; 216 | } 217 | 218 | 219 | void tfInference::segmentation(const cv::Mat & rgb, int num_class, 220 | cv::Mat & label_output, cv::Mat & distribution_output, 221 | bool & is_successful) { 222 | 223 | cv::Mat input_img_data = rgb; 224 | rgb.convertTo(input_img_data, CV_32FC3); 225 | 226 | TF_Tensor* input_img_tensor[1]; 227 | std::vector input_shape = {1, rgb.rows, rgb.cols, 3}; 228 | input_img_tensor[0] = data_to_tensor(TF_FLOAT, 229 | input_shape.data(), input_shape.size(), 230 | input_img_data.data, 231 | input_img_data.elemSize() * input_img_data.total()); 232 | if (input_img_tensor[0] == nullptr) { 233 | std::cout<<"Error: input tensor creation fails\n"; 234 | is_successful = false; 235 | return ; 236 | } 237 | 238 | // setting up input/output 239 | TF_Output inputs[1] = {{input_op, 0}}; 240 | TF_Output outputs[2] = { {output_label_op , 0}, 241 | {output_distribution_op, 0} }; 242 | 243 | //std::cout<<"Input data type is "<(TF_TensorData(output_tensors[0])); 269 | float * distribution_flat = static_cast(TF_TensorData(output_tensors[1])); 270 | 271 | // convert to output cv mat and Eigen Mat 272 | cv::Mat out_label_img(input_shape[1], input_shape[2], CV_32SC1, label_img_flat); 273 | out_label_img.convertTo(label_output, CV_8UC1); 274 | cv::Mat distribution_img(input_shape[1], input_shape[2], CV_32FC(num_class), distribution_flat); 275 | distribution_output = distribution_img.clone(); 276 | 277 | TF_DeleteTensor(input_img_tensor[0]); 278 | TF_DeleteTensor(output_tensors[0]); 279 | TF_DeleteTensor(output_tensors[1]); 280 | 281 | is_successful = true; 282 | return ; 283 | 284 | 285 | } 286 | 287 | } 288 | 289 | 290 | -------------------------------------------------------------------------------- /launch/belltower.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | ?> 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /launch/cassie_lidar_py.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | [0.0388, -0.9992, -0.0075, 0.0738, 0.1324,0.0126,-0.9911, 0.0279, 0.9904,0.0374,0.1328, -0.1829] 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | [0, 8, 13] 54 | [2,3] 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /launch/cassie_lidar_semanticbki.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | [0.0388, -0.9992, -0.0075, 0.0738, 0.1324,0.0126,-0.9911, 0.0279, 0.9904,0.0374,0.1328, -0.1829] 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 50 | 51 | 52 | 53 | 54 | 55 | [0, 8, 13] 56 | [2,3] 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | ?> 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /launch/cassie_stereo_cpp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | ?> 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /launch/cassie_stereo_map_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ?> 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | ?> 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | [0, 8, 13] 52 | [2,3] 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /launch/cassie_stereo_py.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ?> 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | > 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | [0, 8, 13] 53 | [2,3] 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /launch/cassie_stereo_py_multi_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ?> 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | > 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | > 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | [0, 8, 13] 87 | [2,3] 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /launch/cassie_stereo_py_two_camera.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ?> 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | > 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | > 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | ?> 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | [0, 8, 13] 90 | [2,3] 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /launch/nclt_classification.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | ?> 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /launch/nclt_distribution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | ?> 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /launch/nclt_distribution_deeplab.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /launch/nclt_label_octomap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | ?> 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /launch/nclt_label_voxblox_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | --> 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /launch/nclt_labeled_pc_painting_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | ?> 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /launch/offline_cassie_stereo_py.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | ?> 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /launch/realsense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | ?> 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /launch/rvsm_nclt_semantic_octmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | --> ?> 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /launch/segmentation_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 29 | [0.0388, -0.9992, -0.0075, 0.0738, 0.1324,0.0126,-0.9911, 0.0279, 0.9904,0.0374,0.1328, -0.1829] 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | ?> 49 | 51 | 52 | 53 | 54 | 55 | 56 | [0, 8, 13] 57 | [2,3] 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | ?> 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /launch/traversability_segmentation_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /msg/ImageLabelDistribution.msg: -------------------------------------------------------------------------------- 1 | 2 | std_msgs/Header header 3 | std_msgs/Float32MultiArray distribution -------------------------------------------------------------------------------- /msg/ImageLabelDistribution.msg~: -------------------------------------------------------------------------------- 1 | 2 | std_msgs/Header header 3 | std_msgsFloat32MultiArray distribution -------------------------------------------------------------------------------- /octomap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/octomap.png -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | SegmentationMapping 4 | 0.0.0 5 | The SegmentationMapping package 6 | 7 | 8 | 9 | 10 | Ray Zhang 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | roscpp 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | message_filters 53 | message_generation 54 | rospy 55 | sensor_msgs 56 | geometry_msgs 57 | std_msgs 58 | nav_msgs 59 | tf2_ros 60 | pcl_ros 61 | cv_bridge 62 | octomap_ros 63 | 64 | 65 | message_filters 66 | rospy 67 | sensor_msgs 68 | cv_bridge 69 | message_filters 70 | message_runtime 71 | std_msgs 72 | rospy 73 | sensor_msgs 74 | geometry_msgs 75 | cv_bridge 76 | 77 | nav_msgs 78 | tf2_ros 79 | pcl_ros 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /scripts/NeuralNetConfigs.py: -------------------------------------------------------------------------------- 1 | from collections import namedtuple 2 | NeuralNetConfigs = namedtuple("NeuralNetConfigs", "path \ 3 | num_classes \ 4 | image_input_tensor \ 5 | is_train_input_tensor \ 6 | input_width \ 7 | input_height \ 8 | label_output_tensor \ 9 | distribution_output_tensor \ 10 | ") 11 | -------------------------------------------------------------------------------- /scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/scripts/__init__.py -------------------------------------------------------------------------------- /scripts/distort.py: -------------------------------------------------------------------------------- 1 | """ 2 | Demonstrating how to undistort images. 3 | 4 | Reads in the given calibration file, parses it, and uses it to undistort the given 5 | image. Then display both the original and undistorted images. 6 | 7 | To use: 8 | 9 | python undistort.py image calibration_file 10 | """ 11 | 12 | import numpy as np 13 | import cv2 14 | import matplotlib.pyplot as plt 15 | import argparse 16 | import re, pdb 17 | from scipy.interpolate import RectBivariateSpline 18 | 19 | class DistortMap(object): 20 | 21 | def __init__(self, undist2distorted_map, scale=1.0, fmask=None): 22 | # undist2distorted_map example: D2U_Cam1_1616X1232.txt 23 | # read in distort 24 | 25 | with open(undist2distorted_map, 'r') as f: 26 | #chunks = f.readline().rstrip().split(' ') 27 | header = f.readline().rstrip() 28 | 29 | # chunks[0]: width chunks[1]: height 30 | chunks = re.sub(r'[^0-9,]', '', header).split(',') 31 | self.mapu = np.zeros((int(chunks[1]),int(chunks[0])), 32 | dtype=np.float32) 33 | self.mapv = np.zeros((int(chunks[1]),int(chunks[0])), 34 | dtype=np.float32) 35 | 36 | # undistorted lidar -> distorted camera index 37 | # [v_projected_lidar, u_projected_lidar] --- > (v_cam, u_cam) 38 | for line in f.readlines(): 39 | chunks = line.rstrip().split(' ') 40 | self.mapu[int(chunks[0]),int(chunks[1])] = float(chunks[3]) 41 | self.mapv[int(chunks[0]),int(chunks[1])] = float(chunks[2]) 42 | 43 | 44 | def distort(self, lidar_projected_2d): 45 | ''' 46 | lidar_projected_2d: 3*N np array. The last row contains only 1s 47 | ''' 48 | distorted = []#np.ones(lidar_projected_2d.shape) 49 | remaining_ind = [] 50 | counter = 0 51 | for col in range(lidar_projected_2d.shape[1]): 52 | u_f = lidar_projected_2d[0, col] 53 | v_f = lidar_projected_2d[1, col] 54 | if u_f < 0 or u_f >= 1600 or v_f < 0 or v_f >= 1200: 55 | continue 56 | remaining_ind.append(col) 57 | u_l = int(lidar_projected_2d[0, col]) 58 | u_u = int(lidar_projected_2d[0, col]) + 1 59 | v_l = int(lidar_projected_2d[1, col]) 60 | v_u = int(lidar_projected_2d[1, col]) + 1 61 | 62 | # ex: lu: v is l, u is u 63 | # the (v, u) at four grid corners 64 | u_ll = self.mapu[v_l, u_l] 65 | v_ll = self.mapv[v_l, u_l] 66 | 67 | u_lu = self.mapu[v_l, u_u] 68 | v_lu = self.mapv[v_l, u_u] 69 | 70 | u_ul = self.mapu[v_u, u_l] 71 | v_ul = self.mapv[v_u, u_l] 72 | 73 | u_uu = self.mapu[v_u, u_u] 74 | v_uu = self.mapv[v_u, u_u] 75 | 76 | dist = np.ones((1,3)) 77 | sp_u = RectBivariateSpline(np.array([v_l, v_u]), \ 78 | np.array([u_l, u_u]), \ 79 | np.array([[u_ll, u_lu],[u_ul, u_uu]]), kx=1, ky=1) 80 | sp_v = RectBivariateSpline(np.array([v_l, v_u]), \ 81 | np.array([u_l, u_u]), \ 82 | np.array([[v_ll, v_lu],[v_ul, v_uu]]), kx=1, ky=1) 83 | 84 | dist[0 ,0] = sp_u.ev(v_f, u_f) 85 | dist[0, 1] = sp_v.ev(v_f, u_f) 86 | 87 | distorted.append(dist) 88 | distorted = np.squeeze(np.array(distorted)).transpose() 89 | return distorted, remaining_ind 90 | 91 | 92 | 93 | def main(): 94 | parser = argparse.ArgumentParser(description="Undistort images") 95 | parser.add_argument('image', metavar='img', type=str, help='image to undistort') 96 | parser.add_argument('map', metavar='map', type=str, help='undistortion map') 97 | 98 | args = parser.parse_args() 99 | 100 | distort = DistortMap(args.map) 101 | print 'Loaded camera calibration' 102 | 103 | 104 | 105 | #cv2.namedWindow('Undistorted Image', cv2.WINDOW_NORMAL) 106 | #cv2.imshow('Undistorted Image', im_undistorted) 107 | cv2.imwrite("undist.png", im_undistorted) 108 | #cv2.waitKey(0) 109 | #cv2.destroyAllWindows() 110 | 111 | if __name__ == "__main__": 112 | main() 113 | -------------------------------------------------------------------------------- /scripts/gen_nclt_gt_pc.bash: -------------------------------------------------------------------------------- 1 | #python get_projected_gt.py --input_pointcloud ~/rvm_nclt.txt --rgb_img ~/Documents/nclt_0429_rvm/1335704515132779_rgb.tiff --gt_img ~/Documents/nclt_0429_rvm/1335704515132779_bw.png --cam2lidar_projection ~/perl_code/workspace/src/segmentation_projection/config/nclt_cams/cam2lidar_4.npy --cam_intrinsic ~/perl_code/workspace/src/segmentation_projection/config/nclt_cams/nclt_intrinsic_4.npy --distortion_map ~/perl_code/workspace/src/segmentation_projection/config/nclt_cams/U2D_Cam4_1616X1232.txt --output_pointcloud out.txt 2 | 3 | nclt_eval_root=$1 # root folder containing configs, 4 | seg_lidar_pts=$2 5 | output=$3 6 | 7 | python get_projected_gt.py --segmented_lidar_folder $2 --gt_folder $1/gt_may23/ --config_folder $1/config/ --output_folder $3/ --rgb_img_folder $1/rgb_may23 8 | -------------------------------------------------------------------------------- /scripts/get_projected_gt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys, os, pdb 4 | import numpy as np 5 | import cv2 6 | import argparse 7 | from distort import DistortMap 8 | from helper import get_cropped_uv_rotated, is_out_of_bound, is_out_of_bound_rotated 9 | from label2color import background, label_to_color 10 | from pcl import pcl_visualization 11 | import pcl 12 | 13 | def gt_viewer(lidar_final, labels_final): 14 | lidar = np.transpose(lidar_final).astype(np.float32) 15 | cloud = pcl.PointCloud_PointXYZRGB() 16 | for i in range(len(labels_final) ): 17 | # set Point Plane 18 | if labels_final[i] in label_to_color: 19 | color = label_to_color[labels_final[i]] 20 | else: 21 | color = label_to_color[0] 22 | 23 | lidar[i][3] = color[0] << 16 | color[1] << 8 | color[2] 24 | cloud.from_array(lidar) 25 | visual = pcl_visualization.CloudViewing() 26 | visual.ShowColorCloud(cloud) 27 | 28 | v = True 29 | while v: 30 | v=not(visual.WasStopped()) 31 | 32 | def gt_projection(lidar, lidar_distribution, 33 | rgb_img, gt_img, cam2lidar, intrinsic, distort_map, output_file): 34 | 35 | num_classes = lidar_distribution.shape[0] 36 | 37 | # project lidar points into camera coordinates 38 | T_c2l = cam2lidar[:3, :] 39 | 40 | lidar_in_cam = np.matmul(intrinsic, T_c2l ) 41 | projected_lidar_2d = np.matmul( lidar_in_cam, lidar) 42 | projected_lidar_2d[0, :] = projected_lidar_2d[0, :] / projected_lidar_2d[2, :] 43 | projected_lidar_2d[1, :] = projected_lidar_2d[1, :] / projected_lidar_2d[2, :] 44 | 45 | # filter the points on the image 46 | idx_infront = projected_lidar_2d[2, :]>0 47 | print("idx_front sum is " + str(np.sum(idx_infront))) 48 | filtered_distribution = lidar_distribution[:, idx_infront] 49 | points_on_img = projected_lidar_2d[:, idx_infront] 50 | lidar_on_img = lidar[:, idx_infront] 51 | 52 | 53 | # distort the lidar points based on the distortion map file 54 | projected_lidar_2d, remaining_ind = distort_map.distort(points_on_img) 55 | lidar_on_img = lidar_on_img[:, remaining_ind] 56 | filtered_distribution = filtered_distribution[:, remaining_ind] 57 | print(projected_lidar_2d.shape, lidar_on_img.shape, filtered_distribution.shape) 58 | 59 | projected_points = [] 60 | projected_index = [] # just for visualization 61 | labels = [] 62 | #original_rgb = [] 63 | class_distribution = [] 64 | gt_label_file = open(output_file, 'w') 65 | for col in range(projected_lidar_2d.shape[1]): 66 | u, v, _ = projected_lidar_2d[:, col] 67 | u ,v = get_cropped_uv_rotated(u, v, rgb_img.shape[1] * 1.0 / 1200 ) 68 | if is_out_of_bound(u, v, rgb_img.shape[1], rgb_img.shape[0]): 69 | continue 70 | projected_points.append(lidar_on_img[:, col]) 71 | 72 | if gt_img[v, u] < num_classes : 73 | label = gt_img[int(v), int(u)] 74 | else: 75 | label = 0 76 | labels.append(label) 77 | 78 | # write gt results to the file 79 | gt_label_file.write("{} {} {} ".format(lidar_on_img[0, col], lidar_on_img[1, col], lidar_on_img[2, col])) 80 | for i in range(num_classes): 81 | gt_label_file.write("{} ".format(filtered_distribution[i, col])) 82 | gt_label_file.write(str(label)) 83 | gt_label_file.write("\n") 84 | 85 | #original_rgb.append(rgb_img[int(v), int(u), :]) 86 | projected_index.append(col) 87 | gt_label_file.close() 88 | print("Finish writing to {}, # of final points is {}".format(output_file, len(projected_index))) 89 | 90 | lidars_left = lidar_on_img[:, projected_index] 91 | distribution_left = filtered_distribution[:, projected_index] 92 | ####################################################################### 93 | # for debug use: visualize the projection on the original rgb image 94 | ####################################################################### 95 | for j in range(len(projected_index)): 96 | col = projected_index[j] 97 | u = int(round(projected_lidar_2d[0, col] , 0)) 98 | v = int(round(projected_lidar_2d[1, col] , 0)) 99 | u ,v = get_cropped_uv_rotated(u, v, rgb_img.shape[1] * 1.0 / 1200 ) 100 | if labels[j] in label_to_color: 101 | color = label_to_color[labels[j]] 102 | else: 103 | color = label_to_color[0] 104 | 105 | cv2.circle(rgb_img, (u, v),2, (color[2], color[1], color[0] ) ) 106 | #cv2.imshow("gt projection", rgb_img) 107 | #cv2.waitKey(500) 108 | ######################################################################## 109 | return lidars_left, labels, distribution_left 110 | 111 | 112 | def read_input_pc_with_distribution(file_name): 113 | lidar = [] 114 | lidar_distribution = [] 115 | with open(file_name) as f: 116 | for line in f: 117 | point = line.split() 118 | point_np = np.array([float(item) for item in point]) 119 | 120 | lidar.append(np.array([point_np[0],point_np[1],point_np[2], 1])) 121 | 122 | lidar_distribution.append(point_np[3:]) 123 | 124 | lidar = np.transpose(np.array(lidar)) 125 | lidar_distribution = np.transpose(np.array(lidar_distribution)) 126 | print("Finish reading data... lidar shape is {}, lidar_distribution shape is {}".format(lidar.shape, lidar_distribution.shape)) 127 | return lidar, lidar_distribution 128 | 129 | 130 | def batch_gt_projection_nclt(query_lidar_folder, 131 | gt_folder, 132 | config_folder, 133 | rgb_folder, 134 | output_folder): 135 | cam2lidar_mats = [] 136 | distortions = [] 137 | intrinsic = [] 138 | for i in range(5): 139 | print("extrinsic {}/cam2lidar_{}.npy".format(config_folder, i+1)) 140 | extrinsic = np.load("{}/cam2lidar_{}.npy".format(config_folder, i+1)) 141 | cam2lidar_mats.append(extrinsic) 142 | print(cam2lidar_mats[-1]) 143 | print("intrinsic") 144 | intrinsic.append(np.load("{}/nclt_intrinsic_{}.npy".format(config_folder, i+1))) 145 | 146 | print(intrinsic[-1]) 147 | print("read camera distortion") 148 | distortions.append(DistortMap("{}/U2D_Cam{}_1616X1232.txt".format(config_folder, i+1))) 149 | 150 | ind = 0 151 | for query_file in sorted(os.listdir(gt_folder)): 152 | 153 | camera_prefix = query_file[:4] 154 | camera_ind = int(camera_prefix[-1]) 155 | 156 | ##if camera_ind == 1 or camera_ind == 4: 157 | ## continue 158 | 159 | ind += 1 160 | camera_i_list = camera_ind - 1 161 | time = query_file[5:-4] 162 | print("Processing "+query_file+", time is "+time, " frame counter is "+str(ind)) 163 | if os.path.exists(query_lidar_folder + "/" + time + ".txt" ) == False: 164 | continue 165 | 166 | 167 | lidar, lidar_distribution = read_input_pc_with_distribution( query_lidar_folder + "/" + time + ".txt" ) 168 | 169 | gt = cv2.imread( gt_folder + "/" + query_file ) 170 | gt = gt[:, :, 0] 171 | print(lidar.shape, lidar_distribution.shape, gt.shape) 172 | #cv2.imshow("gt", gt) 173 | #cv2.waitKey(500) 174 | rgb_img = cv2.imread( rgb_folder + "/" + query_file[:-4] + ".png") 175 | 176 | lidars_left, labels, distribution_left = gt_projection(lidar, 177 | lidar_distribution, 178 | rgb_img, 179 | gt, 180 | cam2lidar_mats[camera_i_list], 181 | intrinsic[camera_i_list], 182 | distortions[camera_i_list], 183 | output_folder+"/"+camera_prefix + "_"+ time+".txt" ) 184 | 185 | #gt_viewer(lidars_left, labels) 186 | 187 | if __name__ == "__main__": 188 | parser = argparse.ArgumentParser(description='Process some integers.') 189 | parser.add_argument('--segmented_lidar_folder', type=str, nargs=1) 190 | parser.add_argument('--gt_folder', type=str, nargs=1) 191 | parser.add_argument('--config_folder', type=str, nargs=1) 192 | parser.add_argument('--output_folder', type=str, nargs=1) 193 | parser.add_argument('--rgb_img_folder', type=str, nargs=1) 194 | #parser.add_argument('--rgb_img_shape', type=int, nargs=2) 195 | 196 | args = parser.parse_args() 197 | 198 | 199 | batch_gt_projection_nclt(args.segmented_lidar_folder[0], 200 | #args.rgb_img_shape, 201 | args.gt_folder[0], 202 | args.config_folder[0], 203 | args.rgb_img_folder[0], 204 | args.output_folder[0]) 205 | -------------------------------------------------------------------------------- /scripts/gt_pose2bag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import rospy, sys, os 4 | import rosbag 5 | import geometry_msgs, tf 6 | import tf.msg 7 | import geometry_msgs.msg 8 | import pdb 9 | 10 | def gen_tf_msg_from_file(fname): 11 | msgs = [] 12 | 13 | with open(fname, 'r') as f: 14 | for i, line in enumerate(f): 15 | words = line.split(',') 16 | if i == 0: 17 | origin = [] 18 | origin.append( float(words[1])) 19 | origin.append ( float(words[2])) 20 | origin.append( float(words[3])) 21 | 22 | 23 | time = int(words[0]) 24 | secs = time // 1000000 25 | nsecs = (time - secs * 1000000) * 1000 26 | x = float(words[1]) 27 | y = float(words[2]) 28 | z = float(words[3]) 29 | R = float(words[4]) 30 | P = float(words[5]) 31 | Y = float(words[6]) 32 | # convert to local coordinate frame, located at the origin 33 | x = x - origin[0] 34 | y = y - origin[1] 35 | z = z - origin[2] 36 | # create TF msg 37 | tf_msg = tf.msg.tfMessage() 38 | geo_msg = geometry_msgs.msg.TransformStamped() 39 | geo_msg.header.stamp = rospy.Time(secs,nsecs) 40 | geo_msg.header.seq = i 41 | geo_msg.header.frame_id = "map" 42 | geo_msg.child_frame_id = "body" 43 | geo_msg.transform.translation.x = x 44 | geo_msg.transform.translation.y = y 45 | geo_msg.transform.translation.z = z 46 | angles = tf.transformations.quaternion_from_euler(R,P,Y) 47 | geo_msg.transform.rotation.x = angles[0] 48 | geo_msg.transform.rotation.y = angles[1] 49 | geo_msg.transform.rotation.z = angles[2] 50 | geo_msg.transform.rotation.w = angles[3] 51 | tf_msg.transforms.append(geo_msg) 52 | msgs.append(tf_msg) 53 | return msgs 54 | 55 | def write_bagfile(data_set_name, tf_msgs ): 56 | ''' Write all ROS msgs, into the bagfile. 57 | ''' 58 | bag = rosbag.Bag(data_set_name, 'w') 59 | # write the tf msgs into the bagfile 60 | for msg in tf_msgs: 61 | print(msg.transforms[0].header.stamp) 62 | bag.write('/tf', msg, msg.transforms[0].header.stamp ) 63 | bag.close() 64 | print "Finished Bagfile IO" 65 | 66 | 67 | if __name__ == "__main__": 68 | gt_pose_fname = sys.argv[1] 69 | output_bag = sys.argv[2] 70 | print("Gen tf msgs") 71 | msgs = gen_tf_msg_from_file(gt_pose_fname) 72 | print("Write bag file") 73 | write_bagfile(output_bag, msgs) 74 | -------------------------------------------------------------------------------- /scripts/helper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | # source: https://github.com/UMich-BipedLab/segmentation_projection 5 | # maintainer: Ray Zhang rzh@umich.edu 6 | 7 | 8 | 9 | import numpy as np 10 | 11 | import pcl # python-pcl 12 | import pcl.pcl_visualization 13 | from label2color import label_to_color 14 | 15 | def publish_pcl_pc2_label(lidar, labels): 16 | ''' 17 | for debugging: visualizing the constructed labeled pointcloud before 18 | we publish them 19 | lidar: a python array 20 | ''' 21 | cloud = pcl.PointCloud_PointXYZRGB() 22 | points = np.zeros((len(lidar), 4), dtype=np.float32) 23 | for i in range(len(lidar)): 24 | # set Point Plane 25 | points[i][0] = lidar[i][0] 26 | points[i][1] = lidar[i][1] 27 | points[i][2] = lidar[i][2] 28 | color = label_to_color[labels[i]] 29 | points[i][3] = int(color[0]) << 16 | int(color[1]) << 8 | int(color[2]) 30 | cloud.from_array(points) 31 | pcl_vis = pcl.pcl_visualization.CloudViewing() 32 | pcl_vis.ShowColorCloud(cloud) 33 | v = True 34 | while v: 35 | v=not(pcl_vis.WasStopped()) 36 | 37 | def publish_pcl_pc2(lidar): 38 | ''' 39 | for debugging: visualizing the constructed labeled pointcloud before 40 | we publish them 41 | lidar: a 3xN numpy array 42 | ''' 43 | print("visualizing uncolored pcl") 44 | cloud = pcl.PointCloud_PointXYZRGB() 45 | points = np.zeros((lidar.shape[1], 4), dtype=np.float32) 46 | for i in range(lidar.shape[1] ): 47 | # set Point Plane 48 | points[i][0] = lidar[0, i] 49 | points[i][1] = lidar[1, i] 50 | points[i][2] = lidar[2, i] 51 | points[i][3] = 0 << 16 | 255 << 8 | 0 52 | cloud.from_array(points) 53 | pcl_vis = pcl.pcl_visualization.CloudViewing() 54 | pcl_vis.ShowColorCloud(cloud) 55 | v = True 56 | while v: 57 | v=not(pcl_vis.WasStopped()) 58 | 59 | def get_cropped_uv_rotated( u, v, small_over_large_scale): 60 | u = u - 500 61 | v = 1200 - 1 - v 62 | u = int(round(u * small_over_large_scale, 0)) 63 | v = int(round(v * small_over_large_scale, 0)) 64 | u_new = v 65 | v_new = u 66 | return u_new,v_new 67 | 68 | def is_out_of_bound(u, v, max_u, max_v): 69 | #if u < 500 or u >= 1100 or \ 70 | # v < 0 or v >= 1200: 71 | if u >= max_u or u < 0 or \ 72 | v >= max_v or v < 0: 73 | return True 74 | else: 75 | return False 76 | 77 | def is_out_of_bound_rotated(u, v): 78 | if u < 0 or u > 1200 or \ 79 | v< 500 or v > 1100 : 80 | return True 81 | else: 82 | return False 83 | 84 | 85 | def softmax(dist_arr): 86 | exps = np.exp(dist_arr) 87 | return exps/np.sum(exps) 88 | 89 | def softmax_img(dist_arr): 90 | exps = np.exp(dist_arr) 91 | return exps/np.sum(exps) 92 | -------------------------------------------------------------------------------- /scripts/label2color.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # source: https://github.com/UMich-BipedLab/segmentation_projection 4 | # maintainer: Ray Zhang rzh@umich.edu 5 | 6 | 7 | background = 255 8 | 9 | # RGB 10 | label_to_color = { 11 | 2: (250,250,250), # road 12 | #3: (128, 64,128), # sidewalk 13 | 3: (250, 250, 250), # sidewalk 14 | 5: (250,128,0), # building 15 | 10: (192,192,192), # pole 16 | 12: (250,250,0 ), # traffic sign 17 | 6: (107,142, 35), # vegetation 18 | 4: (128,128, 0 ), # terrain 19 | 13: ( 135, 206, 235 ), # sky 20 | 1: ( 30, 144, 250 ), # water 21 | 8 :(220, 20, 60), # person 22 | 7: ( 0, 0,142), # car 23 | 9 : (119, 11, 32),# bike 24 | 11 : (123, 104, 238), # stair 25 | 26 | 0: (0 ,0, 0), # background 27 | background: (0,0,0) # background 28 | } 29 | 30 | label_to_color = { 31 | 2: (245,190,133), # road 32 | #3: (128, 64,128), # sidewalk 33 | 3: (234, 190, 133), # sidewalk 34 | 5: (93,43,77), # building 35 | 10: (122,21,14), # pole 36 | 12: (0,0,0 ), # traffic sign 37 | 6: (211,175, 141), # vegetation 38 | 4: (26,14, 24 ), # terrain 39 | 13: ( 140, 60, 78 ), # sky 40 | 1: ( 0, 0, 0 ), # water 41 | 8 :(56, 7, 8), # person 42 | 7: ( 0, 0,0), # car 43 | 9 : (0, 0, 0),# bike 44 | 11 : (0, 0, 0), # stair 45 | 46 | 0: (0 ,0, 0), # background 47 | background: (0,0,0) # background 48 | } 49 | -------------------------------------------------------------------------------- /scripts/label2traversability.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # source: https://github.com/UMich-BipedLab/segmentation_projection 4 | # maintainer: Lu Gan ganlu@umich.edu 5 | 6 | label_to_traversability = { 7 | 1: 0, # water 8 | 2: 1, # road 9 | 3: 1, # sidewalk 10 | 4: 1, # terrain 11 | 5: 0, # building 12 | 6: 0, # vegetation 13 | 7: 0, # car 14 | 8: 0, # person 15 | 9: 0, # bike 16 | 10: 0, # pole 17 | 11: 1, # stair 18 | 12: 0, # traffic sign 19 | 13: 0, # sky 20 | 0: 0 # background 21 | } 22 | -------------------------------------------------------------------------------- /scripts/project_vel_to_cam.py: -------------------------------------------------------------------------------- 1 | # !/usr/bin/python 2 | # 3 | # source: http://robots.engin.umich.edu/nclt/ 4 | # 5 | # Demonstrates how to project velodyne points to camera imagery. Requires a binary 6 | # velodyne sync file, undistorted image, and assumes that the calibration files are 7 | # in the directory. 8 | # 9 | # To use: 10 | # 11 | # python project_vel_to_cam.py vel img cam_num 12 | # 13 | # vel: The velodyne binary file (timestamp.bin) 14 | # img: The undistorted image (timestamp.tiff) 15 | # cam_num: The index (0 through 5) of the camera 16 | # 17 | 18 | import sys 19 | import struct 20 | import matplotlib.pyplot as plt 21 | import matplotlib.image as mpimg 22 | import numpy as np 23 | 24 | #from undistort import * 25 | 26 | def convert(x_s, y_s, z_s): 27 | 28 | scaling = 0.005 # 5 mm 29 | offset = -100.0 30 | 31 | x = x_s * scaling + offset 32 | y = y_s * scaling + offset 33 | z = z_s * scaling + offset 34 | 35 | return x, y, z 36 | 37 | def load_vel_hits(filename): 38 | 39 | f_bin = open(filename, "r") 40 | 41 | hits = [] 42 | 43 | while True: 44 | 45 | x_str = f_bin.read(2) 46 | if x_str == '': # eof 47 | break 48 | 49 | x = struct.unpack('0 153 | x_im = x_im[idx_infront] 154 | y_im = y_im[idx_infront] 155 | z_im = z_im[idx_infront] 156 | 157 | plt.figure(1) 158 | plt.imshow(image) 159 | plt.hold(True) 160 | plt.scatter(x_im, y_im, c=z_im, s=5, linewidths=0) 161 | plt.xlim(0, 1616) 162 | plt.ylim(0, 1232) 163 | plt.show() 164 | 165 | return 0 166 | 167 | if __name__ == '__main__': 168 | sys.exit(main(sys.argv)) 169 | -------------------------------------------------------------------------------- /scripts/projection.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | # source: https://github.com/UMich-BipedLab/segmentation_projection 5 | # maintainer: Ray Zhang rzh@umich.edu 6 | 7 | 8 | 9 | import sys,os, pdb 10 | import numpy as np 11 | import tensorflow as tf 12 | import cv2 13 | from collections import namedtuple 14 | import rospy, pdb 15 | 16 | from label2color import background, label_to_color 17 | from helper import get_cropped_uv_rotated, is_out_of_bound, is_out_of_bound_rotated, softmax, publish_pcl_pc2, publish_pcl_pc2_label 18 | 19 | from NeuralNetConfigs import NeuralNetConfigs 20 | 21 | class LidarSeg: 22 | def __init__(self, net_configs): 23 | 24 | # read the network 25 | with tf.gfile.GFile(net_configs.path, 'rb') as f: 26 | graph_def = tf.GraphDef() 27 | graph_def.ParseFromString(f.read()) 28 | 29 | # tf configs 30 | tf_config = tf.ConfigProto() 31 | tf_config.gpu_options.allow_growth = True 32 | self.sess = tf.Session(config=tf_config) 33 | 34 | # the input and output of the neural net 35 | self.y, = tf.import_graph_def(graph_def, return_elements=[net_configs.label_output_tensor]) 36 | self.G = tf.get_default_graph() 37 | self.x = self.G.get_tensor_by_name(net_configs.image_input_tensor) 38 | if net_configs.is_train_input_tensor is not None: 39 | self.is_train_input_tensor = self.G.get_tensor_by_name(net_configs.is_train_input_tensor) 40 | else: 41 | self.is_train_input_tensor = None 42 | if net_configs.distribution_output_tensor is not None: 43 | self.distribution_tensor = self.G.get_tensor_by_name(net_configs.distribution_output_tensor) 44 | else: 45 | self.distribution_tensor = None 46 | self.num_output_class = net_configs.num_classes 47 | pdb.set_trace() 48 | # initialization 49 | tf.global_variables_initializer().run(session=self.sess) 50 | 51 | self.intrinsic = [] 52 | self.cam2lidar = [] 53 | self.distort_map = [] 54 | self.counter = 0 55 | 56 | def add_cam(self,intrinsic_mat, cam2lidar, distort): 57 | """ 58 | param: intrinsic_mat: 3x4 59 | extrinsic_mat: 4x4, 60 | distort: pixelwise map for distortion 61 | """ 62 | self.intrinsic.append(intrinsic_mat) 63 | self.cam2lidar.append( cam2lidar ) 64 | self.distort_map.append(distort) 65 | 66 | 67 | 68 | def project_lidar_to_seg(self, lidar, rgb_img, camera_ind, original_img): 69 | """ 70 | assume the lidar points can all be projected to this img 71 | assume the rgb img shape meets the requirement of the neural net input 72 | 73 | lidar points: 3xN 74 | """ 75 | if self.distribution_tensor is not None: 76 | if self.is_train_input_tensor is not None: 77 | distribution = self.sess.run([self.distribution_tensor], 78 | feed_dict={self.x: np.expand_dims(cv2.cvtColor(rgb_img, cv2.COLOR_BGR2RGB),axis=0), 79 | self.is_train_input_tensor: False})[0] 80 | else: 81 | distribution = self.sess.run([self.distribution_tensor], 82 | feed_dict={self.x: np.expand_dims(cv2.cvtColor(rgb_img, cv2.COLOR_BGR2RGB),axis=0)})[0] 83 | 84 | distribution = distribution[0, :, :, :] 85 | else: 86 | if self.is_train_input_tensor is not None: 87 | out = self.sess.run(self.y, 88 | feed_dict={self.x: np.expand_dims(cv2.cvtColor(rgb_img, cv2.COLOR_BGR2RGB),axis=0), 89 | self.is_train_input_tensor: False}) 90 | else: 91 | out = self.sess.run(self.y, 92 | feed_dict={self.x: np.expand_dims(cv2.cvtColor(rgb_img, cv2.COLOR_BGR2RGB),axis=0)}) 93 | 94 | distribution = [] 95 | out = out [0,:,:] 96 | 97 | 98 | # project lidar points into camera coordinates 99 | T_c2l = self.cam2lidar[camera_ind][:3, :] 100 | lidar_in_cam = np.matmul(self.intrinsic[camera_ind], T_c2l ) 101 | projected_lidar_2d = np.matmul( lidar_in_cam, lidar) 102 | projected_lidar_2d[0, :] = projected_lidar_2d[0, :] / projected_lidar_2d[2, :] 103 | projected_lidar_2d[1, :] = projected_lidar_2d[1, :] / projected_lidar_2d[2, :] 104 | #print("total number of lidar : "+str(projected_lidar_2d.shape)) 105 | idx_infront = projected_lidar_2d[2, :]>0 106 | print("idx_front sum is " + str(np.sum(idx_infront))) 107 | x_im = projected_lidar_2d[0, :][idx_infront] 108 | y_im = projected_lidar_2d[1, :][idx_infront] 109 | z_im = projected_lidar_2d[2, :][idx_infront] 110 | points_on_img = np.zeros((3, x_im.size)) 111 | points_on_img[0, :] = x_im 112 | points_on_img[1, :] = y_im 113 | points_on_img[2, :] = z_im 114 | lidar_on_img = np.zeros((3, x_im.size)) 115 | lidar_on_img[0, :] = lidar[0, :][idx_infront] 116 | lidar_on_img[1, :] = lidar[1, :][idx_infront] 117 | lidar_on_img[2, :] = lidar[2, :][idx_infront] 118 | 119 | # distort the lidar points based on the distortion map file 120 | projected_lidar_2d, remaining_ind = self.distort_map[camera_ind].distort(points_on_img) 121 | lidar_on_img = lidar_on_img[:, remaining_ind] 122 | print(projected_lidar_2d.shape, lidar_on_img.shape) 123 | ####################################################################### 124 | # for debug use: visualize the projection on the original rgb image 125 | ####################################################################### 126 | #for col in range(projected_lidar_2d.shape[1]): 127 | # u = int(round(projected_lidar_2d[0, col] , 0)) 128 | # v = int(round(projected_lidar_2d[1, col] , 0)) 129 | # cv2.circle(original_img, (u, v),2, (0,0,255)) 130 | #cv2.imwrite("original_project"+str(self.counter)+".png", original_img) 131 | #cv2.imshow("original projection", original_img) 132 | #cv2.waitKey(100) 133 | #print("write projection") 134 | #cv2.imwrite("labels_"+str(self.counter)+".png", out) 135 | ######################################################################## 136 | 137 | projected_points = [] 138 | projected_index = [] # just for visualization 139 | labels = [] 140 | original_rgb = [] 141 | class_distribution = [] 142 | for col in range(projected_lidar_2d.shape[1]): 143 | u, v, _ = projected_lidar_2d[:, col] 144 | u ,v = get_cropped_uv_rotated(u, v, rgb_img.shape[1] * 1.0 / 1200 ) 145 | if is_out_of_bound(u, v, rgb_img.shape[1], rgb_img.shape[0]): 146 | continue 147 | projected_points.append(lidar_on_img[:, col]) 148 | if self.distribution_tensor is not None: 149 | distribution_normalized = softmax(distribution[int(v), int(u), :]) 150 | distribution_normalized[0] += np.sum(distribution_normalized[self.num_output_class:]) 151 | distribution_normalized = distribution_normalized[:self.num_output_class] 152 | class_distribution.append(distribution_normalized) 153 | label = np.argmax(distribution_normalized) 154 | else: 155 | label = out[int(v), int(u)] if out[v, u] < self.num_output_class else 0 156 | labels.append(label) 157 | original_rgb.append(rgb_img[int(v), int(u), :]) 158 | projected_index.append(col) 159 | print(" shape of projected points from camera # " + str(camera_ind)+ " is "+str(points_on_img.shape)+", # of in range points is "+str(len(labels))) 160 | ########################################################################## 161 | # uncomment this if you want to visualize the projection && labeling result 162 | self.visualization(labels, projected_index, projected_lidar_2d, rgb_img) 163 | self.counter +=1 164 | ########################################################################## 165 | #publish_pcl_pc2_label(projected_points, labels ) 166 | return labels, projected_points, class_distribution, original_rgb 167 | 168 | 169 | def visualization(self, labels,index, projected_points_2d, rgb_img): 170 | to_show = rgb_img 171 | #print("num of projected lidar points is "+str(len(labels))) 172 | for i in range(len(labels )): 173 | p = (int(projected_points_2d[0, index[i]]), 174 | int(projected_points_2d[1, index[i]])) 175 | #cv2.putText(to_show,. str(int(labels[i])), 176 | # p, 177 | # cv2.FONT_HERSHEY_SIMPLEX, 5, (0,0,203)) 178 | #if is_out_of_bound(p[0], p[1]): continue 179 | 180 | if labels[i] in label_to_color: 181 | color = label_to_color[labels[i]] 182 | else: 183 | color = label_to_color[background] 184 | 185 | cv2.circle(to_show,get_cropped_uv_rotated(p[0], p[1], 512/600.0),2, (color[2], color[1], color[0] )) 186 | #cv2.imwrite("segmentation_projection_out/projected"+str(self.counter)+".png", to_show) 187 | 188 | cv2.imshow("projected",to_show) 189 | cv2.waitKey(100) 190 | 191 | 192 | 193 | 194 | 195 | 196 | -------------------------------------------------------------------------------- /scripts/rgbd2pc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | # source: https://github.com/UMich-BipedLab/segmentation_projection 5 | # maintainer: Ray Zhang rzh@umich.edu 6 | 7 | 8 | # ros 9 | import rospy 10 | import ros_numpy 11 | import message_filters 12 | from sensor_msgs.msg import Image, PointCloud, Pointcloud2, ChannelFloat32 13 | from geometry_msgs.msg import Point32 14 | from cv_bridge import CvBridge, CvBridgeError 15 | 16 | from label2color import label_to_color, background 17 | from helper import publish_pcl_pc2_label 18 | 19 | import cv2, time 20 | import numpy as np 21 | from scipy.ndimage import rotate 22 | # debug 23 | import pdb 24 | import os, sys 25 | 26 | 27 | class RgbdSegmentationNode: 28 | ''' 29 | This class takes RGBD segmented images and correspoinding depth images, 30 | and generates the labeled pointcloud msg 31 | ''' 32 | def __init__(self): 33 | rospy.init_node('rgbd_segmentation_node', anonymous=True) 34 | rospy.sleep(0.5) 35 | 36 | self.is_input_from_file = rospy.get_param("~is_input_from_file") 37 | if self.is_input_from_file: 38 | self.segmented_img_folder = rospy.get_param("~segmented_img_folder") 39 | if not os.path.exists(self.segmented_img_folder): 40 | print("segmented image folder does not exist: " + self.segmented_img_folder) 41 | exit(0) 42 | #self.rgb_img_folder = rospy.get_param("~rgb_img_folder") 43 | #self.depth_img_folder = rospy.get_param("~depth_img_folder") 44 | self.is_input_segmented = rospy.get_param("~is_input_segmented") 45 | 46 | labeled_pc_topic = rospy.get_param("~labeled_pointcloud") 47 | self.labeled_pc_publisher = rospy.Publisher(labeled_pc_topic, PointCloud, queue_size = 40) 48 | 49 | 50 | def publish_dense_labeled_pointcloud(self): 51 | pass 52 | 53 | def generate_labeled_pc_from_scenenet(self): 54 | trajectories = sorted(os.listdir(self.segmented_img_folder)) 55 | 56 | 57 | -------------------------------------------------------------------------------- /scripts/segmentation_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | 5 | # ros 6 | import rospy 7 | import ros_numpy 8 | import message_filters 9 | from sensor_msgs.msg import Image, PointCloud2 10 | from cv_bridge import CvBridge, CvBridgeError 11 | from std_msgs.msg import Float32MultiArray, MultiArrayLayout, MultiArrayDimension 12 | from SegmentationMapping.msg import ImageLabelDistribution 13 | 14 | # map label to color 15 | from label2color import label_to_color, background 16 | from NeuralNetConfigs import NeuralNetConfigs 17 | # visualize pcl 18 | #from helper import publish_pcl_pc2_label 19 | 20 | import cv2, time 21 | import numpy as np 22 | from threading import Lock 23 | from scipy.ndimage import rotate, zoom 24 | 25 | import sys, os 26 | if sys.version_info[0] < 3: 27 | import Queue as queue 28 | else: 29 | import queue 30 | 31 | import tensorflow as tf 32 | 33 | # debug 34 | import pdb 35 | 36 | 37 | 38 | class SegmentationNode: 39 | def __init__(self): 40 | 41 | rospy.init_node('~', anonymous=True) 42 | 43 | self.network_config = NeuralNetConfigs(rospy.get_param("~neural_net_graph"),\ 44 | rospy.get_param("~num_classes"),\ 45 | rospy.get_param("~network_image_input_tensor"),\ 46 | "", \ 47 | rospy.get_param("~neural_net_input_width"),\ 48 | rospy.get_param("~neural_net_input_height"), \ 49 | rospy.get_param("~network_label_output_tensor"),\ 50 | rospy.get_param("~network_distribution_output_tensor")) 51 | print(self.network_config) 52 | self.tf_init() 53 | 54 | self.labeled_img_pub = rospy.Publisher("~label_topic", 55 | Image, queue_size=1) 56 | self.distribution_pub = rospy.Publisher("~distribution_topic", 57 | ImageLabelDistribution, queue_size=1) 58 | self.distribution_at_lidar_time_pub = rospy.Publisher("~distribution_at_lidar_time_topic", 59 | ImageLabelDistribution, queue_size=1) 60 | #self.sub = rospy.Subscriber('~color_topic', Image, self.callback) 61 | self.input_rgb_img_ = message_filters.Subscriber('~color_topic', 62 | Image, queue_size=5) 63 | 64 | self.input_lidar_ = message_filters.Subscriber('/velodyne_points', PointCloud2, queue_size=1) 65 | ts = message_filters.ApproximateTimeSynchronizer([self.input_rgb_img_, self.input_lidar_], 50, 0.04) 66 | ts.registerCallback(self.callback) 67 | 68 | 69 | self.skip_img_freq = rospy.get_param('~skip_input_img_freq') 70 | self.counter = 0 71 | self.bridge = CvBridge() 72 | 73 | 74 | def callback(self, img_msg, lidar_msg): 75 | print("segmentaion call back") 76 | if self.counter % self.skip_img_freq != 0: 77 | self.counter += 1 78 | return 79 | else: 80 | self.counter += 1 81 | now = rospy.Time.now() 82 | rospy.loginfo("New callback time %i %i, img_time: %i %i, depth_time %i %i", now.secs, now.nsecs, img_msg.header.stamp.secs, img_msg.header.stamp.nsecs, lidar_msg.header.stamp.secs, lidar_msg.header.stamp.nsecs ) 83 | #rospy.loginfo("New callback time %i %i, img_time: %i %i", now.secs, now.nsecs, img_msg.header.stamp.secs, img_msg.header.stamp.nsecs ) 84 | original_img = self.bridge.imgmsg_to_cv2(img_msg , desired_encoding="rgb8") 85 | #original_img = cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB) 86 | labeled_img, distribution = self.infer(original_img) 87 | self.publish_label_and_distribution(labeled_img, distribution, img_msg.header, lidar_msg.header) 88 | 89 | def tf_init(self): 90 | print("open graph..") 91 | with tf.gfile.GFile(self.network_config.path, 'rb') as f: 92 | self.graph_def = tf.GraphDef() 93 | self.graph_def.ParseFromString(f.read()) 94 | print("open graph complete") 95 | 96 | config = tf.ConfigProto() 97 | config.gpu_options.allow_growth = True 98 | self.sess = tf.Session(config=config) 99 | self.y, = tf.import_graph_def(self.graph_def, return_elements=[self.network_config.label_output_tensor], name='') 100 | self.G = tf.get_default_graph() 101 | self.x = self.G.get_tensor_by_name(self.network_config.image_input_tensor) 102 | 103 | self.dist = self.G.get_tensor_by_name(self.network_config.distribution_output_tensor) 104 | tf.global_variables_initializer().run(session=self.sess) 105 | print("Tf init finish") 106 | 107 | 108 | def publish_label_and_distribution(self, labeled_img, distribution, header, lidar_header=None): 109 | #now = rospy.Time.now() 110 | #rospy.loginfo("Going to publish at time %i %i\n\n", now.secs, now.nsecs) 111 | 112 | label_msg = self.bridge.cv2_to_imgmsg(labeled_img, encoding="mono8") 113 | label_msg.header = header 114 | 115 | distribution_msg = ImageLabelDistribution() 116 | m_arr = Float32MultiArray() 117 | m_arr.layout.data_offset = 0 118 | m_arr.layout.dim = [MultiArrayDimension() for _ in range(3)] 119 | m_arr.layout.dim[0].label = "h" 120 | m_arr.layout.dim[0].size = labeled_img.shape[0] 121 | m_arr.layout.dim[0].stride = self.network_config.num_classes * labeled_img.size 122 | m_arr.layout.dim[1].label = "w" 123 | m_arr.layout.dim[1].size = labeled_img.shape[1] 124 | m_arr.layout.dim[1].stride = self.network_config.num_classes * labeled_img.shape[1] 125 | m_arr.layout.dim[2].label = "c" 126 | m_arr.layout.dim[2].size = self.network_config.num_classes 127 | m_arr.layout.dim[2].stride = self.network_config.num_classes 128 | m_arr.data = distribution.flatten().tolist() 129 | 130 | 131 | distribution_msg.header = header 132 | distribution_msg.distribution = m_arr 133 | 134 | self.labeled_img_pub.publish(label_msg) 135 | self.distribution_pub.publish(distribution_msg) 136 | 137 | #if lidar_header is not None: 138 | # distribution_msg.header.stamp = lidar_header.stamp 139 | # self.distribution_at_lidar_time_pub.publish(distribution_msg) 140 | 141 | now = rospy.Time.now() 142 | rospy.loginfo("End callabck at time %i %i\n\n", now.secs, now.nsecs) 143 | 144 | def infer(self, rgb_img): 145 | 146 | num_class = self.network_config.num_classes 147 | 148 | #if rgb_img.shape[0] != self.network_config.input_height or \ 149 | # rgb_img.shape[1] != self.network_config.input_width: 150 | # rgb = cv2.resize(rgb_img, \ 151 | # dsize=(self.network_config.input_width, self.network_config.input_height),\ 152 | # interpolation=cv2.INTER_CUBIC) 153 | #else: 154 | # rgb = rgb_img 155 | 156 | rgb = rgb_img 157 | rgb = np.expand_dims(rgb, axis=0) 158 | 159 | label_out, dist_out = self.sess.run([self.y, self.dist], feed_dict={self.x: rgb}) 160 | 161 | now = rospy.Time.now() 162 | 163 | dist_out = dist_out[0, :, :, :] 164 | label_out = label_out[0, :, : ].astype(np.uint8) 165 | rospy.loginfo("after segmentation time %i %i", now.secs, now.nsecs ) 166 | 167 | ''' 168 | dist_exp = np.exp(dist_out) 169 | dist_class = dist_exp 170 | #dist_class = zoom(dist_exp, (1.2, 640.0/800 , 1)).astype(np.float32) 171 | dist_sum = np.sum(dist_class, axis=2, keepdims=1) 172 | 173 | dist_class = dist_class / dist_sum 174 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 175 | background_sum = np.sum(dist_class[:, :, num_class:], axis=2) 176 | dist_class = dist_class[:, :, :num_class] 177 | dist_class[:,:,0] += background_sum 178 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 179 | label_img = np.argmax(dist_class, axis=2).astype(np.uint8) 180 | print (label_img.dtype) 181 | #label_img = cv2.resize(label_img, \ 182 | # dsize=(rgb_img.shape[1], rgb_img.shape[0]),\ 183 | # interpolation=cv2.INTER_NEAREST).astype(np.uint8) 184 | 185 | ''' 186 | 187 | return label_out, dist_out 188 | 189 | def spin(self): 190 | rospy.spin() 191 | 192 | if __name__ == "__main__": 193 | node = SegmentationNode() 194 | node.spin() 195 | -------------------------------------------------------------------------------- /scripts/segmentation_node_two_camera: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | #!/usr/bin/python 4 | 5 | 6 | 7 | # ros 8 | import rospy 9 | import ros_numpy 10 | import message_filters 11 | from sensor_msgs.msg import Image 12 | from cv_bridge import CvBridge, CvBridgeError 13 | from std_msgs.msg import Float32MultiArray, MultiArrayLayout, MultiArrayDimension 14 | from SegmentationMapping.msg import ImageLabelDistribution 15 | 16 | # map label to color 17 | from label2color import label_to_color, background 18 | from NeuralNetConfigs import NeuralNetConfigs 19 | # visualize pcl 20 | #from helper import publish_pcl_pc2_label 21 | 22 | import cv2, time 23 | import numpy as np 24 | from threading import Lock 25 | from scipy.ndimage import rotate, zoom 26 | 27 | import sys, os 28 | if sys.version_info[0] < 3: 29 | import Queue as queue 30 | else: 31 | import queue 32 | 33 | import tensorflow as tf 34 | 35 | # debug 36 | import pdb 37 | 38 | 39 | 40 | class SegmentationNodeTwoCamera: 41 | def __init__(self): 42 | rospy.init_node('~', anonymous=True) 43 | rospy.sleep(0.5) 44 | 45 | 46 | self.network_config = NeuralNetConfigs(rospy.get_param("~neural_net_graph"),\ 47 | rospy.get_param("~num_classes"),\ 48 | rospy.get_param("~network_image_input_tensor"),\ 49 | "", \ 50 | rospy.get_param("~neural_net_input_width"),\ 51 | rospy.get_param("~neural_net_input_height"), \ 52 | rospy.get_param("~network_label_output_tensor"),\ 53 | rospy.get_param("~network_distribution_output_tensor")) 54 | print(self.network_config) 55 | self.tf_init() 56 | 57 | self.labeled_img_pub_1 = rospy.Publisher("~label_topic_1", Image, queue_size=1) 58 | self.distribution_pub_1 = rospy.Publisher("~distribution_topic_1", 59 | ImageLabelDistribution, queue_size=1) 60 | self.labeled_img_pub_2 = rospy.Publisher("~label_topic_2", Image, queue_size=1) 61 | self.distribution_pub_2 = rospy.Publisher("~distribution_topic_2", 62 | ImageLabelDistribution, queue_size=1) 63 | 64 | self.input_rgb_img_1 = message_filters.Subscriber('~color_topic_1', Image, queue_size=4) 65 | self.input_depth_img_1 = message_filters.Subscriber('~depth_topic_1', Image, queue_size=4) 66 | self.input_rgb_img_2 = message_filters.Subscriber('~color_topic_2', Image, queue_size=4) 67 | self.input_depth_img_2 = message_filters.Subscriber('~depth_topic_2', Image, queue_size=4) 68 | #ts = message_filters.TimeSynchronizer([self.input_rgb_img_, self.input_depth_img_, ], 3) 69 | ts = message_filters.ApproximateTimeSynchronizer([self.input_rgb_img_1, self.input_depth_img_1, 70 | self.input_rgb_img_2, self.input_depth_img_2], 2, 0.04, 71 | allow_headerless=False) 72 | ts.registerCallback(self.callback) 73 | 74 | 75 | self.skip_img_freq = rospy.get_param('~skip_input_img_freq') 76 | self.counter = 0 77 | self.bridge = CvBridge() 78 | 79 | 80 | 81 | 82 | def callback(self, img_msg_1, depth_msg_1, img_msg_2, depth_msg_2): 83 | now = rospy.Time.now() 84 | rospy.loginfo("New callback time %i %i, img1_time: %i %i, depth1_time %i %i, img2_time: %i %i, depth2_time: %i %i", 85 | now.secs, now.nsecs, 86 | img_msg_1.header.stamp.secs, img_msg_1.header.stamp.nsecs, 87 | depth_msg_1.header.stamp.secs, depth_msg_1.header.stamp.nsecs, 88 | img_msg_2.header.stamp.secs, img_msg_2.header.stamp.nsecs, 89 | depth_msg_2.header.stamp.secs, depth_msg_2.header.stamp.nsecs ) 90 | 91 | rgb_img_1 = self.bridge.imgmsg_to_cv2(img_msg_1 , desired_encoding="rgb8") + 25 92 | rgb_img_2 = self.bridge.imgmsg_to_cv2(img_msg_2 , desired_encoding="rgb8") 93 | img2_rows = rgb_img_2.shape[0] 94 | 95 | original_img = np.concatenate((rgb_img_2, rgb_img_1), axis=0) 96 | #original_img = cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB) 97 | labeled_img, distribution = self.infer(original_img) 98 | 99 | print ("writing segmentations/{}.png".format(self.counter)) 100 | cv2.imwrite("segmentations/{}_seg.png".format(self.counter), labeled_img ) 101 | cv2.imwrite("segmentations/{}_rgb.png".format(self.counter), cv2.cvtColor(original_img, cv2.COLOR_RGB2BGR )) 102 | self.counter += 1 103 | #cv2.imshow("stacked", labeled_img * 15) 104 | #cv2.waitKey(500) 105 | 106 | self.publish_label_and_distribution(labeled_img[:img2_rows, : ], distribution[:img2_rows, :, :], img_msg_2.header, 107 | self.labeled_img_pub_2, self.distribution_pub_2) 108 | self.publish_label_and_distribution(labeled_img[img2_rows:, : ], distribution[img2_rows:, :, : ], img_msg_1.header, 109 | self.labeled_img_pub_1, self.distribution_pub_1) 110 | 111 | def tf_init(self): 112 | 113 | with tf.gfile.GFile(self.network_config.path, 'rb') as f: 114 | self.graph_def = tf.GraphDef() 115 | self.graph_def.ParseFromString(f.read()) 116 | 117 | 118 | config = tf.ConfigProto() 119 | config.gpu_options.allow_growth = True 120 | self.sess = tf.Session(config=config) 121 | self.y, = tf.import_graph_def(self.graph_def, return_elements=[self.network_config.label_output_tensor], name='') 122 | self.G = tf.get_default_graph() 123 | self.x = self.G.get_tensor_by_name(self.network_config.image_input_tensor) 124 | 125 | self.dist = self.G.get_tensor_by_name(self.network_config.distribution_output_tensor) 126 | tf.global_variables_initializer().run(session=self.sess) 127 | print("Tf init finish") 128 | 129 | 130 | def publish_label_and_distribution(self, labeled_img, distribution, header, label_pub, dist_pub): 131 | 132 | label_msg = self.bridge.cv2_to_imgmsg(labeled_img, encoding="mono8") 133 | label_msg.header = header 134 | 135 | distribution_msg = ImageLabelDistribution() 136 | m_arr = Float32MultiArray() 137 | m_arr.layout.data_offset = 0 138 | m_arr.layout.dim = [MultiArrayDimension() for _ in range(3)] 139 | m_arr.layout.dim[0].label = "h" 140 | m_arr.layout.dim[0].size = labeled_img.shape[0] 141 | m_arr.layout.dim[0].stride = self.network_config.num_classes * labeled_img.size 142 | m_arr.layout.dim[1].label = "w" 143 | m_arr.layout.dim[1].size = labeled_img.shape[1] 144 | m_arr.layout.dim[1].stride = self.network_config.num_classes * labeled_img.shape[1] 145 | m_arr.layout.dim[2].label = "c" 146 | m_arr.layout.dim[2].size = self.network_config.num_classes 147 | m_arr.layout.dim[2].stride = self.network_config.num_classes 148 | m_arr.data = distribution.flatten().tolist() 149 | 150 | 151 | distribution_msg.header = header 152 | distribution_msg.distribution = m_arr 153 | 154 | label_pub.publish(label_msg) 155 | dist_pub.publish(distribution_msg) 156 | 157 | now = rospy.Time.now() 158 | rospy.loginfo("End callabck at time %i %i\n\n", now.secs, now.nsecs) 159 | 160 | def infer(self, rgb_img): 161 | 162 | num_class = self.network_config.num_classes 163 | 164 | #if rgb_img.shape[0] != self.network_config.input_height or \ 165 | # rgb_img.shape[1] != self.network_config.input_width: 166 | # rgb = cv2.resize(rgb_img, \ 167 | # dsize=(self.network_config.input_width, self.network_config.input_height),\ 168 | # interpolation=cv2.INTER_CUBIC) 169 | #else: 170 | # rgb = rgb_img 171 | 172 | rgb = rgb_img 173 | rgb = np.expand_dims(rgb, axis=0) 174 | 175 | label_out, dist_out = self.sess.run([self.y, self.dist], feed_dict={self.x: rgb}) 176 | 177 | now = rospy.Time.now() 178 | rospy.loginfo("Net Infer Fin at time %i %i", now.secs, now.nsecs) 179 | 180 | dist_out = dist_out[0, :, :, :] 181 | label_out = label_out[0, :, : ].astype(np.uint8) 182 | 183 | 184 | ''' 185 | dist_exp = np.exp(dist_out) 186 | dist_class = dist_exp 187 | #dist_class = zoom(dist_exp, (1.2, 640.0/800 , 1)).astype(np.float32) 188 | dist_sum = np.sum(dist_class, axis=2, keepdims=1) 189 | 190 | dist_class = dist_class / dist_sum 191 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 192 | background_sum = np.sum(dist_class[:, :, num_class:], axis=2) 193 | dist_class = dist_class[:, :, :num_class] 194 | dist_class[:,:,0] += background_sum 195 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 196 | label_img = np.argmax(dist_class, axis=2).astype(np.uint8) 197 | print (label_img.dtype) 198 | #label_img = cv2.resize(label_img, \ 199 | # dsize=(rgb_img.shape[1], rgb_img.shape[0]),\ 200 | # interpolation=cv2.INTER_NEAREST).astype(np.uint8) 201 | 202 | ''' 203 | 204 | return label_out, dist_out 205 | 206 | def spin(self): 207 | rospy.spin() 208 | 209 | if __name__ == "__main__": 210 | node = SegmentationNodeTwoCamera() 211 | node.spin() 212 | -------------------------------------------------------------------------------- /scripts/segmentation_visual_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | 5 | # ros 6 | import rospy 7 | import ros_numpy 8 | import message_filters 9 | from sensor_msgs.msg import Image, PointCloud2 10 | from cv_bridge import CvBridge, CvBridgeError 11 | from std_msgs.msg import Float32MultiArray, MultiArrayLayout, MultiArrayDimension 12 | from SegmentationMapping.msg import ImageLabelDistribution 13 | 14 | # map label to color 15 | from label2color import label_to_color, background 16 | from NeuralNetConfigs import NeuralNetConfigs 17 | # visualize pcl 18 | #from helper import publish_pcl_pc2_label 19 | 20 | import cv2, time 21 | import numpy as np 22 | from threading import Lock 23 | from scipy.ndimage import rotate, zoom 24 | 25 | import sys, os 26 | if sys.version_info[0] < 3: 27 | import Queue as queue 28 | else: 29 | import queue 30 | 31 | import tensorflow as tf 32 | 33 | # debug 34 | import pdb 35 | 36 | 37 | 38 | class SegmentationNode: 39 | def __init__(self): 40 | 41 | rospy.init_node('~', anonymous=True) 42 | 43 | self.network_config = NeuralNetConfigs(rospy.get_param("~neural_net_graph"),\ 44 | rospy.get_param("~num_classes"),\ 45 | rospy.get_param("~network_image_input_tensor"),\ 46 | "", \ 47 | rospy.get_param("~neural_net_input_width"),\ 48 | rospy.get_param("~neural_net_input_height"), \ 49 | rospy.get_param("~network_label_output_tensor"),\ 50 | rospy.get_param("~network_distribution_output_tensor")) 51 | print(self.network_config) 52 | self.tf_init() 53 | 54 | self.labeled_img_pub = rospy.Publisher("~label_topic", 55 | Image, queue_size=1) 56 | self.labeled_color_pub = rospy.Publisher("~label_color_topic", 57 | Image, queue_size=1) 58 | self.gray_img_pub = rospy.Publisher("~gray_img_topic", 59 | Image, queue_size=1) 60 | self.distribution_pub = rospy.Publisher("~distribution_topic", 61 | ImageLabelDistribution, queue_size=1) 62 | self.distribution_at_lidar_time_pub = rospy.Publisher("~distribution_at_lidar_time_topic", 63 | ImageLabelDistribution, queue_size=1) 64 | self.sub = rospy.Subscriber('~color_topic', Image, self.callback) 65 | 66 | 67 | 68 | self.skip_img_freq = rospy.get_param('~skip_input_img_freq') 69 | self.counter = 0 70 | self.bridge = CvBridge() 71 | 72 | 73 | def callback(self, img_msg): 74 | print("segmentaion call back") 75 | if self.counter % self.skip_img_freq != 0: 76 | self.counter += 1 77 | return 78 | else: 79 | self.counter += 1 80 | now = rospy.Time.now() 81 | rospy.loginfo("New callback time %i %i, img_time: %i %i", now.secs, now.nsecs, img_msg.header.stamp.secs, img_msg.header.stamp.nsecs ) 82 | #rospy.loginfo("New callback time %i %i, img_time: %i %i", now.secs, now.nsecs, img_msg.header.stamp.secs, img_msg.header.stamp.nsecs ) 83 | original_img = self.bridge.imgmsg_to_cv2(img_msg , desired_encoding="rgb8") 84 | #original_img = cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB) 85 | labeled_img, distribution = self.infer(original_img) 86 | self.publish_label_and_distribution(original_img, labeled_img, distribution, img_msg.header) 87 | 88 | def tf_init(self): 89 | print("open graph..") 90 | with tf.gfile.GFile(self.network_config.path, 'rb') as f: 91 | self.graph_def = tf.GraphDef() 92 | self.graph_def.ParseFromString(f.read()) 93 | print("open graph complete") 94 | 95 | config = tf.ConfigProto() 96 | config.gpu_options.allow_growth = True 97 | self.sess = tf.Session(config=config) 98 | self.y, = tf.import_graph_def(self.graph_def, return_elements=[self.network_config.label_output_tensor], name='') 99 | self.G = tf.get_default_graph() 100 | self.x = self.G.get_tensor_by_name(self.network_config.image_input_tensor) 101 | 102 | self.dist = self.G.get_tensor_by_name(self.network_config.distribution_output_tensor) 103 | tf.global_variables_initializer().run(session=self.sess) 104 | print("Tf init finish") 105 | 106 | 107 | def publish_label_and_distribution(self, original_img, labeled_img, distribution, header): 108 | #now = rospy.Time.now() 109 | #rospy.loginfo("Going to publish at time %i %i\n\n", now.secs, now.nsecs) 110 | 111 | label_msg = self.bridge.cv2_to_imgmsg(labeled_img, encoding="mono8") 112 | label_msg.header = header 113 | 114 | distribution_msg = ImageLabelDistribution() 115 | m_arr = Float32MultiArray() 116 | m_arr.layout.data_offset = 0 117 | m_arr.layout.dim = [MultiArrayDimension() for _ in range(3)] 118 | m_arr.layout.dim[0].label = "h" 119 | m_arr.layout.dim[0].size = labeled_img.shape[0] 120 | m_arr.layout.dim[0].stride = self.network_config.num_classes * labeled_img.size 121 | m_arr.layout.dim[1].label = "w" 122 | m_arr.layout.dim[1].size = labeled_img.shape[1] 123 | m_arr.layout.dim[1].stride = self.network_config.num_classes * labeled_img.shape[1] 124 | m_arr.layout.dim[2].label = "c" 125 | m_arr.layout.dim[2].size = self.network_config.num_classes 126 | m_arr.layout.dim[2].stride = self.network_config.num_classes 127 | m_arr.data = distribution.flatten().tolist() 128 | 129 | 130 | distribution_msg.header = header 131 | distribution_msg.distribution = m_arr 132 | 133 | self.labeled_img_pub.publish(label_msg) 134 | self.distribution_pub.publish(distribution_msg) 135 | 136 | color_img = np.zeros((labeled_img.shape[0], labeled_img.shape[1], 3)) 137 | for r in range(labeled_img.shape[0]): 138 | for c in range(labeled_img.shape[1]): 139 | color_img[r,c, :] = label_to_color[labeled_img[r,c]] 140 | 141 | color_img_msg = self.bridge.cv2_to_imgmsg(color_img.astype('uint8'), encoding='rgb8') 142 | color_img_msg.header = header 143 | self.labeled_color_pub.publish(color_img_msg) 144 | 145 | 146 | gray_img_msg = self.bridge.cv2_to_imgmsg(cv2.cvtColor(original_img, cv2.COLOR_RGB2GRAY), encoding='mono8') 147 | gray_img_msg.header = header 148 | self.gray_img_pub.publish(gray_img_msg) 149 | 150 | now = rospy.Time.now() 151 | rospy.loginfo("End callabck at time %i %i\n\n", now.secs, now.nsecs) 152 | 153 | def infer(self, rgb_img): 154 | 155 | num_class = self.network_config.num_classes 156 | 157 | #if rgb_img.shape[0] != self.network_config.input_height or \ 158 | # rgb_img.shape[1] != self.network_config.input_width: 159 | # rgb = cv2.resize(rgb_img, \ 160 | # dsize=(self.network_config.input_width, self.network_config.input_height),\ 161 | # interpolation=cv2.INTER_CUBIC) 162 | #else: 163 | # rgb = rgb_img 164 | 165 | rgb = rgb_img 166 | rgb = np.expand_dims(rgb, axis=0) 167 | 168 | label_out, dist_out = self.sess.run([self.y, self.dist], feed_dict={self.x: rgb}) 169 | 170 | now = rospy.Time.now() 171 | 172 | dist_out = dist_out[0, :, :, :] 173 | label_out = label_out[0, :, : ].astype(np.uint8) 174 | rospy.loginfo("after segmentation time %i %i", now.secs, now.nsecs ) 175 | 176 | ''' 177 | dist_exp = np.exp(dist_out) 178 | dist_class = dist_exp 179 | #dist_class = zoom(dist_exp, (1.2, 640.0/800 , 1)).astype(np.float32) 180 | dist_sum = np.sum(dist_class, axis=2, keepdims=1) 181 | 182 | dist_class = dist_class / dist_sum 183 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 184 | background_sum = np.sum(dist_class[:, :, num_class:], axis=2) 185 | dist_class = dist_class[:, :, :num_class] 186 | dist_class[:,:,0] += background_sum 187 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 188 | label_img = np.argmax(dist_class, axis=2).astype(np.uint8) 189 | print (label_img.dtype) 190 | #label_img = cv2.resize(label_img, \ 191 | # dsize=(rgb_img.shape[1], rgb_img.shape[0]),\ 192 | # interpolation=cv2.INTER_NEAREST).astype(np.uint8) 193 | 194 | ''' 195 | 196 | return label_out, dist_out 197 | 198 | def spin(self): 199 | rospy.spin() 200 | 201 | if __name__ == "__main__": 202 | node = SegmentationNode() 203 | node.spin() 204 | -------------------------------------------------------------------------------- /scripts/traversability_segmentation_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | 4 | 5 | # ros 6 | import rospy 7 | import ros_numpy 8 | import message_filters 9 | from sensor_msgs.msg import Image, PointCloud2 10 | from cv_bridge import CvBridge, CvBridgeError 11 | from std_msgs.msg import Float32MultiArray, MultiArrayLayout, MultiArrayDimension 12 | from SegmentationMapping.msg import ImageLabelDistribution 13 | 14 | # map label to color 15 | from label2traversability import label_to_traversability 16 | from NeuralNetConfigs import NeuralNetConfigs 17 | # visualize pcl 18 | #from helper import publish_pcl_pc2_label 19 | 20 | import cv2, time 21 | import numpy as np 22 | from threading import Lock 23 | from scipy.ndimage import rotate, zoom 24 | 25 | import sys, os 26 | if sys.version_info[0] < 3: 27 | import Queue as queue 28 | else: 29 | import queue 30 | 31 | import tensorflow as tf 32 | 33 | # debug 34 | import pdb 35 | 36 | 37 | 38 | class SegmentationNode: 39 | def __init__(self): 40 | 41 | rospy.init_node('~', anonymous=True) 42 | 43 | self.output_folder = rospy.get_param("~output_folder") 44 | 45 | self.network_config = NeuralNetConfigs(rospy.get_param("~neural_net_graph"),\ 46 | rospy.get_param("~num_classes"),\ 47 | rospy.get_param("~network_image_input_tensor"),\ 48 | "", \ 49 | rospy.get_param("~neural_net_input_width"),\ 50 | rospy.get_param("~neural_net_input_height"), \ 51 | rospy.get_param("~network_label_output_tensor"),\ 52 | rospy.get_param("~network_distribution_output_tensor")) 53 | print(self.network_config) 54 | self.tf_init() 55 | 56 | self.labeled_img_pub = rospy.Publisher("~label_topic", 57 | Image, queue_size=1) 58 | self.distribution_pub = rospy.Publisher("~distribution_topic", 59 | ImageLabelDistribution, queue_size=1) 60 | self.distribution_at_lidar_time_pub = rospy.Publisher("~distribution_at_lidar_time_topic", 61 | ImageLabelDistribution, queue_size=1) 62 | self.sub = rospy.Subscriber('~color_topic', Image, self.callback) 63 | #self.input_rgb_img_ = message_filters.Subscriber('~color_topic', 64 | # Image, queue_size=5) 65 | 66 | #self.input_lidar_ = message_filters.Subscriber('/velodyne_points', PointCloud2, queue_size=1) 67 | #ts = message_filters.ApproximateTimeSynchronizer([self.input_rgb_img_, self.input_lidar_], 50, 0.04) 68 | #ts.registerCallback(self.callback) 69 | 70 | 71 | self.skip_img_freq = rospy.get_param('~skip_input_img_freq') 72 | self.counter = 0 73 | self.bridge = CvBridge() 74 | 75 | 76 | def callback(self, img_msg): 77 | print("segmentaion call back") 78 | #if self.counter % self.skip_img_freq != 0: 79 | # self.counter += 1 80 | # return 81 | #else: 82 | # self.counter += 1 83 | #now = rospy.Time.now() 84 | #rospy.loginfo("New callback time %i %i, img_time: %i %i, depth_time %i %i", now.secs, now.nsecs, img_msg.header.stamp.secs, img_msg.header.stamp.nsecs, lidar_msg.header.stamp.secs, lidar_msg.header.stamp.nsecs ) 85 | #rospy.loginfo("New callback time %i %i, img_time: %i %i", now.secs, now.nsecs, img_msg.header.stamp.secs, img_msg.header.stamp.nsecs ) 86 | original_img = self.bridge.imgmsg_to_cv2(img_msg , desired_encoding="rgb8") 87 | #original_img = cv2.cvtColor(original_img, cv2.COLOR_BGR2RGB) 88 | 89 | # semantic label to traversability label 90 | labeled_img, distribution = self.infer(original_img) 91 | for r in range(labeled_img.shape[0]): 92 | for c in range(labeled_img.shape[1]): 93 | labeled_img[r, c] = label_to_traversability[labeled_img[r, c]] 94 | 95 | self.save_labeled_img(labeled_img, img_msg.header) 96 | #self.publish_label_and_distribution(labeled_img, distribution, img_msg.header, lidar_msg.header) 97 | 98 | def tf_init(self): 99 | print("open graph..") 100 | with tf.gfile.GFile(self.network_config.path, 'rb') as f: 101 | self.graph_def = tf.GraphDef() 102 | self.graph_def.ParseFromString(f.read()) 103 | print("open graph complete") 104 | 105 | config = tf.ConfigProto() 106 | config.gpu_options.allow_growth = True 107 | self.sess = tf.Session(config=config) 108 | self.y, = tf.import_graph_def(self.graph_def, return_elements=[self.network_config.label_output_tensor], name='') 109 | self.G = tf.get_default_graph() 110 | self.x = self.G.get_tensor_by_name(self.network_config.image_input_tensor) 111 | 112 | self.dist = self.G.get_tensor_by_name(self.network_config.distribution_output_tensor) 113 | tf.global_variables_initializer().run(session=self.sess) 114 | print("Tf init finish") 115 | 116 | def save_labeled_img(self, labeled_img, header): 117 | timestamp = header.stamp.to_nsec() 118 | img_name = self.output_folder + str(timestamp) + ".png" 119 | cv2.imwrite(img_name, labeled_img) 120 | 121 | def infer(self, rgb_img): 122 | 123 | num_class = self.network_config.num_classes 124 | 125 | #if rgb_img.shape[0] != self.network_config.input_height or \ 126 | # rgb_img.shape[1] != self.network_config.input_width: 127 | # rgb = cv2.resize(rgb_img, \ 128 | # dsize=(self.network_config.input_width, self.network_config.input_height),\ 129 | # interpolation=cv2.INTER_CUBIC) 130 | #else: 131 | # rgb = rgb_img 132 | 133 | rgb = rgb_img 134 | rgb = np.expand_dims(rgb, axis=0) 135 | 136 | label_out, dist_out = self.sess.run([self.y, self.dist], feed_dict={self.x: rgb}) 137 | 138 | now = rospy.Time.now() 139 | 140 | dist_out = dist_out[0, :, :, :] 141 | label_out = label_out[0, :, : ].astype(np.uint8) 142 | rospy.loginfo("after segmentation time %i %i", now.secs, now.nsecs ) 143 | 144 | ''' 145 | dist_exp = np.exp(dist_out) 146 | dist_class = dist_exp 147 | #dist_class = zoom(dist_exp, (1.2, 640.0/800 , 1)).astype(np.float32) 148 | dist_sum = np.sum(dist_class, axis=2, keepdims=1) 149 | 150 | dist_class = dist_class / dist_sum 151 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 152 | background_sum = np.sum(dist_class[:, :, num_class:], axis=2) 153 | dist_class = dist_class[:, :, :num_class] 154 | dist_class[:,:,0] += background_sum 155 | assert(np.sum(dist_class[1,1,:]) > 0.99 and np.sum(dist_class[1,1,:]) < 1.01) 156 | label_img = np.argmax(dist_class, axis=2).astype(np.uint8) 157 | print (label_img.dtype) 158 | #label_img = cv2.resize(label_img, \ 159 | # dsize=(rgb_img.shape[1], rgb_img.shape[0]),\ 160 | # interpolation=cv2.INTER_NEAREST).astype(np.uint8) 161 | 162 | ''' 163 | 164 | return label_out, dist_out 165 | 166 | def spin(self): 167 | rospy.spin() 168 | 169 | if __name__ == "__main__": 170 | node = SegmentationNode() 171 | node.spin() 172 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | # packages=['segmentation_projection'], 9 | # package_dir={'': 'scripts'}, 10 | ) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /src/labeled_pc_map_node.cpp: -------------------------------------------------------------------------------- 1 | /*********************************** 2 | * 3 | * labeled_pc_map_node 4 | * 5 | ***********************************/ 6 | /* Author: Ray Zhang, rzh@umich.edu */ 7 | 8 | 9 | 10 | #include "labeled_pc_map.hpp" 11 | #include "PointSegmentedDistribution.hpp" 12 | 13 | #include 14 | #include 15 | 16 | // for nclt: 14 classes 17 | POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointSegmentedDistribution<14>, 18 | (float, x, x) 19 | (float, y, y) 20 | (float, z, z) 21 | (float, rgb, rgb) 22 | (int, label, label) 23 | (float[14],label_distribution, label_distribution) 24 | ) 25 | 26 | 27 | 28 | int main(int argc, char ** argv){ 29 | 30 | ros::init(argc, argv, "labeled_pc_map_node"); 31 | //ros::NodeHandle nh("~"); 32 | ROS_INFO("nclt 14-class pc_painter init...."); 33 | SegmentationMapping::PointCloudPainter<14> painter; 34 | 35 | //pcl::PointCloud> pc; 36 | //pcl::io::loadPCDFile ("/home/biped/.ros/segmented_pcd_cc/1335704128112920045.pcd", pc); 37 | //for (int i = 0; i!= 14; i++) { 38 | // std::cout< 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | #include "PointSegmentedDistribution.hpp" 17 | 18 | using namespace octomap; 19 | namespace fs = boost::filesystem; 20 | 21 | POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointSegmentedDistribution<14>, 22 | (float, x, x) 23 | (float, y, y) 24 | (float, z, z) 25 | (float, rgb, rgb) 26 | (int, label, label) 27 | (float[14],label_distribution, label_distribution) 28 | ) 29 | 30 | void PrintQueryInfo(point3d query, SemanticOcTreeNode* node) { 31 | if (node != NULL) { 32 | std::cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << std::endl; 33 | std::cout << "color of node is: " << node->getColor() << std::endl; 34 | std::cout << "semantics of node is: " << node->getSemantics() << std::endl; 35 | } 36 | else 37 | std::cout << "occupancy probability at " << query << ":\t is unknown" << std::endl; 38 | } 39 | 40 | 41 | 42 | void QueryPointCloudSemantics( 43 | const SemanticOcTree* tree, 44 | const pcl::PointCloud::Ptr pc, 45 | std::string file_name) { 46 | 47 | std::string txt_file = file_name.substr(0, file_name.size()-8) + ".txt"; 48 | std::cout << txt_file << std::endl; 49 | std::ofstream data_out(txt_file); 50 | 51 | std::cout << "Performing some queries ..." << std::endl; 52 | for (int j = 0; j < pc->points.size(); ++j) { 53 | point3d query (pc->points[j].x, pc->points[j].y, pc->points[j].z); 54 | SemanticOcTreeNode* node = tree->search(query); 55 | if (node != NULL && node->getOccupancy() > 0.5 && node->isSemanticsSet()) { 56 | //PrintQueryInfo(query, node); 57 | std::cout << node->isSemanticsSet() << std::endl; 58 | auto semantics = node->getSemantics(); 59 | std::cout << semantics.label.size() << std::endl; 60 | data_out << pc->points[j].x << " " 61 | << pc->points[j].y << " " 62 | << pc->points[j].z; 63 | 64 | for (int c = 0; c < semantics.label.size(); ++c) 65 | data_out << " " << semantics.label[c]; 66 | data_out.close(); 67 | } else 68 | continue; 69 | } 70 | } 71 | 72 | 73 | std::unordered_map> ReadPointCloudPoses(std::string pose_file) { 74 | std::ifstream pose_in(pose_file); 75 | std::string pose_line; 76 | std::unordered_map> time2pose; 77 | 78 | while (std::getline(pose_in, pose_line)) { 79 | std::stringstream ss(pose_line); 80 | uint64_t t; 81 | ss >> t; 82 | //uint64_t time = (uint64_t) (std::round((double)t / 1000.0) + 0.1); 83 | 84 | std::vector pose; 85 | for (int i = 0; i < 12; ++i) { 86 | double ele; 87 | ss >> ele; 88 | pose.push_back(ele); 89 | } 90 | time2pose[t] = pose; 91 | } 92 | return time2pose; 93 | } 94 | 95 | 96 | pcl::PointCloud::Ptr read_nclt_pc_file(const std::string & filename) { 97 | std::ifstream infile(filename); 98 | 99 | pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud); 100 | pc_ptr->header.frame_id = "velodyne"; 101 | //std::stringstream ss(filename.substr(0, filename.size()-4)); 102 | //pc_ptr->header.stamp << ss; 103 | //std::cout<<"read lidar points at time stamp "<header.stamp<<". "; 104 | 105 | std::string line; 106 | 107 | while (std::getline(infile, line)) { 108 | std::stringstream ss(line); 109 | pcl::PointXYZ p; 110 | float x, y, z; 111 | char comma; 112 | if (!(ss >> x) || 113 | !(ss >> y) || 114 | !(ss >> z)) { 115 | std::cerr<<"sstream convert "<> intensity >> l; 120 | p.x = x; 121 | p.y = y; 122 | p.z = z; 123 | pc_ptr->push_back(p); 124 | 125 | } 126 | //std::cout<<" # of points is "<size()<> time2pose = ReadPointCloudPoses(pose_file); 140 | std::cout << "Read poses size: " << time2pose.size() << std::endl; 141 | 142 | // Read gt times 143 | //std::string time_file = "/home/biped/perl_code/rvsm/nclt_may23_gt/gt_times.txt"; 144 | //std::ifstream time_in(time_file); 145 | //std::string time_line; 146 | //std::string seq_path("/home/biped//perl_code/gicp/visualization/data/"); 147 | 148 | std::string seg_pcds("/home/biped/perl_code/workspace/src/segmentation_projection/data/nclt_04_29/seg_pcds_downsampled/"); 149 | std::string seg_pcds_global("/home/biped/perl_code/workspace/src/segmentation_projection/data/nclt_04_29/seg_pcds_downsampled_global/"); 150 | 151 | 152 | std::vector pcds; 153 | for(auto& entry : fs::directory_iterator(seg_pcds ) ) 154 | pcds.push_back(entry.path()); 155 | std::sort(pcds.begin(), pcds.end()); 156 | 157 | for (int i = 1; i < pcds.size() ; i++) { 158 | //while (std::getline(time_in, time_line)) { 159 | 160 | /* 161 | std::stringstream ss(time_line); 162 | uint64_t time; 163 | ss >> time; 164 | std::string csv_file = seq_path + "/" + std::to_string(time) + ".bin.csv"; 165 | if (!fs::exists(csv_file)) { 166 | std::cout << "Can't find csv file " << csv_file << " at time " << time << std::endl; 167 | continue; 168 | } 169 | 170 | pcl::PointCloud::Ptr pc = read_nclt_pc_file(csv_file); 171 | */ 172 | //std::cout << "Process " << pc->size() << " points at time " << time << std::endl; 173 | 174 | 175 | 176 | std::string name = pcds[i].string(); 177 | 178 | pcl::PointCloud>::Ptr pc(new pcl::PointCloud>); 179 | pcl::io::loadPCDFile> ( name , *pc); 180 | 181 | 182 | std::istringstream iss(name.substr(name.size()-20, 16 ) ); 183 | uint64_t time; 184 | iss >> time; 185 | std::cout<<"processing time "<>::Ptr global_pc(new pcl::PointCloud >); 198 | pcl::transformPointCloud> (*pc, *global_pc, T); 199 | 200 | pcl::io::savePCDFile(seg_pcds_global + "/" + std::to_string(time) + ".pcd" , *global_pc ); 201 | //QueryPointCloudSemantics(read_semantic_tree, global_pc, csv_file); 202 | } 203 | return 0; 204 | } 205 | 206 | 207 | -------------------------------------------------------------------------------- /src/rvsm_nclt_node.cpp: -------------------------------------------------------------------------------- 1 | /*********************************** 2 | * 3 | * labeled_pc_map_node 4 | * 5 | ***********************************/ 6 | /* Author: Ray Zhang, rzh@umich.edu */ 7 | 8 | 9 | 10 | #include "labeled_pc_map.hpp" 11 | #include "PointSegmentedDistribution.hpp" 12 | 13 | 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | using namespace octomap; 31 | using namespace boost::filesystem; 32 | 33 | // for nclt: 14 classes 34 | POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointSegmentedDistribution<14>, 35 | (float, x, x) 36 | (float, y, y) 37 | (float, z, z) 38 | (float, rgb, rgb) 39 | (int, label, label) 40 | (float[14],label_distribution, label_distribution) 41 | ) 42 | 43 | void PrintQueryInfo(point3d query, SemanticOcTreeNode* node) { 44 | if (node != NULL) { 45 | std::cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << std::endl; 46 | std::cout << "color of node is: " << node->getColor() << std::endl; 47 | std::cout << "semantics of node is: " << node->getSemantics() << std::endl; 48 | } 49 | else 50 | std::cout << "occupancy probability at " << query << ":\t is unknown" << std::endl; 51 | } 52 | 53 | 54 | 55 | void QueryPointCloudSemantics(const std::shared_ptr< SemanticOcTree> tree, 56 | const pcl::PointCloud::Ptr pc, 57 | const pcl::PointCloud::Ptr pc_local, 58 | std::string file_name) { 59 | 60 | std::string txt_file = file_name.substr(0, file_name.size()-8) + ".txt"; 61 | std::cout << txt_file << std::endl; 62 | std::ofstream data_out(txt_file); 63 | 64 | std::cout << "Performing some queries ... # of points is "<size()<<", writing to " << txt_file<points.size(); ++j) { 67 | point3d query (pc->points[j].x, pc->points[j].y, pc->points[j].z); 68 | const SemanticOcTreeNode* node = tree->search(query); 69 | if (node != NULL && node->getOccupancy() > 0.35 && node->isSemanticsSet()) { 70 | count += 1; 71 | //PrintQueryInfo(query, node); 72 | auto semantics = node->getSemantics(); 73 | data_out << pc_local->points[j].x << " " 74 | << pc_local->points[j].y << " " 75 | << pc_local->points[j].z; 76 | 77 | for (int c = 0; c < semantics.label.size(); ++c) 78 | data_out << " " << semantics.label[c]; 79 | data_out<<"\n"; 80 | } else 81 | continue; 82 | } 83 | std::cout<<"Count is "<> ReadPointCloudPoses(std::string pose_file) { 89 | std::ifstream pose_in(pose_file); 90 | std::string pose_line; 91 | std::unordered_map> time2pose; 92 | 93 | while (std::getline(pose_in, pose_line)) { 94 | std::stringstream ss(pose_line); 95 | uint64_t t; 96 | ss >> t; 97 | uint64_t time = t; // (uint64_t) (std::round((double)t / 1000.0) + 0.1); 98 | 99 | std::vector pose; 100 | for (int i = 0; i < 12; ++i) { 101 | double ele; 102 | ss >> ele; 103 | pose.push_back(ele); 104 | } 105 | time2pose[time] = pose; 106 | } 107 | return time2pose; 108 | } 109 | 110 | 111 | pcl::PointCloud::Ptr read_nclt_pc_file(const std::string & filename) { 112 | std::ifstream infile(filename); 113 | 114 | pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud); 115 | pc_ptr->header.frame_id = "velodyne"; 116 | //std::stringstream ss(filename.substr(0, filename.size()-4)); 117 | //pc_ptr->header.stamp << ss; 118 | //std::cout<<"read lidar points at time stamp "<header.stamp<<". "; 119 | 120 | std::string line; 121 | 122 | while (std::getline(infile, line)) { 123 | std::stringstream ss(line); 124 | pcl::PointXYZ p; 125 | float x, y, z; 126 | char comma; 127 | if (!(ss >> x) || 128 | !(ss >> y) || 129 | !(ss >> z)) { 130 | std::cerr<<"sstream convert "<> intensity >> l; 135 | p.x = x; 136 | p.y = y; 137 | p.z = z; 138 | pc_ptr->push_back(p); 139 | 140 | } 141 | //std::cout<<" # of points is "<size()<& tokens) { 150 | std::stringstream ss( input_line ); 151 | 152 | while( ss.good() ) { 153 | std::string substr; 154 | std::getline( ss, substr, ',' ); 155 | tokens.push_back( substr ); 156 | } 157 | } 158 | 159 | template 160 | typename pcl::PointCloud>::Ptr 161 | read_seg_pcd_text(const std::string & file_name) { 162 | // write to a pcd file 163 | typename pcl::PointCloud>::Ptr in_cloud(new typename pcl::PointCloud>); 164 | std::ifstream infile(file_name); 165 | typename pcl::PointSegmentedDistribution point; 166 | float rf; 167 | float gf; 168 | float bf; 169 | while(!infile.eof()) { 170 | 171 | infile >> point.x >> point.y >> point.z >> rf >> gf >> bf >> point.label; 172 | //infile >> point.x >> point.y >> point.z >> point.label; 173 | for (int j = 0 ;j < NUM_CLASS; j++) 174 | infile >> point.label_distribution[j]; 175 | uint8_t r = int(rf*255), g = int(gf*255), b = int(bf*255); 176 | //uint8_t r = int(250), g = int(250), b = int(250); 177 | // Example: Red color 178 | //std::cout << (int)r << " " <<(int) g << " " <<(int) b <(&rgb); 181 | in_cloud->push_back(point); 182 | //std::cout<<"file "<size is "<size()<<", xyz "< octree_ptr ) { 189 | 190 | // Read pc poses 191 | std::string pose_file = "/home/biped/perl_code/workspace/src/segmentation_projection/data/nclt_04_29/pose_sequence_16.txt"; 192 | std::unordered_map> time2pose = ReadPointCloudPoses(pose_file); 193 | std::cout << "Query: Read poses size: " << time2pose.size() << std::endl; 194 | 195 | // Read gt times 196 | std::string time_file = "/home/biped/perl_code/rvsm/nclt_may23_gt/gt_times.txt"; 197 | std::ifstream time_in(time_file); 198 | std::string time_line; 199 | //std::string seq_path("/home/biped//perl_code/gicp/visualization/data/rvm_04_29/"); 200 | std::string seq_path("/home/biped//perl_code/workspace/src/segmentation_projection/data/nclt_04_29/nclt_lidar_downsampled/"); 201 | 202 | while (std::getline(time_in, time_line)) { 203 | std::stringstream ss(time_line); 204 | uint64_t time; 205 | ss >> time; 206 | std::string csv_file = seq_path + "/" + std::to_string(time) + ".bin.csv"; 207 | if (!exists(csv_file)) { 208 | std::cout << "Can't find csv file " << csv_file << " at time " << time << std::endl; 209 | continue; 210 | } 211 | 212 | pcl::PointCloud::Ptr pc = read_nclt_pc_file(csv_file); 213 | //std::cout << "Query:: Process " << pc->size() << " points at time " << time << std::endl; 214 | Eigen::Matrix4d T; 215 | T.setIdentity(); 216 | for (int r=0; r<3; ++r){ 217 | for (int c=0; c<4; ++c) { 218 | T(r, c) = (time2pose.at(time))[4*r+c]; 219 | } 220 | } 221 | Eigen::Affine3d aff(T.matrix()); 222 | 223 | 224 | // Transform point cloud to global coordinates 225 | pcl::PointCloud::Ptr global_pc(new pcl::PointCloud); 226 | pcl::transformPointCloud (*pc, *global_pc, T); 227 | QueryPointCloudSemantics(octree_ptr, global_pc, pc, csv_file); 228 | } 229 | 230 | } 231 | 232 | int main(int argc, char ** argv) { 233 | 234 | ros::init(argc, argv, "nclt_map_node"); 235 | ROS_INFO("nclt 14-class pc_painter init...."); 236 | SegmentationMapping::PointCloudPainter<14> painter; 237 | 238 | std::string seg_pcds(argv[1]); // segmented pcds 239 | std::string pose_path(argv[2]); 240 | 241 | std::ifstream in_pose(pose_path); 242 | std::string pose_line; 243 | int counter = 0; 244 | 245 | while (std::getline(in_pose, pose_line)) { 246 | 247 | std::stringstream ss(pose_line); 248 | uint64_t pose_time; 249 | ss >> pose_time; 250 | 251 | 252 | std::string file = seg_pcds + "/" + std::to_string(pose_time) + ".txt" ; 253 | if ( !exists( file ) ) { 254 | std::cout << "Can't find pcd file "<>::Ptr pc = read_seg_pcd_text<14>(file); 260 | //pcl::PointCloud>::Ptr pc(new pcl::PointCloud>); 261 | //pcl::io::loadPCDFile> (file, *pc); 262 | 263 | Eigen::Matrix4d T; 264 | T.setIdentity(); 265 | 266 | for (int r = 0; r < 3; r++){ 267 | for (int c = 0; c < 4; c++) { 268 | double pose; 269 | ss >> pose; 270 | T(r, c) = pose; 271 | } 272 | } 273 | 274 | 275 | //for gloabl frame points 276 | //Eigen::Matrix4d T_inv; 277 | //T_inv = T.inverse(); 278 | //Eigen::Affine3d T_inv_aff(T_inv.matrix()); 279 | //pcl::transformPointCloud(*pc, *pc, T_inv_aff); 280 | 281 | Eigen::Affine3d aff(T.matrix()); 282 | std::cout<<"Process "<size()<<" labeled points at time "<< pose_time< stereo_seg; 9 | 10 | ros::spin(); 11 | 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /youtube.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UMich-BipedLab/SegmentationMapping/b58eec234ae6fd78a2e7ba8b2398ff05fb467e19/youtube.png --------------------------------------------------------------------------------