├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── README.md ├── cmake ├── FindCSparse.cmake ├── FindCholmod.cmake └── FindG2O.cmake ├── config ├── bucket_detector.yaml ├── bucket_detector_workspace.yaml ├── yolo_detector.yaml ├── yolo_detector_kitti.yaml └── yolo_detector_rotonda.yaml ├── include ├── g2o │ └── edge_se3_plane.hpp ├── planar_segmentation │ ├── detected_object.h │ ├── plane_segmentation.h │ └── point_cloud_segmentation.h ├── ps_graph_slam │ ├── data_association.h │ ├── graph_slam.hpp │ ├── information_matrix_calculator.hpp │ ├── keyframe.hpp │ ├── keyframe_updater.hpp │ ├── landmark.h │ ├── ros_time_hash.hpp │ ├── ros_utils.hpp │ └── semantic_graph_slam.h ├── semantic_graph_slam_ros.h └── tools.h ├── launch ├── ps_slam_with_rovio_pose_yolo.launch ├── ps_slam_with_rovio_pose_yolo_corridor.launch ├── ps_slam_with_snap_pose_bucket_det_lab_data.launch ├── ps_slam_with_snap_pose_bucket_det_lab_data_with_octomap.launch └── semantic_graph_slam.launch ├── msg ├── BoundingBox.msg ├── BoundingBoxes.msg ├── DetectedObjects.msg ├── ObjectInfo.msg └── acl_msgs │ └── ViconState.msg ├── octomap.gif ├── package.xml ├── rviz ├── graph_semantic_slam.rviz ├── rviz.rviz └── semantic_slam_rviz.rviz ├── semantic.gif ├── semantic_slam.png ├── src ├── planar_segmentation │ └── plane_segmentation.cpp ├── ps_graph_slam │ ├── graph_slam.cpp │ ├── information_matrix_calculator.cpp │ ├── keyframe.cpp │ └── semantic_graph_slam.cpp ├── semantic_graph_SLAM_node.cpp └── semantic_graph_slam_ros.cpp └── travis_build.sh /.gitignore: -------------------------------------------------------------------------------- 1 | # Temp 2 | *~ 3 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | 2 | os: linux 3 | language: shell 4 | dist: xenial 5 | before_install: 6 | 7 | install: 8 | 9 | services: 10 | - docker 11 | script: 12 | - docker pull hridaybavle/semantic_slam:v1 13 | - docker container create --name dockervm -t -i hridaybavle/semantic_slam:v1 bash 14 | - docker start dockervm 15 | - docker cp travis_build.sh dockervm:/root/ 16 | - docker exec -it dockervm /bin/bash /root/travis_build.sh 17 | 18 | after_success: 19 | 20 | before_deploy: 21 | 22 | deploy: 23 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | set(PROJECT_NAME semantic_SLAM) 4 | project(${PROJECT_NAME}) 5 | 6 | set(CMAKE_CXX_STANDARD 14) 7 | add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 8 | set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 9 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 10 | set(CMAKE_BUILD_TYPE "RELEASE") 11 | 12 | 13 | FILE(GLOB_RECURSE SEMANTIC_SLAM_SOURCE_FILES 14 | ${CMAKE_CURRENT_SOURCE_DIR}/src/planar_segmentation/*.cpp 15 | ${CMAKE_CURRENT_SOURCE_DIR}/src/ps_graph_slam/*.cpp 16 | ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) 17 | 18 | 19 | find_package(catkin REQUIRED 20 | COMPONENTS roscpp std_msgs sensor_msgs image_transport tf_conversions tf message_generation cv_bridge image_transport pcl_ros pcl_msgs pcl_conversions) 21 | 22 | find_package(PCL 1.7 REQUIRED 23 | COMPONENTS common io sample_consensus segmentation filters) 24 | 25 | find_package(G2O REQUIRED) 26 | include_directories(SYSTEM PUBLIC ${G2O_INCLUDE_DIRS}) 27 | link_directories(${G2O_LIBRARY_DIRS}) 28 | 29 | FIND_PACKAGE(Cholmod) 30 | include_directories(${CHOLMOD_INCLUDE_DIR}) 31 | 32 | find_package(CSparse REQUIRED) 33 | include_directories(${CSPARSE_INCLUDE_DIR}) 34 | 35 | find_package(OpenMP) 36 | if (OPENMP_FOUND) 37 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 38 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 39 | endif() 40 | 41 | 42 | find_package(OpenCV REQUIRED) 43 | 44 | find_package(Eigen3) 45 | if(NOT EIGEN3_FOUND) 46 | # Fallback to cmake_modules 47 | find_package(cmake_modules REQUIRED) 48 | find_package(Eigen REQUIRED) 49 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 50 | set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES}) # Not strictly necessary as Eigen is head only 51 | # Possibly map additional variables to the EIGEN3_ prefix. 52 | message(WARN "Using Eigen2!") 53 | else() 54 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 55 | endif() 56 | 57 | add_message_files( 58 | FILES 59 | ObjectInfo.msg 60 | DetectedObjects.msg 61 | acl_msgs/ViconState.msg 62 | BoundingBox.msg 63 | BoundingBoxes.msg 64 | ) 65 | 66 | generate_messages( 67 | DEPENDENCIES 68 | std_msgs 69 | sensor_msgs 70 | ) 71 | 72 | catkin_package( 73 | INCLUDE_DIRS include 74 | LIBRARIES semantic_SLAM OpenCV PCL 75 | CATKIN_DEPENDS roscpp std_msgs sensor_msgs image_transport tf_conversions tf message_runtime cv_bridge image_transport pcl_ros pcl_msgs pcl_conversions 76 | ) 77 | 78 | 79 | include_directories(include) 80 | include_directories(${catkin_INCLUDE_DIRS}) 81 | include_directories(SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) 82 | include_directories(${OpenCV_INCLUDE_DIRS}) 83 | include_directories(${PCL_INCLUDE_DIRS}) 84 | 85 | link_directories(${PCL_LIBRARY_DIRS}) 86 | add_definitions(${PCL_DEFINITIONS}) 87 | 88 | #SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse ) 89 | 90 | add_library(SEMANTIC_SLAM ${SEMANTIC_SLAM_SOURCE_FILES}) 91 | add_dependencies(SEMANTIC_SLAM ${PROJECT_NAME}_gencpp) 92 | add_dependencies(SEMANTIC_SLAM ${catkin_EXPORTED_TARGETS}) 93 | target_link_libraries(SEMANTIC_SLAM ${catkin_LIBRARIES}) 94 | target_link_libraries(SEMANTIC_SLAM ${EIGEN3_LIBRARIES}) 95 | target_link_libraries(SEMANTIC_SLAM ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_SAMPLE_CONSENSUS_LIBRARIES} ${PCL_SEGMENTATION_LIBRARIES} ${PCL_FILTERS_LIBRARIES}) 96 | target_link_libraries(SEMANTIC_SLAM ${G2O_TYPES_DATA} 97 | ${G2O_CORE_LIBRARY} 98 | ${G2O_STUFF_LIBRARY} 99 | ${G2O_SOLVER_PCG} 100 | ${G2O_SOLVER_CSPARSE} # be aware of that CSPARSE is released under LGPL 101 | ${G2O_SOLVER_CHOLMOD} # be aware of that cholmod is released under GPL 102 | ${G2O_TYPES_SLAM3D} 103 | ${G2O_TYPES_SLAM3D_ADDONS} 104 | ) 105 | 106 | 107 | add_executable(semantic_graph_SLAM_node src/semantic_graph_SLAM_node.cpp) 108 | add_dependencies(semantic_graph_SLAM_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 109 | target_link_libraries(semantic_graph_SLAM_node SEMANTIC_SLAM) 110 | target_link_libraries(semantic_graph_SLAM_node ${OpenCV_LIBS}) 111 | target_link_libraries(semantic_graph_SLAM_node ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Semantic SLAM # 2 | 3 | This package can perform optimization of pose estimated from VO/VIO methods which tend to drift over time. It uses planar surfaces extracted from object detections in order to create a sparse semantic map of the environment, thus optimizing the drift of the VO/VIO algorithms. 4 | 5 | ### In order to run this package you will need two additional modules ### 6 | - A VO/VIO algorithm: [ROVIO](https://github.com/ethz-asl/rovio), [VINS_MONO](https://github.com/HKUST-Aerial-Robotics/VINS-Mono), [OKVIS](https://github.com/ethz-asl/okvis) etc. 7 | - An Object Detector: [Yolo](https://github.com/leggedrobotics/darknet_ros), [Shape Color Detector](https://bitbucket.org/hridaybavle/bucket_detector) 8 | 9 | Currently it can extract planar surfaces and create a semantic map from from the following objects: 10 | - chair 11 | - tvmonitor 12 | - book 13 | - keyboard 14 | - laptop 15 | - bucket 16 | - car 17 | 18 | ### Related Paper: ### 19 | 20 | ```latex 21 | @ARTICLE{9045978, 22 | author={Bavle, Hriday and De La Puente, Paloma and How, Jonathan P. and Campoy, Pascual}, 23 | journal={IEEE Access}, 24 | title={VPS-SLAM: Visual Planar Semantic SLAM for Aerial Robotic Systems}, 25 | year={2020}, 26 | volume={8}, 27 | number={}, 28 | pages={60704-60718}, 29 | doi={10.1109/ACCESS.2020.2983121}} 30 | ``` 31 | 32 | ### Video ### 33 | 34 | Semantic SLAM 36 | 37 | ### How do I set it up? 38 | 39 | **First install g2o following these instructions (Tested on Kinetic and Melodic Distributions):** 40 | ``` 41 | - sudo apt-get install ros-$ROS_DISTRO-libg2o 42 | - sudo cp -r /opt/ros/$ROS_DISTRO/lib/libg2o_* /usr/local/lib 43 | - sudo cp -r /opt/ros/$ROS_DISTRO/include/g2o /usr/local/include 44 | ``` 45 | **Install OctopMap server for map generation capabilities:** 46 | ``` 47 | - sudo apt install ros-$ROS_DISTRO-octomap* 48 | ``` 49 | 50 | ### Try a simple example with pre-recorded VIO pose and a blue bucket detector: 51 | 52 | **Create a ros workspace and clone the following packages:** 53 | 54 | - Download the rosbag: 55 | ``` 56 | wget -P ~/Downloads/ https://www.dropbox.com/s/jnywuvcn2m9ubu2/entire_lab_3_rounds.bag 57 | ``` 58 | - Create a workspace, clone the repo and compile: 59 | ``` 60 | mkdir -p workspace/ros/semantic_slam_ws/src/ && cd workspace/ros/semantic_slam_ws/src/ 61 | git clone https://github.com/hridaybavle/semantic_slam && git clone https://bitbucket.org/hridaybavle/bucket_detector.git 62 | cd .. && catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release 63 | ``` 64 | - Launch and visualize 65 | ``` 66 | source devel/setup.bash 67 | roslaunch semantic_SLAM ps_slam_with_snap_pose_bucket_det_lab_data_with_octomap.launch bagfile:=${HOME}/Downloads/entire_lab_3_rounds.bag show_rviz:=true 68 | ``` 69 | 70 | ![test](octomap.gif) 71 | 72 | ### Using Docker Image 73 | 74 | If the code is giving problems with you local machine, you can try the docker image created with the repo and the required settings. 75 | 76 | **Download Docker from: [Docker](https://docs.docker.com/engine/install/ubuntu/)** 77 | 78 | **Follow the commands to run the algorithm with the docker** 79 | ``` 80 | docker pull hridaybavle/semantic_slam:v1 81 | docker run --rm -it --net="host" -p 11311:11311 hridaybavle/semantic_slam:v1 bash 82 | cd ~/workspace/ros/semantic_slam_ws/ 83 | source devel/setup.bash 84 | roslaunch semantic_SLAM ps_slam_with_snap_pose_bucket_det_lab_data_with_octomap.launch bagfile:=${HOME}/Downloads/entire_lab_3_rounds.bag show_rviz:=false 85 | ``` 86 | **Open a new terminal and rviz in local machine** 87 | ``` 88 | cd ~/Downloads/ && wget https://raw.githubusercontent.com/hridaybavle/semantic_slam/master/rviz/graph_semantic_slam.rviz 89 | rviz -d graph_semantic_slam.rviz 90 | ``` 91 | 92 | ### Subsribed Topics 93 | 94 | - **/SQ04/snap_vislam/vislam/pose** ([geometry_msgs/PoseStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html)) 95 | The default snapdragon VIO pose published in NED in frame. This message can be remapped remapped to any other VO pose message publishing in NED frame. ([See frame conventions](https://en.wikipedia.org/wiki/Axes_conventions)) 96 | 97 | 98 | - **/rovio/odometry** ([geometry_msgs/PoseStamped](http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html)) 99 | The VIO odometry published in ENU frame. Can be remapped to the desired topic name in the launcher. 100 | 101 | 102 | - **/depth_registered/points** ([sensor_msgs/PointCloud2](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html)) 103 | The point cloud required for planar surface extraction. 104 | 105 | 106 | - **/darknet_ros/bounding_boxes**([darknet_msgs_ros/BoundingBoxes](https://github.com/leggedrobotics/darknet_ros)) 107 | The detection bounding boxes published by yolo if using the yolo detector ros package. 108 | 109 | - **/image_processed/bounding_boxes**([ShapeColor_ObjectDetection/DetectedObjects](https://hridaybavle@bitbucket.org/hridaybavle/bucket_detector.git)) 110 | The detection bounding boxes if using the bucket detector. It can be downloaded from the link above. 111 | 112 | 113 | ### Published Topics 114 | 115 | - **robot_pose**([geometry_msgs/PoseStamped](http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html)) 116 | The pose of the robot estimated by the algo. 117 | 118 | - **robot_path**([nav_msgs/Path](http://docs.ros.org/melodic/api/nav_msgs/html/msg/Path.html)) 119 | The path of the robot psoe for visualization. 120 | 121 | - **keyframe_poses**([geometry_msgs/PoseArray](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseArray.html)) 122 | The poses of the keyframe being added to the g2o graph. 123 | 124 | - **mapped_landmarks**([visualization_msgs/MarkerArray](http://docs.ros.org/melodic/api/visualization_msgs/html/msg/MarkerArray.html)) 125 | The mapped semantic planar surfaces. 126 | 127 | - **detected_landmarks**([visualization_msgs/MarkerArray](http://docs.ros.org/melodic/api/visualization_msgs/html/msg/MarkerArray.html)) 128 | The detected landmarks in the current frame. 129 | 130 | The configurations of the algorithms can be found inside the cfg folder in order to be changed accordingly. 131 | 132 | ### Published TFs 133 | 134 | - **map to odom transform:** The transform published between the map frame and the odom frame after the corrections from the semantic SLAM. 135 | 136 | - **base_link to odom transform:** The transform published between the base_link (on the robot) frame and the odom frame as estimated by the VO/VIO algorithm. 137 | 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /cmake/FindCSparse.cmake: -------------------------------------------------------------------------------- 1 | # Look for csparse; note the difference in the directory specifications! 2 | find_path(CSPARSE_INCLUDE_DIR NAMES cs.h 3 | PATHS 4 | /usr/include/suitesparse 5 | /usr/include 6 | /opt/local/include 7 | /usr/local/include 8 | /sw/include 9 | /usr/include/ufsparse 10 | /opt/local/include/ufsparse 11 | /usr/local/include/ufsparse 12 | /sw/include/ufsparse 13 | PATH_SUFFIXES 14 | suitesparse 15 | ) 16 | 17 | find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse 18 | PATHS 19 | /usr/lib 20 | /usr/local/lib 21 | /opt/local/lib 22 | /sw/lib 23 | ) 24 | 25 | include(FindPackageHandleStandardArgs) 26 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG 27 | CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY) 28 | -------------------------------------------------------------------------------- /cmake/FindCholmod.cmake: -------------------------------------------------------------------------------- 1 | # Cholmod lib usually requires linking to a blas and lapack library. 2 | # It is up to the user of this module to find a BLAS and link to it. 3 | 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 5 | set(CHOLMOD_FIND_QUIETLY TRUE) 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 7 | 8 | find_path(CHOLMOD_INCLUDE_DIR 9 | NAMES 10 | cholmod.h 11 | PATHS 12 | $ENV{CHOLMODDIR} 13 | ${INCLUDE_INSTALL_DIR} 14 | PATH_SUFFIXES 15 | suitesparse 16 | ufsparse 17 | ) 18 | 19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY}) 21 | 22 | if(CHOLMOD_LIBRARIES) 23 | 24 | get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH) 25 | 26 | find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 27 | if (AMD_LIBRARY) 28 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY}) 29 | else () 30 | set(CHOLMOD_LIBRARIES FALSE) 31 | endif () 32 | 33 | endif(CHOLMOD_LIBRARIES) 34 | 35 | if(CHOLMOD_LIBRARIES) 36 | 37 | find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 38 | if (COLAMD_LIBRARY) 39 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY}) 40 | else () 41 | set(CHOLMOD_LIBRARIES FALSE) 42 | endif () 43 | 44 | endif(CHOLMOD_LIBRARIES) 45 | 46 | if(CHOLMOD_LIBRARIES) 47 | 48 | find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 49 | if (CAMD_LIBRARY) 50 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY}) 51 | else () 52 | set(CHOLMOD_LIBRARIES FALSE) 53 | endif () 54 | 55 | endif(CHOLMOD_LIBRARIES) 56 | 57 | if(CHOLMOD_LIBRARIES) 58 | 59 | find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 60 | if (CCOLAMD_LIBRARY) 61 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY}) 62 | else () 63 | set(CHOLMOD_LIBRARIES FALSE) 64 | endif () 65 | 66 | endif(CHOLMOD_LIBRARIES) 67 | 68 | if(CHOLMOD_LIBRARIES) 69 | 70 | find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 71 | if (CHOLMOD_METIS_LIBRARY) 72 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY}) 73 | endif () 74 | 75 | endif(CHOLMOD_LIBRARIES) 76 | 77 | if(CHOLMOD_LIBRARIES) 78 | find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 79 | PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR}) 80 | if (CHOLMOD_SUITESPARSECONFIG_LIBRARY) 81 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY}) 82 | endif () 83 | endif(CHOLMOD_LIBRARIES) 84 | 85 | include(FindPackageHandleStandardArgs) 86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG 87 | CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 88 | 89 | mark_as_advanced(CHOLMOD_LIBRARIES) 90 | -------------------------------------------------------------------------------- /cmake/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | # Find the header files 2 | 3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 4 | ${G2O_ROOT}/include 5 | $ENV{G2O_ROOT}/include 6 | $ENV{G2O_ROOT} 7 | /usr/local/include 8 | /usr/include 9 | /opt/local/include 10 | /sw/local/include 11 | /sw/include 12 | NO_DEFAULT_PATH 13 | ) 14 | 15 | # Macro to unify finding both the debug and release versions of the 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 17 | # macro. 18 | 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 20 | 21 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 22 | NAMES "g2o_${MYLIBRARYNAME}_d" 23 | PATHS 24 | ${G2O_ROOT}/lib/Debug 25 | ${G2O_ROOT}/lib 26 | $ENV{G2O_ROOT}/lib/Debug 27 | $ENV{G2O_ROOT}/lib 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 32 | NAMES "g2o_${MYLIBRARYNAME}_d" 33 | PATHS 34 | ~/Library/Frameworks 35 | /Library/Frameworks 36 | /usr/local/lib 37 | /usr/local/lib64 38 | /usr/lib 39 | /usr/lib64 40 | /opt/local/lib 41 | /sw/local/lib 42 | /sw/lib 43 | ) 44 | 45 | FIND_LIBRARY(${MYLIBRARY} 46 | NAMES "g2o_${MYLIBRARYNAME}" 47 | PATHS 48 | ${G2O_ROOT}/lib/Release 49 | ${G2O_ROOT}/lib 50 | $ENV{G2O_ROOT}/lib/Release 51 | $ENV{G2O_ROOT}/lib 52 | NO_DEFAULT_PATH 53 | ) 54 | 55 | FIND_LIBRARY(${MYLIBRARY} 56 | NAMES "g2o_${MYLIBRARYNAME}" 57 | PATHS 58 | ~/Library/Frameworks 59 | /Library/Frameworks 60 | /usr/local/lib 61 | /usr/local/lib64 62 | /usr/lib 63 | /usr/lib64 64 | /opt/local/lib 65 | /sw/local/lib 66 | /sw/lib 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons) 103 | 104 | # G2O solvers declared found if we found at least one solver 105 | SET(G2O_SOLVERS_FOUND "NO") 106 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 107 | SET(G2O_SOLVERS_FOUND "YES") 108 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 109 | 110 | # G2O itself declared found if we found the core libraries and at least one solver 111 | SET(G2O_FOUND "NO") 112 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 113 | SET(G2O_FOUND "YES") 114 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 115 | -------------------------------------------------------------------------------- /config/bucket_detector.yaml: -------------------------------------------------------------------------------- 1 | #general params 2 | verbose: true 3 | camera_angle: 33.93 4 | update_key_using_det: true 5 | add_first_lan: true 6 | first_lan_x: 1.42 7 | first_lan_y: -0.028 8 | first_lan_z: 0.15 9 | #keyframe 10 | keyframe_delta_trans: 0.5 11 | keyframe_delta_angle: 0.5 12 | keyframe_delta_time: 1.0 13 | #plane segmentation 14 | num_point_seg: 100 15 | norm_point_thres: 1000 16 | planar_area: 0 17 | #data association related 18 | maha_dist_thres: 1.5 19 | use_maha_dist: false 20 | use_eq_dist: true 21 | eq_dist_thres: 1.5 22 | land_noise_low: 0.4 23 | land_noise_high: 0.4 24 | #odom information cal 25 | use_const_inf_matrix: true 26 | const_stddev_x: 0.00667 27 | const_stddev_q: 0.00001 28 | #graph slam related 29 | save_graph: false 30 | save_graph_path: /home/hriday/Desktop/semantic_graph.g2o 31 | compute_txt_for_ate: true 32 | 33 | -------------------------------------------------------------------------------- /config/bucket_detector_workspace.yaml: -------------------------------------------------------------------------------- 1 | #general params 2 | verbose: false 3 | camera_angle: 33.93 4 | update_key_using_det: true 5 | add_first_lan: false 6 | #keyframe 7 | keyframe_delta_trans: 0.5 8 | keyframe_delta_angle: 0.5 9 | keyframe_delta_time: 1.0 10 | #plane segmentation 11 | num_point_seg: 100 12 | norm_point_thres: 5000 13 | planar_area: 0.01 14 | #data association related 15 | maha_dist_thres: 1.5 16 | use_maha_dist: false 17 | use_eq_dist: true 18 | eq_dist_thres: 1.5 19 | land_noise_low: 0.1 20 | land_noise_high: 0.1 21 | #odom information cal 22 | use_const_inf_matrix: true 23 | const_stddev_x: 0.00667 24 | const_stddev_q: 0.00001 25 | #graph slam related 26 | save_graph: false 27 | save_graph_path: /home/hriday/Desktop/semantic_graph.g2o 28 | compute_txt_for_ate: false 29 | -------------------------------------------------------------------------------- /config/yolo_detector.yaml: -------------------------------------------------------------------------------- 1 | #general params 2 | verbose: false 3 | camera_angle: 0 4 | update_key_using_det: false 5 | add_first_lan: false 6 | #keyframe 7 | keyframe_delta_trans: 0.5 8 | keyframe_delta_angle: 0.5 9 | keyframe_delta_time: 1.0 10 | #plane segmentation 11 | num_point_seg: 500 12 | norm_point_thres: 5000 13 | planar_area: 0.20 14 | #data association related 15 | maha_dist_thres: 0.584 16 | use_maha_dist: true 17 | use_eq_dist: false 18 | eq_dist_thres: 1.5 19 | land_noise_low: 0.4 20 | land_noise_high: 0.4 21 | #odom information cal 22 | use_const_inf_matrix: true 23 | const_stddev_x: 0.00667 24 | const_stddev_q: 0.0001 25 | #graph slam related 26 | save_graph: false 27 | save_graph_path: /home/hriday/Desktop/semantic_graph.g2o 28 | compute_txt_for_ate: true 29 | -------------------------------------------------------------------------------- /config/yolo_detector_kitti.yaml: -------------------------------------------------------------------------------- 1 | #general params 2 | verbose: false 3 | camera_angle: 0.0 4 | update_key_using_det: true 5 | add_first_lan: false 6 | using_kitti: true 7 | #keyframe 8 | keyframe_delta_trans: 0.5 9 | keyframe_delta_angle: 0.5 10 | keyframe_delta_time: 1.0 11 | #plane segmentation 12 | num_point_seg: 100 13 | norm_point_thres: 100 14 | #data association related 15 | maha_dist_thres: 1.5 16 | use_maha_dist: false 17 | use_eq_dist: true 18 | eq_dist_thres: 1.5 19 | land_noise_low: 0.1 20 | land_noise_high: 0.1 21 | #odom information cal 22 | use_const_inf_matrix: true 23 | const_stddev_x: 0.00667 24 | const_stddev_q: 0.00001 25 | #graph slam related 26 | save_graph: false 27 | save_graph_path: /home/hriday/Desktop/semantic_graph.g2o 28 | -------------------------------------------------------------------------------- /config/yolo_detector_rotonda.yaml: -------------------------------------------------------------------------------- 1 | #general params 2 | verbose: false 3 | camera_angle: 0 4 | update_key_using_det: false 5 | add_first_lan: false 6 | use_rovio_odom: true 7 | use_rtab_map_odom: false 8 | #keyframe 9 | keyframe_delta_trans: 0.5 10 | keyframe_delta_angle: 0.5 11 | keyframe_delta_time: 1.0 12 | #plane segmentation 13 | num_point_seg: 500 14 | norm_point_thres: 5000 15 | planar_area: 0.1 16 | #data association related 17 | maha_dist_thres: 2.36 18 | use_maha_dist: true 19 | use_eq_dist: false 20 | eq_dist_thres: 1.5 21 | land_noise_low: 0.5 22 | land_noise_high: 0.5 23 | #odom information cal 24 | use_const_inf_matrix: true 25 | const_stddev_x: 0.00067 26 | const_stddev_q: 0.00001 27 | #graph slam related 28 | save_graph: false 29 | save_graph_path: /home/hriday/Desktop/semantic_graph.g2o 30 | compute_txt_for_ate: true 31 | -------------------------------------------------------------------------------- /include/g2o/edge_se3_plane.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KKL_G2O_EDGE_SE3_PLANE_HPP 2 | #define KKL_G2O_EDGE_SE3_PLANE_HPP 3 | #include 4 | #include 5 | #include 6 | 7 | namespace g2o { 8 | class EdgeSE3Plane : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, 9 | g2o::VertexPlane> { 10 | public: 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | EdgeSE3Plane() 13 | : BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() {} 14 | 15 | void computeError() override { 16 | const g2o::VertexSE3 *v1 = 17 | static_cast(_vertices[0]); 18 | const g2o::VertexPlane *v2 = 19 | static_cast(_vertices[1]); 20 | 21 | Eigen::Isometry3d w2n = v1->estimate().inverse(); 22 | Plane3D local_plane = w2n * v2->estimate(); 23 | _error = local_plane.ominus(_measurement); 24 | } 25 | 26 | void setMeasurement(const g2o::Plane3D &m) override { _measurement = m; } 27 | 28 | virtual bool read(std::istream &is) override { 29 | Eigen::Vector4d v; 30 | is >> v(0) >> v(1) >> v(2) >> v(3); 31 | setMeasurement(Plane3D(v)); 32 | for (int i = 0; i < information().rows(); ++i) 33 | for (int j = i; j < information().cols(); ++j) { 34 | is >> information()(i, j); 35 | if (i != j) 36 | information()(j, i) = information()(i, j); 37 | } 38 | return true; 39 | } 40 | virtual bool write(std::ostream &os) const override { 41 | Eigen::Vector4d v = _measurement.toVector(); 42 | os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " "; 43 | for (int i = 0; i < information().rows(); ++i) 44 | for (int j = i; j < information().cols(); ++j) 45 | os << " " << information()(i, j); 46 | return os.good(); 47 | } 48 | }; 49 | } // namespace g2o 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /include/planar_segmentation/detected_object.h: -------------------------------------------------------------------------------- 1 | #ifndef DETECTED_OBJECT_H 2 | #define DETECTED_OBJECT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "Eigen/Dense" 13 | 14 | struct detected_object { 15 | 16 | int id; 17 | float prob; 18 | float num_points; 19 | std::string type; 20 | std::string plane_type; 21 | Eigen::Vector3f pose; 22 | Eigen::Vector3f world_pose; 23 | Eigen::Vector4f normal_orientation; 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /include/planar_segmentation/plane_segmentation.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "semantic_SLAM/ObjectInfo.h" 8 | 9 | #include "sensor_msgs/PointCloud2.h" 10 | 11 | // ros 12 | #include "ros/ros.h" 13 | 14 | // PCL ROS 15 | #include 16 | #include 17 | 18 | // PCL 19 | #include "pcl/point_types.h" 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | // opencv 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | const int num_centroids_normals = 4; 42 | const int num_centroids_distance = 2; 43 | const int num_centroids_vert_dist = 1; 44 | const int num_centroids_pose = 2; 45 | 46 | class plane_segmentation { 47 | public: 48 | plane_segmentation(bool verbose); 49 | ~plane_segmentation(); 50 | 51 | private: 52 | bool verbose_; 53 | double num_points_seg_; 54 | double normal_point_thres_; 55 | double planar_area_; 56 | 57 | public: 58 | struct segmented_objects { 59 | std::string type; 60 | float prob; 61 | pcl::PointCloud::Ptr segmented_point_cloud; 62 | }; 63 | 64 | struct segmented_planes { 65 | cv::Mat final_pose_mat; 66 | pcl::PointCloud planar_points; 67 | }; 68 | 69 | plane_segmentation::segmented_objects 70 | segmentPointCloudData(semantic_SLAM::ObjectInfo object_info, 71 | sensor_msgs::PointCloud2 point_cloud, 72 | sensor_msgs::PointCloud2 &segmented_point_cloud); 73 | 74 | pcl::PointCloud::Ptr computeNormalsFromPointCloud( 75 | pcl::PointCloud::Ptr point_cloud, 76 | pcl::PointIndices::Ptr inliers); 77 | 78 | cv::Mat 79 | computeHorizontalPlane(pcl::PointCloud::Ptr point_cloud, 80 | pcl::PointCloud::Ptr point_normal, 81 | Eigen::Matrix4f transformation_mat, 82 | Eigen::MatrixXf final_pose, float &point_size, 83 | pcl::PointCloud::Ptr 84 | &segemented_plane_from_point_cloud); 85 | 86 | std::vector 87 | computeAllVerticalPlanes(pcl::PointCloud::Ptr point_cloud, 88 | pcl::PointCloud::Ptr point_normal, 89 | Eigen::Matrix4f transformation_mat, 90 | Eigen::MatrixXf final_pose, float &point_size, 91 | pcl::PointCloud::Ptr 92 | &segemented_plane_from_point_cloud); 93 | 94 | std::vector 95 | multiPlaneSegmentation(pcl::PointCloud::Ptr point_cloud, 96 | pcl::PointCloud::Ptr point_normal, 97 | pcl::PointIndices::Ptr inliers, 98 | Eigen::Matrix4f transformation_mat); 99 | 100 | std::vector 101 | clusterAndSegmentAllPlanes(pcl::PointCloud::Ptr point_cloud, 102 | pcl::PointCloud::Ptr point_normal, 103 | Eigen::Matrix4f transformation_mat); 104 | 105 | cv::Mat NormalBasedClusteringAndSegmentation( 106 | pcl::PointCloud::Ptr point_cloud, 107 | pcl::PointCloud::Ptr point_normal, 108 | Eigen::Matrix4f transformation_mat, 109 | std::vector::Ptr> 110 | &segmented_points_using_first_kmeans); 111 | 112 | std::vector 113 | distanceBasedSegmentation(std::vector::Ptr> 114 | segmented_points_using_first_kmeans, 115 | cv::Mat filtered_centroids, 116 | std::vector::Ptr> 117 | &final_segmented_vec_of_points_from_distances); 118 | 119 | std::vector 120 | getFinalPoseWithNormals(std::vector::Ptr> 121 | segmented_points_vec_using_second_kmeans, 122 | std::vector final_normals_with_distances); 123 | 124 | void removeNans(pcl::PointCloud::Ptr point_cloud, 125 | pcl::PointCloud::Ptr point_normal, 126 | cv::Mat &normal_filtered_points_mat, 127 | pcl::PointCloud::Ptr &points_without_nans); 128 | 129 | cv::Mat filterCentroids(cv::Mat centroids, 130 | Eigen::Vector4f normals_horizontal_plane_in_cam, 131 | std::vector &filtered_centroids_id); 132 | 133 | double computeKmeans(cv::Mat points, const int num_centroids, cv::Mat &labels, 134 | cv::Mat ¢roids); 135 | 136 | cv::Mat findNearestNeighbours(cv::Mat centroids); 137 | 138 | float computeDotProduct(Eigen::Vector4f vector_a, Eigen::Vector4f vector_b); 139 | 140 | std::vector 141 | computeAllPlanes(pcl::PointCloud::Ptr point_cloud, 142 | Eigen::Matrix4f transformation_mat); 143 | pcl::PointCloud::Ptr compute2DConvexHull( 144 | pcl::PointCloud::Ptr filtered_point_cloud); 145 | 146 | pcl::PointCloud::Ptr 147 | preprocessPointCloud(pcl::PointCloud::Ptr point_cloud, 148 | pcl::PointIndices::Ptr &inliers); 149 | 150 | pcl::PointCloud::Ptr 151 | downsamplePointcloud(pcl::PointCloud::Ptr point_cloud, 152 | pcl::PointIndices::Ptr &inliers); 153 | pcl::PointCloud::Ptr 154 | removeOutliers(pcl::PointCloud::Ptr point_cloud, 155 | pcl::PointIndices::Ptr &inliers); 156 | pcl::PointCloud::Ptr 157 | distance_filter(pcl::PointCloud::Ptr point_cloud); 158 | }; 159 | -------------------------------------------------------------------------------- /include/planar_segmentation/point_cloud_segmentation.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_CLOUD_SEGMENTATION 2 | #define POINT_CLOUD_SEGMENTATION 3 | 4 | #include "planar_segmentation/detected_object.h" 5 | #include "planar_segmentation/plane_segmentation.h" 6 | #include "tools.h" 7 | 8 | class point_cloud_segmentation { 9 | 10 | public: 11 | std::unique_ptr plane_seg_obj; 12 | bool verbose_; 13 | 14 | public: 15 | point_cloud_segmentation(bool verbose) { 16 | verbose_ = verbose; 17 | plane_seg_obj.reset(new plane_segmentation(verbose_)); 18 | std::cout << "pc segmentation constructor " << std::endl; 19 | } 20 | 21 | ~point_cloud_segmentation() { 22 | std::cout << "pc segmentation destructor " << std::endl; 23 | } 24 | 25 | public: 26 | std::vector segmentPlanarSurfaces( 27 | pcl::PointCloud::Ptr segmented_point_cloud, 28 | pcl::PointCloud::Ptr segmented_point_cloud_normal, 29 | pcl::PointIndices::Ptr inliers, Eigen::Matrix4f transformation_mat, 30 | Eigen::VectorXf robot_pose, std::string object_type, float prob) { 31 | std::vector complete_obj_info_vec; 32 | 33 | std::vector planar_surf_vec; 34 | planar_surf_vec.clear(); 35 | 36 | // ros::Time t1 = ros::Time::now(); 37 | planar_surf_vec = plane_seg_obj->multiPlaneSegmentation( 38 | segmented_point_cloud, segmented_point_cloud_normal, inliers, 39 | transformation_mat); 40 | 41 | if (!planar_surf_vec.empty()) { 42 | for (int j = 0; j < planar_surf_vec.size(); ++j) { 43 | Eigen::Vector4f final_detected_point_cam_frame, 44 | final_detected_point_world_frame; 45 | final_detected_point_cam_frame.setOnes(), 46 | final_detected_point_world_frame.setOnes(); 47 | 48 | final_detected_point_cam_frame(0) = 49 | planar_surf_vec[j].final_pose_mat.at(0, 0); 50 | final_detected_point_cam_frame(1) = 51 | planar_surf_vec[j].final_pose_mat.at(0, 1); 52 | final_detected_point_cam_frame(2) = 53 | planar_surf_vec[j].final_pose_mat.at(0, 2); 54 | 55 | final_detected_point_world_frame = 56 | transformation_mat * final_detected_point_cam_frame; 57 | 58 | float final_object_height; 59 | final_object_height = 60 | final_detected_point_world_frame(2) + robot_pose(2); 61 | 62 | // pose_vec.push_back(final_pose_of_object_in_robot); 63 | // do the same above procedure for the normal orientation 64 | Eigen::Vector4f normal_orientation_cam_frame; 65 | normal_orientation_cam_frame.setOnes(); 66 | 67 | normal_orientation_cam_frame(0) = 68 | planar_surf_vec[j].final_pose_mat.at(0, 3); 69 | normal_orientation_cam_frame(1) = 70 | planar_surf_vec[j].final_pose_mat.at(0, 4); 71 | normal_orientation_cam_frame(2) = 72 | planar_surf_vec[j].final_pose_mat.at(0, 5); 73 | normal_orientation_cam_frame(3) = 74 | planar_surf_vec[j].final_pose_mat.at(0, 6); 75 | 76 | detected_object complete_obj_info; 77 | 78 | // if its zeros its horizontal else its vertical 79 | if (planar_surf_vec[j].final_pose_mat.at(0, 7) == 0) 80 | complete_obj_info.plane_type = "horizontal"; 81 | else if (planar_surf_vec[j].final_pose_mat.at(0, 7) == 1) 82 | complete_obj_info.plane_type = "vertical"; 83 | 84 | complete_obj_info.type = object_type; 85 | complete_obj_info.prob = prob; 86 | complete_obj_info.num_points = planar_surf_vec[j].planar_points.size(); 87 | complete_obj_info.pose(0) = final_detected_point_cam_frame(0); 88 | complete_obj_info.pose(1) = final_detected_point_cam_frame(1); 89 | complete_obj_info.pose(2) = final_detected_point_cam_frame(2); 90 | complete_obj_info.normal_orientation = normal_orientation_cam_frame; 91 | complete_obj_info.world_pose 92 | << final_detected_point_world_frame(0) + robot_pose(0), 93 | final_detected_point_world_frame(1) + robot_pose(1), 94 | final_detected_point_world_frame(2) + robot_pose(2); 95 | // complete_obj_info.planar_points = 96 | // planar_surf_vec[j].planar_points; 97 | 98 | complete_obj_info_vec.push_back(complete_obj_info); 99 | } 100 | } 101 | 102 | return complete_obj_info_vec; 103 | } 104 | 105 | std::vector 106 | segmentallPointCloudData(Eigen::VectorXf robot_pose, float cam_angle, 107 | std::vector object_info, 108 | sensor_msgs::PointCloud2 point_cloud) { 109 | 110 | sensor_msgs::PointCloud2 segmented_point_cloud; 111 | std::vector 112 | segmented_objects_from_point_cloud; 113 | 114 | Eigen::Matrix4f transformation_mat; 115 | semantic_tools sem_tool_obj; 116 | sem_tool_obj.transformNormalsToWorld(robot_pose, transformation_mat, 117 | cam_angle); 118 | 119 | std::vector complete_obj_info_vec; 120 | complete_obj_info_vec.clear(); 121 | 122 | // This segments the PC according to the received bounding box data in the 123 | // 2D image 124 | segmented_objects_from_point_cloud.clear(); 125 | for (int i = 0; i < object_info.size(); ++i) { 126 | if (object_info[i].type == "chair" || 127 | object_info[i].type == "tvmonitor" || object_info[i].type == "book" || 128 | object_info[i].type == "keyboard" || 129 | object_info[i].type == "laptop" || object_info[i].type == "bucket" || 130 | object_info[i].type == "car") { 131 | plane_segmentation::segmented_objects 132 | single_segmented_object_from_point_cloud; 133 | single_segmented_object_from_point_cloud = 134 | plane_seg_obj->segmentPointCloudData(object_info[i], point_cloud, 135 | segmented_point_cloud); 136 | 137 | if (single_segmented_object_from_point_cloud.type != "spurious") 138 | segmented_objects_from_point_cloud.push_back( 139 | single_segmented_object_from_point_cloud); 140 | } 141 | } 142 | 143 | for (int i = 0; i < segmented_objects_from_point_cloud.size(); ++i) { 144 | 145 | if (!segmented_objects_from_point_cloud[i] 146 | .segmented_point_cloud->empty()) { 147 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 148 | pcl::PointCloud::Ptr cloud_filtered( 149 | new pcl::PointCloud); 150 | // cloud_filtered = 151 | // plane_segmentation_obj_.preprocessPointCloud(segmented_objects_from_point_cloud[i].segmented_point_cloud, 152 | // inliers); 153 | 154 | // This calculates the normals of the segmented pointcloud 155 | pcl::PointCloud::Ptr segmented_point_cloud_normal( 156 | new pcl::PointCloud); 157 | segmented_point_cloud_normal = 158 | plane_seg_obj->computeNormalsFromPointCloud( 159 | segmented_objects_from_point_cloud[i].segmented_point_cloud, 160 | inliers); 161 | 162 | if (segmented_point_cloud_normal->empty()) 163 | continue; 164 | 165 | std::vector current_obj_info_vec; 166 | current_obj_info_vec.clear(); 167 | // this computes all the planar surfaces from the detected segmented 168 | // data 169 | current_obj_info_vec = segmentPlanarSurfaces( 170 | segmented_objects_from_point_cloud[i].segmented_point_cloud, 171 | segmented_point_cloud_normal, inliers, transformation_mat, 172 | robot_pose, segmented_objects_from_point_cloud[i].type, 173 | segmented_objects_from_point_cloud[i].prob); 174 | 175 | for (int j = 0; j < current_obj_info_vec.size(); ++j) 176 | complete_obj_info_vec.push_back(current_obj_info_vec[j]); 177 | } 178 | } 179 | 180 | return complete_obj_info_vec; 181 | } 182 | }; 183 | 184 | #endif 185 | -------------------------------------------------------------------------------- /include/ps_graph_slam/data_association.h: -------------------------------------------------------------------------------- 1 | #ifndef DATA_ASSOCIATION 2 | #define DATA_ASSOCIATION 3 | 4 | #include "ros/ros.h" 5 | 6 | #include "eigen3/Eigen/Eigen" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "planar_segmentation/detected_object.h" 16 | #include "ps_graph_slam/landmark.h" 17 | #include "tools.h" 18 | 19 | class data_association { 20 | 21 | public: 22 | data_association(bool verbose) { 23 | std::cout << "data association constructor " << std::endl; 24 | verbose_ = verbose; 25 | init(); 26 | } 27 | ~data_association() { 28 | std::cout << "data association destructor " << std::endl; 29 | } 30 | 31 | private: 32 | bool verbose_; 33 | bool first_object_; 34 | std::vector landmarks_; 35 | Eigen::Matrix3f Q_; 36 | double maha_dist_thres_, eq_dist_thres_; 37 | semantic_tools sem_tool_obj_; 38 | double land_noise_low_, land_noise_high_; 39 | bool use_maha_dist_, use_eq_dist_; 40 | bool eq_dist_; 41 | bool use_rtab_map_odom_; 42 | 43 | void init() { 44 | first_object_ = true; 45 | use_maha_dist_ = true; 46 | landmarks_.clear(); 47 | Q_.setZero(); 48 | 49 | ros::param::param("~maha_dist_thres", maha_dist_thres_, 0.5); 50 | ros::param::param("~eq_dist_thres", eq_dist_thres_, 1.21); 51 | ros::param::param("~land_noise_low", land_noise_low_, 0.5); 52 | ros::param::param("~land_noise_high", land_noise_high_, 0.9); 53 | ros::param::param("~use_maha_dist", use_maha_dist_, true); 54 | ros::param::param("~use_eq_dist", use_eq_dist_, false); 55 | ros::param::param("~use_rtab_map_odom", use_rtab_map_odom_, false); 56 | 57 | std::cout << "land_noise_low: " << land_noise_low_ << std::endl; 58 | std::cout << "land_noise_high: " << land_noise_high_ << std::endl; 59 | std::cout << "use_maha_dist: " << use_maha_dist_ << std::endl; 60 | std::cout << "use_eq_dist: " << use_eq_dist_ << std::endl; 61 | std::cout << "maha dist thres: " << maha_dist_thres_ << std::endl; 62 | std::cout << "eq dist thres: " << eq_dist_thres_ << std::endl; 63 | 64 | Q_(0, 0) = land_noise_low_; 65 | Q_(1, 1) = land_noise_low_; 66 | Q_(2, 2) = land_noise_low_; 67 | } 68 | 69 | public: 70 | void addFirstLandmark(landmark l) { 71 | first_object_ = false; 72 | landmarks_.push_back(l); 73 | } 74 | 75 | std::vector find_matches(std::vector seg_obj_info, 76 | Eigen::VectorXf robot_pose, 77 | float cam_angle) { 78 | std::vector landmark_vec; 79 | if (first_object_) { 80 | for (int i = 0; i < seg_obj_info.size(); ++i) 81 | landmark_vec.push_back( 82 | this->map_a_new_lan(seg_obj_info[i], robot_pose, cam_angle)); 83 | ; 84 | if (landmark_vec.size() > 0) 85 | first_object_ = false; 86 | } else { 87 | landmark_vec = 88 | this->associate_lanmarks(seg_obj_info, robot_pose, cam_angle); 89 | } 90 | 91 | if (verbose_) 92 | std::cout << "landmarks size " << landmarks_.size() << std::endl; 93 | 94 | return landmark_vec; 95 | } 96 | 97 | std::vector 98 | associate_lanmarks(std::vector seg_obj_info, 99 | Eigen::VectorXf robot_pose, float cam_angle) { 100 | bool found_nearest_neighbour = false; 101 | float distance = 0; 102 | float distance_min = std::numeric_limits::max(); 103 | 104 | std::vector landmark_vec; 105 | 106 | for (int j = 0; j < seg_obj_info.size(); ++j) { 107 | int neareast_landmarks_id; 108 | 109 | // if(seg_obj_info[j].num_points < 300) 110 | // { 111 | // Q_(0,0) = land_noise_high_; 112 | // Q_(1,1) = land_noise_high_; 113 | // Q_(2,2) = land_noise_high_; 114 | // } 115 | // else 116 | // { 117 | // Q_(0,0) = land_noise_low_; 118 | // Q_(1,1) = land_noise_low_; 119 | // Q_(2,2) = land_noise_low_; 120 | // } 121 | 122 | for (int i = 0; i < landmarks_.size(); ++i) { 123 | if (seg_obj_info[j].type == landmarks_[i].type) { 124 | if (seg_obj_info[j].plane_type == landmarks_[i].plane_type) { 125 | 126 | // transform the detected object pose to world frame 127 | Eigen::Vector4f obj_pose_cam; 128 | obj_pose_cam.setOnes(); 129 | 130 | obj_pose_cam << seg_obj_info[j].pose(0), seg_obj_info[j].pose(1), 131 | seg_obj_info[j].pose(2); 132 | 133 | Eigen::Vector4f obj_pose_world = 134 | this->convertPoseToWorld(robot_pose, cam_angle, obj_pose_cam); 135 | 136 | // transform detected normals in world 137 | Eigen::Vector4f obj_normals_world = this->convertNormalsToWorld( 138 | robot_pose, cam_angle, seg_obj_info[j].normal_orientation); 139 | 140 | // if(fabs(obj_normals_world(0) - 141 | // landmarks_[i].normal_orientation(0)) < 0.1 142 | // && 143 | // fabs(obj_normals_world(1) - 144 | // landmarks_[i].normal_orientation(1)) 145 | // < 0.1 && fabs(obj_normals_world(2) 146 | // - 147 | // landmarks_[i].normal_orientation(2)) 148 | // < 0.1) 149 | { 150 | 151 | found_nearest_neighbour = true; 152 | 153 | Eigen::VectorXf expected_meas; 154 | Eigen::MatrixXf H = 155 | this->landmarkMeasurementModel(landmarks_[i], expected_meas); 156 | 157 | Eigen::VectorXf actual_meas; 158 | actual_meas.resize(3); 159 | actual_meas = obj_pose_world; 160 | 161 | if (verbose_) { 162 | std::cout << "actual meas: " << actual_meas << std::endl; 163 | std::cout << "expected meas: " << expected_meas << std::endl; 164 | } 165 | 166 | // sigma recovered from graph slam optimization 167 | if (use_maha_dist_) { 168 | Eigen::MatrixXf sigma; 169 | sigma.resize(3, 3); 170 | sigma = landmarks_[i].covariance; 171 | if (verbose_) 172 | std::cout << "landmark: " << i << "sigma: " << sigma 173 | << std::endl; 174 | 175 | Eigen::MatrixXf Q = H * sigma * H.transpose() + Q_; 176 | 177 | Eigen::VectorXf z_diff; 178 | z_diff.resize(3); 179 | z_diff = actual_meas - expected_meas; 180 | 181 | if (verbose_) 182 | std::cout << "z_diff: " << z_diff << std::endl; 183 | 184 | distance = z_diff.transpose() * Q.inverse() * z_diff; 185 | if (verbose_) 186 | std::cout << "cov maha distance: " << distance << std::endl; 187 | } else if (use_eq_dist_) { 188 | distance = sem_tool_obj_.dist(actual_meas(0), expected_meas(0), 189 | actual_meas(1), expected_meas(1), 190 | actual_meas(2), expected_meas(2)); 191 | if (verbose_) 192 | std::cout << "euclidean distance: " << distance << std::endl; 193 | } 194 | 195 | if (distance < distance_min) { 196 | distance_min = distance; 197 | neareast_landmarks_id = i; 198 | } 199 | } 200 | } 201 | } 202 | } 203 | 204 | // if the type of the object did not get matched 205 | if (!found_nearest_neighbour) { 206 | landmark_vec.push_back( 207 | this->map_a_new_lan(seg_obj_info[j], robot_pose, cam_angle)); 208 | } else if (found_nearest_neighbour) { 209 | found_nearest_neighbour = false; 210 | 211 | if (verbose_) 212 | std::cout << "distance_min: " << distance_min << std::endl; 213 | 214 | if (use_maha_dist_) { 215 | if (distance_min > maha_dist_thres_) 216 | landmark_vec.push_back( 217 | this->map_a_new_lan(seg_obj_info[j], robot_pose, cam_angle)); 218 | else 219 | landmark_vec.push_back(this->inserst_a_mapped_lan( 220 | seg_obj_info[j], robot_pose, cam_angle, 221 | landmarks_[neareast_landmarks_id])); 222 | } else if (use_eq_dist_) { 223 | if (distance_min > eq_dist_thres_) 224 | landmark_vec.push_back( 225 | this->map_a_new_lan(seg_obj_info[j], robot_pose, cam_angle)); 226 | else 227 | landmark_vec.push_back(this->inserst_a_mapped_lan( 228 | seg_obj_info[j], robot_pose, cam_angle, 229 | landmarks_[neareast_landmarks_id])); 230 | } 231 | } 232 | } 233 | 234 | return landmark_vec; 235 | } 236 | 237 | landmark map_a_new_lan(detected_object seg_obj_info, 238 | Eigen::VectorXf robot_pose, float cam_angle) { 239 | 240 | // transform the detected object pose to world frame 241 | Eigen::Vector4f obj_pose_cam; 242 | obj_pose_cam.setOnes(); 243 | 244 | obj_pose_cam << seg_obj_info.pose(0), seg_obj_info.pose(1), 245 | seg_obj_info.pose(2); 246 | 247 | Eigen::Vector4f obj_pose_world = 248 | this->convertPoseToWorld(robot_pose, cam_angle, obj_pose_cam); 249 | 250 | // transform detected normals in world 251 | Eigen::Vector4f obj_normals_world = this->convertNormalsToWorld( 252 | robot_pose, cam_angle, seg_obj_info.normal_orientation); 253 | 254 | // obj pose from cam to robot 255 | Eigen::Vector4f obj_pose_rob = 256 | this->convertCamToRobot(robot_pose, cam_angle, obj_pose_cam); 257 | 258 | landmark new_landmark; 259 | new_landmark.is_new_landmark = true; 260 | new_landmark.id = landmarks_.size(); 261 | new_landmark.local_pose << obj_pose_rob(0), obj_pose_rob(1), 262 | obj_pose_rob(2); 263 | new_landmark.pose << obj_pose_world(0), obj_pose_world(1), 264 | obj_pose_world(2); 265 | new_landmark.covariance = Q_; 266 | new_landmark.normal_orientation = obj_normals_world; 267 | new_landmark.plane_type = seg_obj_info.plane_type; 268 | new_landmark.type = seg_obj_info.type; 269 | landmarks_.push_back(new_landmark); 270 | 271 | if (verbose_) 272 | std::cout << "\033[1;31m new landmark pose \033[0m\n" 273 | << new_landmark.pose << std::endl; 274 | 275 | return new_landmark; 276 | } 277 | 278 | landmark inserst_a_mapped_lan(detected_object seg_obj_info, 279 | Eigen::VectorXf robot_pose, float cam_angle, 280 | landmark l) { 281 | 282 | // transform the detected object pose to world frame 283 | Eigen::Vector4f obj_pose_cam; 284 | obj_pose_cam.setOnes(); 285 | 286 | obj_pose_cam << seg_obj_info.pose(0), seg_obj_info.pose(1), 287 | seg_obj_info.pose(2); 288 | 289 | Eigen::Vector4f obj_pose_world = 290 | this->convertPoseToWorld(robot_pose, cam_angle, obj_pose_cam); 291 | 292 | // transform detected normals in world 293 | Eigen::Vector4f obj_normals_world = this->convertNormalsToWorld( 294 | robot_pose, cam_angle, seg_obj_info.normal_orientation); 295 | 296 | // obj pose from cam to robot 297 | Eigen::Vector4f obj_pose_rob = 298 | this->convertCamToRobot(robot_pose, cam_angle, obj_pose_cam); 299 | 300 | landmark landmark; 301 | landmark.is_new_landmark = false; 302 | landmark.id = l.id; 303 | landmark.local_pose << obj_pose_rob(0), obj_pose_rob(1), obj_pose_rob(2); 304 | landmark.pose << obj_pose_world(0), obj_pose_world(1), obj_pose_world(2); 305 | landmark.covariance = Q_; 306 | landmark.normal_orientation = obj_normals_world; 307 | landmark.plane_type = seg_obj_info.plane_type; 308 | landmark.type = seg_obj_info.type; 309 | landmark.node = l.node; 310 | 311 | if (verbose_) 312 | std::cout << "\033[1;34m matched landmark \033[0m " << landmark.pose 313 | << " " 314 | << "\033[1;34m with mapped landmark \033[0m " << l.pose 315 | << std::endl; 316 | 317 | return landmark; 318 | } 319 | 320 | Eigen::VectorXf convertPoseToWorld(Eigen::VectorXf robot_pose, 321 | float cam_angle, 322 | Eigen::VectorXf pose_to_transform) { 323 | 324 | Eigen::Matrix4f transformation_mat; 325 | sem_tool_obj_.transformNormalsToWorld(robot_pose, transformation_mat, 326 | cam_angle); 327 | 328 | Eigen::VectorXf transformed_pose; 329 | transformed_pose.resize(4); 330 | 331 | transformed_pose = transformation_mat * pose_to_transform; 332 | 333 | // adding the robots pose 334 | transformed_pose(0) += robot_pose(0); 335 | if (!use_rtab_map_odom_) 336 | transformed_pose(1) += robot_pose(1); 337 | else 338 | transformed_pose(1) += robot_pose(1) - 0.04; 339 | 340 | transformed_pose(2) += robot_pose(2); 341 | 342 | return transformed_pose; 343 | } 344 | 345 | Eigen::VectorXf convertNormalsToWorld(Eigen::VectorXf robot_pose, 346 | float cam_angle, 347 | Eigen::VectorXf pose_to_transform) { 348 | 349 | Eigen::Matrix4f transformation_mat; 350 | sem_tool_obj_.transformNormalsToWorld(robot_pose, transformation_mat, 351 | cam_angle); 352 | 353 | Eigen::VectorXf transformed_pose; 354 | transformed_pose.resize(4); 355 | 356 | transformed_pose = transformation_mat * pose_to_transform; 357 | 358 | return transformed_pose; 359 | } 360 | 361 | Eigen::VectorXf convertCamToRobot(Eigen::VectorXf robot_pose, float cam_angle, 362 | Eigen::VectorXf pose_to_transform) { 363 | 364 | Eigen::Matrix4f transformation_mat; 365 | sem_tool_obj_.transformPoseFromCameraToRobot(transformation_mat, cam_angle); 366 | 367 | Eigen::VectorXf transformed_pose; 368 | transformed_pose.resize(4); 369 | 370 | transformed_pose = transformation_mat * pose_to_transform; 371 | 372 | return transformed_pose; 373 | } 374 | 375 | Eigen::MatrixXf landmarkMeasurementModel(landmark l, Eigen::VectorXf &h) { 376 | h.resize(3, 3); 377 | h.setZero(); 378 | h = l.node->estimate().cast(); 379 | 380 | Eigen::MatrixXf H; 381 | H.resize(3, 3); 382 | H.setZero(); 383 | 384 | H(0, 0) = 1; 385 | H(1, 1) = 1; 386 | H(2, 2) = 1; 387 | 388 | return H; 389 | } 390 | 391 | void assignLandmarkNode(int id, g2o::VertexPointXYZ *node) { 392 | landmarks_[id].node = node; 393 | } 394 | 395 | inline void setLandmarkCovs(int id, Eigen::Matrix3f cov) { 396 | landmarks_[id].covariance = cov; 397 | } 398 | 399 | void getMappedLandmarks(std::vector &l) { l = landmarks_; } 400 | }; 401 | 402 | #endif 403 | -------------------------------------------------------------------------------- /include/ps_graph_slam/graph_slam.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GRAPH_SLAM_HPP 2 | #define GRAPH_SLAM_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace g2o { 13 | class VertexSE3; 14 | class VertexPointXYZ; 15 | class EdgeSE3; 16 | class EdgeSE3PointXYZ; 17 | class EdgePointXYZ; 18 | class VertexPlane; 19 | // class EdgeXYZPlane; 20 | // class EdgeSE3Plane; 21 | // class EdgeSE3PriorXY; 22 | // class EdgeSE3PriorXYZ; 23 | } // namespace g2o 24 | 25 | namespace ps_graph_slam { 26 | 27 | class GraphSLAM { 28 | public: 29 | GraphSLAM(bool verbose); 30 | ~GraphSLAM(); 31 | 32 | /** 33 | * @brief add a SE3 node to the graph 34 | * @param pose 35 | * @return registered node 36 | */ 37 | g2o::VertexSE3 *add_se3_node(const Eigen::Isometry3d &pose); 38 | 39 | /** 40 | * @brief add a plane node to the graph 41 | * @param plane_coeffs 42 | * @return registered node 43 | */ 44 | // g2o::VertexPlane* add_plane_node(const Eigen::Vector4d& plane_coeffs); 45 | 46 | /** 47 | * @brief add a point_xyz node to the graph 48 | * @param xyz 49 | * @return registered node 50 | */ 51 | g2o::VertexPointXYZ *add_point_xyz_node(const Eigen::Vector3d &xyz); 52 | 53 | /** 54 | * @brief add an edge between SE3 nodes 55 | * @param v1 node1 56 | * @param v2 node2 57 | * @param relative_pose relative pose between node1 and node2 58 | * @param information_matrix information matrix (it must be 6x6) 59 | * @return registered edge 60 | */ 61 | g2o::EdgeSE3 *add_se3_edge(g2o::VertexSE3 *v1, g2o::VertexSE3 *v2, 62 | const Eigen::Isometry3d &relative_pose, 63 | const Eigen::MatrixXd &information_matrix); 64 | 65 | /** 66 | * @brief add an edge between an SE3 node and a plane node 67 | * @param v_se3 SE3 node 68 | * @param v_plane plane node 69 | * @param plane_coeffs plane coefficients w.r.t. v_se3 70 | * @param information_matrix information matrix (it must be 3x3) 71 | * @return registered edge 72 | */ 73 | // g2o::EdgeSE3Plane* add_se3_plane_edge(g2o::VertexSE3* v_se3, 74 | // g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const 75 | // Eigen::MatrixXd& information_matrix); 76 | 77 | /** 78 | * @brief add an edge between an SE3 node and a plane node 79 | * @param v_xyz xyz node 80 | * @param v_plane plane node 81 | * @param plane_coeffs plane coefficients w.r.t. v_xyz 82 | * @param information_matrix information matrix (it must be 3x3) 83 | * @return registered edge 84 | */ 85 | // g2o::EdgeXYZPlane* add_xyz_plane_edge(g2o::VertexPointXYZ* v_se3, 86 | // g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const 87 | // Eigen::MatrixXd& information_matrix); 88 | 89 | /** 90 | * @brief add an edge between an SE3 node and a point_xyz node 91 | * @param v_se3 SE3 node 92 | * @param v_xyz point_xyz node 93 | * @param xyz xyz coordinate 94 | * @param information information_matrix (it must be 3x3) 95 | * @return registered edge 96 | */ 97 | g2o::EdgeSE3PointXYZ * 98 | add_se3_point_xyz_edge(g2o::VertexSE3 *v_se3, g2o::VertexPointXYZ *v_xyz, 99 | const Eigen::Vector3d &xyz, 100 | const Eigen::MatrixXd &information_matrix); 101 | 102 | /** 103 | * @brief add an edge between two point_xyz nodes 104 | * @param v1_xyz point_xyz node 105 | * @param v2_xyz point_xyz node 106 | * @param xyz xyz coordinate 107 | * @param information information_matrix (it must be 3x3) 108 | * @return registered edge 109 | */ 110 | 111 | g2o::EdgePointXYZ *add_point_xyz_point_xyz_edge( 112 | g2o::VertexPointXYZ *v1_xyz, g2o::VertexPointXYZ *v2_xyz, 113 | const Eigen::Vector3d &xyz, const Eigen::MatrixXd &information_matrix); 114 | 115 | /** 116 | * @brief add a prior edge to an SE3 node 117 | * @param v_se3 118 | * @param xy 119 | * @param information_matrix 120 | * @return 121 | */ 122 | // g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const 123 | // Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix); 124 | 125 | // g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const 126 | // Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); 127 | 128 | /** 129 | * @brief perform graph optimization 130 | */ 131 | bool optimize(); 132 | 133 | /** 134 | * @brief get the cov of the landmarks 135 | */ 136 | bool 137 | computeLandmarkMarginals(g2o::SparseBlockMatrix &spinv, 138 | std::vector> vert_pairs_vec); 139 | 140 | /** 141 | * @brief save the pose graph 142 | * @param filename output filename 143 | */ 144 | void save(const std::string &filename); 145 | 146 | public: 147 | std::shared_ptr graph; // g2o graph 148 | bool verbose_; 149 | // g2o::VertexPlane* floor_plane_node; // ground floor plane node 150 | }; 151 | 152 | } // namespace ps_graph_slam 153 | 154 | #endif // GRAPH_SLAM_HPP 155 | -------------------------------------------------------------------------------- /include/ps_graph_slam/information_matrix_calculator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INFORMATION_MATRIX_CALCULATOR_HPP 2 | #define INFORMATION_MATRIX_CALCULATOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace ps_graph_slam { 9 | 10 | class InformationMatrixCalculator { 11 | public: 12 | using PointT = pcl::PointXYZRGB; 13 | 14 | InformationMatrixCalculator(); 15 | ~InformationMatrixCalculator(); 16 | 17 | Eigen::MatrixXd calc_information_matrix() const; 18 | 19 | private: 20 | double weight(double a, double max_x, double min_y, double max_y, 21 | double x) const { 22 | double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x)); 23 | return min_y + (max_y - min_y) * y; 24 | } 25 | 26 | private: 27 | bool use_const_inf_matrix; 28 | double const_stddev_x; 29 | double const_stddev_q; 30 | 31 | double var_gain_a; 32 | double min_stddev_x; 33 | double max_stddev_x; 34 | double min_stddev_q; 35 | double max_stddev_q; 36 | double fitness_score_thresh; 37 | }; 38 | 39 | } // namespace ps_graph_slam 40 | 41 | #endif // INFORMATION_MATRIX_CALCULATOR_HPP 42 | -------------------------------------------------------------------------------- /include/ps_graph_slam/keyframe.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KEYFRAME_HPP 2 | #define KEYFRAME_HPP 3 | 4 | #include "semantic_SLAM/DetectedObjects.h" 5 | #include "sensor_msgs/PointCloud2.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace g2o { 13 | class VertexSE3; 14 | } 15 | 16 | namespace ps_graph_slam { 17 | 18 | /** 19 | * @brief KeyFrame (pose node) 20 | */ 21 | struct KeyFrame { 22 | public: 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 24 | using PointT = pcl::PointXYZRGB; 25 | using Ptr = std::shared_ptr; 26 | 27 | KeyFrame(const ros::Time &stamp, const Eigen::Isometry3d &odom, 28 | const Eigen::Isometry3d &robot_pose, const Eigen::MatrixXf &odom_cov, 29 | double accum_distance, const sensor_msgs::PointCloud2 &cloud_msg, 30 | const pcl::PointCloud::Ptr &cloud, 31 | std::vector &obj_info); 32 | ~KeyFrame(); 33 | 34 | void dump(const std::string &directory); 35 | 36 | public: 37 | ros::Time stamp; // timestamp 38 | Eigen::Isometry3d odom; // odometry (estimated by scan_matching_odometry) 39 | Eigen::Isometry3d robot_pose; // corrected pose of the robot 40 | Eigen::MatrixXf odom_cov; // odometry covariance 41 | double accum_distance; // accumulated distance from the first node (by 42 | // scan_matching_odometry) 43 | const sensor_msgs::PointCloud2 cloud_msg; // point cloud ros msg 44 | const pcl::PointCloud::Ptr cloud; // pcl point cloud 45 | std::vector obj_info; // the detected b 46 | 47 | g2o::VertexSE3 *node; // node instance 48 | }; 49 | 50 | /** 51 | * @brief KeyFramesnapshot for map cloud generation 52 | */ 53 | struct KeyFrameSnapshot { 54 | public: 55 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 56 | 57 | using PointT = KeyFrame::PointT; 58 | using Ptr = std::shared_ptr; 59 | 60 | KeyFrameSnapshot(const KeyFrame::Ptr &key); 61 | KeyFrameSnapshot(const Eigen::Isometry3d &pose, 62 | const pcl::PointCloud::ConstPtr &cloud); 63 | 64 | ~KeyFrameSnapshot(); 65 | 66 | public: 67 | Eigen::Isometry3d pose; // pose estimated by graph optimization 68 | pcl::PointCloud::ConstPtr cloud; // point cloud 69 | }; 70 | 71 | } // namespace ps_graph_slam 72 | 73 | #endif // KEYFRAME_HPP 74 | -------------------------------------------------------------------------------- /include/ps_graph_slam/keyframe_updater.hpp: -------------------------------------------------------------------------------- 1 | #ifndef KEYFRAME_UPDATER_HPP 2 | #define KEYFRAME_UPDATER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace ps_graph_slam { 8 | 9 | /** 10 | * @brief this class decides if a new frame should be registered to the pose 11 | * graph as a keyframe 12 | */ 13 | class KeyframeUpdater { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | /** 18 | * @brief constructor 19 | * @param pnh 20 | */ 21 | KeyframeUpdater() 22 | : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()) { 23 | ros::param::param("~keyframe_delta_trans", keyframe_delta_trans, 24 | 0.5); 25 | ros::param::param("~keyframe_delta_angle", keyframe_delta_angle, 26 | 0.5); 27 | ros::param::param("~keyframe_delta_time", keyframe_delta_time, 1); 28 | 29 | std::cout << "keyframe_delta_trans " << keyframe_delta_trans << std::endl; 30 | std::cout << "keyframe_delta_angle " << keyframe_delta_angle << std::endl; 31 | std::cout << "keyframe_delta_time " << keyframe_delta_time << std::endl; 32 | 33 | accum_distance = 0.0; 34 | } 35 | 36 | /** 37 | * @brief decide if a new frame should be registered to the graph 38 | * @param pose pose of the frame 39 | * @return if true, the frame should be registered 40 | */ 41 | bool update(const Eigen::Isometry3d &pose, ros::Time current_time) { 42 | // first frame is always registered to the graph 43 | if (is_first) { 44 | is_first = false; 45 | prev_time = current_time; 46 | prev_keypose = pose; 47 | return true; 48 | } 49 | 50 | // calculate the delta transformation from the previous keyframe 51 | Eigen::Isometry3d delta = prev_keypose.inverse() * pose; 52 | double dx = delta.translation().norm(); 53 | double da = std::acos(Eigen::Quaterniond(delta.linear()).w()); 54 | 55 | // updating based on time 56 | if ((current_time - prev_time).sec < keyframe_delta_time && 57 | dx < keyframe_delta_trans && da < keyframe_delta_angle) { 58 | return false; 59 | } 60 | 61 | accum_distance += dx; 62 | prev_keypose = pose; 63 | prev_time = current_time; 64 | return true; 65 | } 66 | 67 | /** 68 | * @brief the last keyframe's accumulated distance from the first keyframe 69 | * @return accumulated distance 70 | */ 71 | double get_accum_distance() const { return accum_distance; } 72 | 73 | private: 74 | // parameters 75 | double keyframe_delta_trans; // 76 | double keyframe_delta_angle; // 77 | double keyframe_delta_time; 78 | 79 | bool is_first; 80 | double accum_distance; 81 | Eigen::Isometry3d prev_keypose; 82 | ros::Time prev_time; 83 | }; 84 | 85 | } // namespace ps_graph_slam 86 | 87 | #endif // KEYFRAME_UPDATOR_HPP 88 | -------------------------------------------------------------------------------- /include/ps_graph_slam/landmark.h: -------------------------------------------------------------------------------- 1 | #ifndef LANDMARK 2 | #define LANDMARK 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "Eigen/Dense" 13 | #include "ps_graph_slam/keyframe.hpp" 14 | #include 15 | 16 | struct landmark { 17 | 18 | public: 19 | int id; 20 | int vert_id; 21 | Eigen::Vector3f pose; 22 | Eigen::Vector3f local_pose; 23 | Eigen::Matrix3f covariance; 24 | bool is_new_landmark; 25 | std::string type; 26 | std::string plane_type; 27 | Eigen::Vector4f normal_orientation; 28 | // pcl::PointCloud mapped_planar_points; 29 | float prob; 30 | 31 | g2o::VertexPointXYZ *node; 32 | }; 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /include/ps_graph_slam/ros_time_hash.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROS_TIME_HASH_HPP 2 | #define ROS_TIME_HASH_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | /** 10 | * @brief Hash calculation for ros::Time 11 | */ 12 | class RosTimeHash { 13 | public: 14 | size_t operator()(const ros::Time &val) const { 15 | size_t seed = 0; 16 | boost::hash_combine(seed, val.sec); 17 | boost::hash_combine(seed, val.nsec); 18 | return seed; 19 | } 20 | }; 21 | 22 | #endif // ROS_TIME_HASHER_HPP 23 | -------------------------------------------------------------------------------- /include/ps_graph_slam/ros_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROS_UTILS_HPP 2 | #define ROS_UTILS_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace ps_graph_slam { 12 | 13 | /** 14 | * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped 15 | * @param stamp timestamp 16 | * @param pose Eigen::Matrix to be converted 17 | * @param frame_id tf frame_id 18 | * @param child_frame_id tf child frame_id 19 | * @return converted TransformStamped 20 | */ 21 | static geometry_msgs::TransformStamped 22 | matrix2transform(const ros::Time &stamp, const Eigen::Matrix4f &pose, 23 | const std::string &frame_id, 24 | const std::string &child_frame_id) { 25 | Eigen::Quaternionf quat(pose.block<3, 3>(0, 0)); 26 | quat.normalize(); 27 | geometry_msgs::Quaternion odom_quat; 28 | odom_quat.w = quat.w(); 29 | odom_quat.x = quat.x(); 30 | odom_quat.y = quat.y(); 31 | odom_quat.z = quat.z(); 32 | 33 | geometry_msgs::TransformStamped odom_trans; 34 | odom_trans.header.stamp = stamp; 35 | odom_trans.header.frame_id = frame_id; 36 | odom_trans.child_frame_id = child_frame_id; 37 | 38 | odom_trans.transform.translation.x = pose(0, 3); 39 | odom_trans.transform.translation.y = pose(1, 3); 40 | odom_trans.transform.translation.z = pose(2, 3); 41 | odom_trans.transform.rotation = odom_quat; 42 | 43 | return odom_trans; 44 | } 45 | 46 | static geometry_msgs::Pose matrix2pose(const ros::Time &stamp, 47 | const Eigen::Matrix4f &pose, 48 | std::string frame_id) { 49 | Eigen::Quaternionf quat(pose.block<3, 3>(0, 0)); 50 | quat.normalize(); 51 | geometry_msgs::Quaternion odom_quat; 52 | odom_quat.w = quat.w(); 53 | odom_quat.x = quat.x(); 54 | odom_quat.y = quat.y(); 55 | odom_quat.z = quat.z(); 56 | 57 | geometry_msgs::Pose odom_pose; 58 | 59 | odom_pose.position.x = pose(0, 3); 60 | odom_pose.position.y = pose(1, 3); 61 | odom_pose.position.z = pose(2, 3); 62 | odom_pose.orientation = odom_quat; 63 | 64 | return odom_pose; 65 | } 66 | 67 | static geometry_msgs::PoseStamped 68 | matrix2posestamped(const ros::Time &stamp, const Eigen::Matrix4f &pose, 69 | std::string frame_id) { 70 | Eigen::Quaternionf quat(pose.block<3, 3>(0, 0)); 71 | quat.normalize(); 72 | geometry_msgs::Quaternion odom_quat; 73 | odom_quat.w = quat.w(); 74 | odom_quat.x = quat.x(); 75 | odom_quat.y = quat.y(); 76 | odom_quat.z = quat.z(); 77 | 78 | geometry_msgs::PoseStamped odom_pose; 79 | odom_pose.header.stamp = stamp; 80 | odom_pose.header.frame_id = frame_id; 81 | 82 | odom_pose.pose.position.x = pose(0, 3); 83 | odom_pose.pose.position.y = pose(1, 3); 84 | odom_pose.pose.position.z = pose(2, 3); 85 | odom_pose.pose.orientation = odom_quat; 86 | 87 | return odom_pose; 88 | } 89 | 90 | static Eigen::VectorXf matrix2vector(const Eigen::Matrix4f &pose) { 91 | Eigen::Quaternionf quat(pose.block<3, 3>(0, 0)); 92 | quat.normalize(); 93 | 94 | // converting quaternions to euler angles 95 | tf::Quaternion q(quat.x(), quat.y(), quat.z(), quat.w()); 96 | tf::Matrix3x3 m(q); 97 | 98 | double yaw, pitch, roll; 99 | m.getEulerYPR(yaw, pitch, roll); 100 | 101 | Eigen::VectorXf robot_pose; 102 | robot_pose.resize(6, 6); 103 | robot_pose << pose(0, 3), pose(1, 3), pose(2, 3), roll, pitch, yaw; 104 | 105 | return robot_pose; 106 | } 107 | 108 | static Eigen::Isometry3d 109 | odom2isometry(const nav_msgs::OdometryConstPtr &odom_msg) { 110 | const auto &orientation = odom_msg->pose.pose.orientation; 111 | const auto &position = odom_msg->pose.pose.position; 112 | 113 | Eigen::Quaterniond quat; 114 | quat.w() = orientation.w; 115 | quat.x() = orientation.x; 116 | quat.y() = orientation.y; 117 | quat.z() = orientation.z; 118 | 119 | Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity(); 120 | isometry.linear() = quat.toRotationMatrix(); 121 | isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z); 122 | return isometry; 123 | } 124 | 125 | static Eigen::Isometry3d 126 | pose2isometry(const geometry_msgs::PoseStamped &pose_msg) { 127 | const auto &orientation = pose_msg.pose.orientation; 128 | const auto &position = pose_msg.pose.position; 129 | 130 | Eigen::Quaterniond quat; 131 | quat.w() = orientation.w; 132 | quat.x() = orientation.x; 133 | quat.y() = orientation.y; 134 | quat.z() = orientation.z; 135 | 136 | Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity(); 137 | isometry.linear() = quat.toRotationMatrix(); 138 | isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z); 139 | 140 | return isometry; 141 | } 142 | 143 | static nav_msgs::OdometryPtr 144 | PoseCam2Robot(const nav_msgs::Odometry::ConstPtr odom_msg) { 145 | 146 | // rotation of -90 147 | Eigen::Matrix3f rot_x_robot, rot_z_robot, transformation_mat; 148 | rot_x_robot << 1, 0, 0, 0, cos(-1.5708), -sin(-1.5708), 0, sin(-1.5708), 149 | cos(-1.5708); 150 | 151 | rot_z_robot << cos(-1.5708), -sin(-1.5708), 0, sin(-1.5708), cos(-1.5708), 0, 152 | 0, 0, 1; 153 | 154 | transformation_mat = rot_z_robot * rot_x_robot; 155 | 156 | // converting quaternions to euler angles 157 | tf::Quaternion quat_cam( 158 | odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, 159 | odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w); 160 | tf::Matrix3x3 m(quat_cam); 161 | 162 | double yaw, pitch, roll; 163 | m.getEulerYPR(yaw, pitch, roll); 164 | 165 | Eigen::Vector3f angle_cam, angle_robot; 166 | angle_cam << roll, pitch, yaw; 167 | 168 | angle_robot = transformation_mat * angle_cam; 169 | 170 | tf::Quaternion quat_robot = tf::createQuaternionFromRPY( 171 | angle_robot(0), angle_robot(1), angle_robot(2)); 172 | 173 | // converting the translations 174 | Eigen::Vector3f trans_cam, trans_robot; 175 | trans_cam << odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, 176 | odom_msg->pose.pose.position.z; 177 | 178 | trans_robot = transformation_mat * trans_cam; 179 | 180 | nav_msgs::OdometryPtr odom_converted(new nav_msgs::Odometry()); 181 | odom_converted->pose.pose.position.x = trans_robot(0); 182 | odom_converted->pose.pose.position.y = trans_robot(1); 183 | odom_converted->pose.pose.position.z = trans_robot(2); 184 | odom_converted->pose.pose.orientation.x = quat_robot.x(); 185 | odom_converted->pose.pose.orientation.y = quat_robot.y(); 186 | odom_converted->pose.pose.orientation.z = quat_robot.z(); 187 | odom_converted->pose.pose.orientation.w = quat_robot.w(); 188 | 189 | return odom_converted; 190 | } 191 | 192 | static geometry_msgs::PoseStamped 193 | poseNED2ENU(const geometry_msgs::PoseStamped &pose_msg) { 194 | 195 | // converting quaternions to euler angles 196 | tf::Quaternion quat_ned( 197 | pose_msg.pose.orientation.x, pose_msg.pose.orientation.y, 198 | pose_msg.pose.orientation.z, pose_msg.pose.orientation.w); 199 | tf::Matrix3x3 m(quat_ned); 200 | 201 | double yaw, pitch, roll; 202 | m.getEulerYPR(yaw, pitch, roll); 203 | 204 | Eigen::Matrix3f trans_mat; 205 | trans_mat << 1, 0, 0, 0, cos(-3.14), sin(-3.14), 0, sin(-3.14), cos(-3.14); 206 | 207 | Eigen::Vector3f angle_ned; 208 | angle_ned << roll, pitch, yaw; 209 | 210 | Eigen::Vector3f angle_enu = trans_mat * angle_ned; 211 | tf::Quaternion quat_enu = 212 | tf::createQuaternionFromRPY(angle_enu(0), angle_enu(1), angle_enu(2)); 213 | 214 | // converting the translations 215 | Eigen::Vector3f trans_ned; 216 | trans_ned << pose_msg.pose.position.x, pose_msg.pose.position.y, 217 | pose_msg.pose.position.z; 218 | 219 | Eigen::Vector3f trans_enu = trans_mat * trans_ned; 220 | 221 | geometry_msgs::PoseStamped pose_converted; 222 | pose_converted.pose.position.x = trans_enu(0); 223 | pose_converted.pose.position.y = trans_enu(1); 224 | pose_converted.pose.position.z = trans_enu(2); 225 | pose_converted.pose.orientation.x = quat_enu.x(); 226 | pose_converted.pose.orientation.y = quat_enu.y(); 227 | pose_converted.pose.orientation.z = quat_enu.z(); 228 | pose_converted.pose.orientation.w = quat_enu.w(); 229 | 230 | return pose_converted; 231 | } 232 | 233 | static nav_msgs::OdometryPtr 234 | RotPoseZ(const nav_msgs::OdometryConstPtr &odom_msg, float first_yaw) { 235 | // converting quaternions to euler angles 236 | tf::Quaternion quat( 237 | odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, 238 | odom_msg->pose.pose.orientation.z, odom_msg->pose.pose.orientation.w); 239 | tf::Matrix3x3 m(quat); 240 | 241 | double yaw, pitch, roll; 242 | m.getEulerYPR(yaw, pitch, roll); 243 | 244 | Eigen::Vector3f angle; 245 | angle << roll, pitch, (yaw - first_yaw); 246 | 247 | Eigen::Matrix3f trans_mat; 248 | trans_mat << cos(1.57), -sin(1.57), 0, sin(1.57), cos(1.57), 0, 0, 0, 1; 249 | 250 | Eigen::Vector3f angle_enu = trans_mat * angle; 251 | tf::Quaternion quat_enu = 252 | tf::createQuaternionFromRPY(angle_enu(0), angle_enu(1), angle_enu(2)); 253 | 254 | // converting the translations 255 | Eigen::Vector3f trans; 256 | trans << odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, 257 | odom_msg->pose.pose.position.z; 258 | 259 | Eigen::Vector3f trans_enu = trans_mat * trans; 260 | 261 | nav_msgs::OdometryPtr odom_conv_msg(new nav_msgs::Odometry()); 262 | odom_conv_msg->pose.pose.position.x = trans_enu(0); 263 | odom_conv_msg->pose.pose.position.y = trans_enu(1); 264 | odom_conv_msg->pose.pose.position.z = trans_enu(2); 265 | odom_conv_msg->pose.pose.orientation.x = quat_enu.x(); 266 | odom_conv_msg->pose.pose.orientation.y = quat_enu.y(); 267 | odom_conv_msg->pose.pose.orientation.z = quat_enu.z(); 268 | odom_conv_msg->pose.pose.orientation.w = quat_enu.w(); 269 | 270 | return odom_conv_msg; 271 | } 272 | 273 | static nav_msgs::OdometryPtr 274 | navMsgsToOrigin(const nav_msgs::OdometryConstPtr &odom_msg, float transform_x, 275 | float transform_y, float transform_z) { 276 | nav_msgs::OdometryPtr odom_to_origin_msg(new nav_msgs::Odometry()); 277 | odom_to_origin_msg->pose.pose.position.x = 278 | odom_msg->pose.pose.position.x - transform_x; 279 | odom_to_origin_msg->pose.pose.position.y = 280 | odom_msg->pose.pose.position.y - transform_y; 281 | odom_to_origin_msg->pose.pose.position.z = 282 | odom_msg->pose.pose.position.z - transform_z; 283 | odom_to_origin_msg->pose.pose.orientation = odom_msg->pose.pose.orientation; 284 | 285 | return odom_to_origin_msg; 286 | } 287 | 288 | static Eigen::MatrixXf 289 | arrayToMatrix(const nav_msgs::OdometryConstPtr &odom_msg) { 290 | 291 | Eigen::MatrixXf odom_cov; 292 | odom_cov.resize(6, 6); 293 | float id = 0; 294 | for (int i = 0; i < 6; ++i) { 295 | for (int j = 0; j < 6; ++j) { 296 | odom_cov(i, j) = odom_msg->pose.covariance[id]; 297 | id++; 298 | } 299 | } 300 | 301 | return odom_cov; 302 | } 303 | 304 | } // namespace ps_graph_slam 305 | 306 | #endif // ROS_UTILS_HPP 307 | -------------------------------------------------------------------------------- /include/ps_graph_slam/semantic_graph_slam.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | // ps graph slam headers 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // landmarks 13 | #include "ps_graph_slam/landmark.h" 14 | 15 | // data association 16 | #include "ps_graph_slam/data_association.h" 17 | 18 | // segmented pointcloud acc to detection 19 | #include "planar_segmentation/point_cloud_segmentation.h" 20 | 21 | class semantic_graph_slam { 22 | public: 23 | semantic_graph_slam(); 24 | ~semantic_graph_slam(); 25 | 26 | public: 27 | bool run(); 28 | void init(bool verbose); 29 | 30 | private: 31 | bool verbose_; 32 | 33 | private: 34 | std::unique_ptr pc_seg_obj_; 35 | std::unique_ptr data_ass_obj_; 36 | 37 | private: 38 | bool empty_keyframe_queue(); 39 | int max_keyframes_per_update_; 40 | 41 | std::mutex trans_odom2map_mutex; 42 | Eigen::Matrix4f trans_odom2map_; 43 | 44 | // odom related 45 | protected: 46 | Eigen::Isometry3d prev_odom_, vio_pose_; 47 | bool first_key_added_; 48 | 49 | public: 50 | void VIOCallback(const ros::Time &stamp, Eigen::Isometry3d odom, 51 | Eigen::MatrixXf odom_cov); 52 | void setVIOPose(Eigen::Isometry3d vio_pose); 53 | void getVIOPose(Eigen::Isometry3d &vio_pose); 54 | 55 | protected: 56 | // robot pose related 57 | Eigen::Isometry3d robot_pose_; 58 | Eigen::Isometry3d map2odom_; 59 | double cam_angled_; 60 | float cam_angle_; 61 | bool add_first_lan_; 62 | double first_lan_x_, first_lan_y_, first_lan_z_; 63 | 64 | public: 65 | void getRobotPose(Eigen::Isometry3d &robot_pose) ; 66 | void getMap2OdomTrans(Eigen::Isometry3d &map2odom); 67 | 68 | private: 69 | // landmark related functions 70 | std::vector 71 | semantic_data_ass(const ps_graph_slam::KeyFrame::Ptr curr_keyframe); 72 | 73 | void empty_landmark_queue(std::vector current_lan_queue, 74 | const auto current_keyframe); 75 | 76 | void getAndSetLandmarkCov(); 77 | void addFirstPoseAndLandmark(); 78 | 79 | std::vector seg_obj_vec_; 80 | void setDetectedObjectsPose(std::vector seg_obj_vec); 81 | 82 | public: 83 | void getMappedLandmarks(std::vector &l_vec); 84 | void getDetectedObjectsPose(std::vector &seg_obj_vec); 85 | 86 | public: 87 | void getKeyframes(std::vector &keyframes); 88 | 89 | private: 90 | // point cloud related 91 | sensor_msgs::PointCloud2 point_cloud_msg_; 92 | bool point_cloud_available_; 93 | ros::Time pc_stamp_; 94 | 95 | public: 96 | void setPointCloudData(sensor_msgs::PointCloud2 point_cloud); 97 | void getPointCloudData(sensor_msgs::PointCloud2 &point_cloud); 98 | 99 | private: 100 | // detection related 101 | bool update_keyframes_using_detections_; 102 | bool object_detection_available_; 103 | std::vector object_info_; 104 | 105 | public: 106 | void 107 | setDetectedObjectInfo(std::vector object_info); 108 | void 109 | getDetectedObjectInfo(std::vector &object_info); 110 | 111 | // graph related 112 | public: 113 | void saveGraph(std::string save_graph_path); 114 | 115 | private: 116 | // keyframe related params 117 | std::deque new_keyframes_; 118 | std::vector keyframes_; 119 | std::unordered_map 120 | keyframe_hash_; 121 | std::deque keyframe_queue_; 122 | 123 | // landmark relared params 124 | std::vector landmarks_vec_; 125 | 126 | std::mutex keyframe_queue_mutex; 127 | 128 | std::unique_ptr graph_slam_; 129 | std::unique_ptr keyframe_updater_; 130 | std::unique_ptr inf_calclator_; 131 | }; 132 | -------------------------------------------------------------------------------- /include/semantic_graph_slam_ros.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | // opencv 15 | #include "opencv2/core.hpp" 16 | #include "opencv2/opencv.hpp" 17 | 18 | // ros dependencies 19 | #include "geometry_msgs/PoseArray.h" 20 | #include "geometry_msgs/PoseStamped.h" 21 | #include "nav_msgs/Odometry.h" 22 | #include "nav_msgs/Path.h" 23 | #include "ros/ros.h" 24 | #include "sensor_msgs/Image.h" 25 | #include "sensor_msgs/Imu.h" 26 | #include "sensor_msgs/MagneticField.h" 27 | #include "sensor_msgs/PointCloud2.h" 28 | #include "tf/transform_broadcaster.h" 29 | #include "tf/transform_listener.h" 30 | #include "visualization_msgs/MarkerArray.h" 31 | #include 32 | // ros time synchronizer 33 | #include 34 | #include 35 | #include 36 | 37 | // ps graph slam headers 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | // landmarks 47 | #include "ps_graph_slam/landmark.h" 48 | 49 | // data association 50 | #include "ps_graph_slam/data_association.h" 51 | 52 | // plane segmentation 53 | //#include "plane_segmentation.h" 54 | 55 | // darknet bounding boxes 56 | #include "semantic_SLAM//BoundingBox.h" 57 | #include "semantic_SLAM/BoundingBoxes.h" 58 | 59 | // segmented pointcloud acc to detection 60 | #include "planar_segmentation/point_cloud_segmentation.h" 61 | 62 | // acl messages 63 | #include "semantic_SLAM/ViconState.h" 64 | 65 | #include "ps_graph_slam/semantic_graph_slam.h" 66 | 67 | class semantic_graph_slam_ros { 68 | 69 | public: 70 | semantic_graph_slam_ros(); 71 | ~semantic_graph_slam_ros(); 72 | 73 | public: 74 | std::unique_ptr pc_seg_obj_; 75 | std::unique_ptr data_ass_obj_; 76 | std::unique_ptr semantic_gslam_obj_; 77 | 78 | public: 79 | void run(); 80 | void open(ros::NodeHandle n); 81 | void init(ros::NodeHandle n); 82 | void addFirstPoseAndLandmark(); 83 | void saveGraph(); 84 | 85 | private: 86 | bool use_rovio_odom_; 87 | bool use_rtab_map_odom_; 88 | bool compute_txt_for_ate_; 89 | bool use_snap_pose_; 90 | bool use_orb_slam_odom_; 91 | bool save_graph_; 92 | std::string save_graph_path_; 93 | 94 | private: 95 | // std::thread* semantic_mapping_pub_th_; 96 | 97 | private: 98 | bool verbose_; 99 | 100 | // test stuff 101 | private: 102 | bool counter_; 103 | landmark test_landmark_; 104 | ps_graph_slam::KeyFrame::Ptr test_keyframe_; 105 | int odom_increments_; 106 | 107 | void add_odom_pose_increments(); 108 | void add_odom_position_increments(); 109 | 110 | protected: 111 | // messages sync 112 | // message_filters::Subscriber odom_msg_sub_; 113 | // message_filters::Subscriber 114 | // point_cloud_msg_sub_; 115 | // message_filters::Subscriber bb_sub_; 116 | 117 | // typedef 118 | // message_filters::sync_policies::ApproximateTime SyncPolicy; 121 | 122 | // message_filters::Synchronizer sync; 123 | 124 | // void synMsgsCallback(const nav_msgs::OdometryConstPtr &odom_msg, 125 | // const sensor_msgs::PointCloud2ConstPtr &cloud_msg, 126 | // const darknet_ros_msgs::BoundingBoxesConstPtr 127 | // &bbs_msg); 128 | 129 | // subscribers 130 | protected: 131 | ros::Subscriber rvio_odom_pose_sub_; 132 | ros::Subscriber snap_odom_pose_sub_; 133 | ros::Subscriber orb_slam_pose_sub_; 134 | ros::Subscriber jackal_odom_pose_sub_; 135 | ros::Subscriber cloud_sub_; 136 | ros::Subscriber detected_object_sub_; 137 | ros::Subscriber simple_detected_object_sub_; 138 | ros::Subscriber optitrack_pose_sub_; 139 | ros::Subscriber vicon_pose_sub_; 140 | 141 | protected: 142 | void rovioVIOCallback(const nav_msgs::OdometryConstPtr &odom_msg); 143 | void snapVIOCallback(const geometry_msgs::PoseStamped &pose_msg); 144 | void orbslamPoseCallback(const geometry_msgs::PoseStamped &pose_msg); 145 | void jackalOdomCallback(const nav_msgs::OdometryConstPtr &odom_msg); 146 | void PointCloudCallback(const sensor_msgs::PointCloud2 &msg); 147 | void detectedObjectDarknetCallback(const semantic_SLAM::BoundingBoxes &msg); 148 | void detectedObjectSimpleCallback(const semantic_SLAM::DetectedObjects &msg); 149 | void optitrackPoseCallback(const nav_msgs::Odometry &msg); 150 | void viconPoseSubCallback(const semantic_SLAM::ViconState &msg); 151 | 152 | // publishers 153 | protected: 154 | ros::Publisher landmarks_pub_; 155 | ros::Publisher detected_lans_pub_; 156 | ros::Publisher keyframe_pose_pub_; 157 | ros::Publisher robot_pose_pub_; 158 | ros::Publisher robot_transform_pub_; 159 | ros::Publisher keyframe_path_pub_; 160 | ros::Publisher optitrack_pose_pub_; 161 | ros::Publisher optitrack_path_pub_; 162 | ros::Publisher optitrack_keyframes_pub_; 163 | ros::Publisher corres_vio_pose_pub_; 164 | ros::Publisher corres_vio_path_; 165 | ros::Publisher corres_vio_keyframes_pub_; 166 | ros::Publisher map_points_pub_; 167 | 168 | protected: 169 | void publishRobotPose(); 170 | void publishMap2Odom(); 171 | void publishVIOkeyframes(ros::Time current_time); 172 | void publishgtKeyframes(ros::Time current_time); 173 | void publishgtKeyframes(); 174 | void publishLandmarks(); 175 | void publishDetectedLandmarks(); 176 | void publishKeyframePoses(); 177 | void publishCorresVIOPose(); 178 | void publish3DPointMap(); 179 | 180 | protected: 181 | void publishVIOTF(geometry_msgs::PoseStamped vio_pose); 182 | void publishRobotPoseTF(geometry_msgs::PoseStamped robot_pose); 183 | void publishMap2OdomTF(geometry_msgs::PoseStamped map2odom_pose); 184 | 185 | // transform listener 186 | protected: 187 | tf::TransformListener gt_pose_listener_; 188 | void transformListener(); 189 | 190 | // robot pose related 191 | private: 192 | std::vector robot_pose_vec_; 193 | geometry_msgs::PoseArray robot_pose_array_; 194 | 195 | private: 196 | // odom related 197 | std::vector vio_key_pose_vec_; 198 | std::vector vio_pose_vec_; 199 | geometry_msgs::PoseArray vio_pose_array_; 200 | 201 | private: 202 | // point cloud related 203 | ros::Time pc_stamp_; 204 | 205 | private: 206 | // optitrack related 207 | std::vector optitrack_pose_vec_; 208 | 209 | // orb slam pose related 210 | private: 211 | std::vector orb_slam_pose_vec_; 212 | 213 | private: 214 | // jackal pose related 215 | float jack_x_transform_; 216 | float jack_y_transform_; 217 | float jack_z_transform_; 218 | float jack_yaw_transform_; 219 | bool first_jack_pose_; 220 | 221 | private: 222 | // vicon pose related 223 | float gt_x_transform_; 224 | float gt_y_transform_; 225 | float gt_z_transform_; 226 | 227 | bool first_gt_pose_; 228 | 229 | public: 230 | void computeATE(); 231 | }; 232 | -------------------------------------------------------------------------------- /include/tools.h: -------------------------------------------------------------------------------- 1 | #ifndef SEMANTIC_TOOLS_H 2 | #define SEMANTIC_TOOLS_H 3 | 4 | #include "eigen3/Eigen/Eigen" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class semantic_tools { 12 | public: 13 | semantic_tools() {} 14 | 15 | ~semantic_tools() {} 16 | 17 | public: 18 | void transformNormalsToWorld(Eigen::VectorXf final_pose, 19 | Eigen::Matrix4f &transformation_mat, 20 | const float real_sense_pitch_angle) { 21 | Eigen::Matrix4f rot_x_cam, rot_x_robot, rot_z_robot, translation_cam, 22 | T_robot_world; 23 | rot_x_cam.setZero(4, 4), rot_x_robot.setZero(4, 4), 24 | rot_z_robot.setZero(4, 4), translation_cam.setZero(4, 4), 25 | T_robot_world.setZero(4, 4); 26 | 27 | float x, y, z, roll, pitch, yaw; 28 | 29 | x = final_pose(0); 30 | y = final_pose(1); 31 | z = final_pose(2); 32 | roll = final_pose(3); 33 | pitch = final_pose(4); 34 | yaw = final_pose(5); 35 | 36 | // pitch angle correction: 37 | // std::cout << "X: " << x << std::endl 38 | // << "Y: " << y << std::endl 39 | // << "Z: " << z << std::endl 40 | // << "roll: " << roll << std::endl 41 | // << "pitch: " << pitch << std::endl 42 | // << "yaw: " << yaw << std::endl; 43 | 44 | rot_x_cam(0, 0) = 1; 45 | rot_x_cam(1, 1) = cos(-real_sense_pitch_angle); 46 | rot_x_cam(1, 2) = -sin(-real_sense_pitch_angle); 47 | rot_x_cam(2, 1) = sin(-real_sense_pitch_angle); 48 | rot_x_cam(2, 2) = cos(-real_sense_pitch_angle); 49 | rot_x_cam(3, 3) = 1; 50 | 51 | // rotation of -90 52 | rot_x_robot(0, 0) = 1; 53 | rot_x_robot(1, 1) = cos(-1.5708); 54 | rot_x_robot(1, 2) = -sin(-1.5708); 55 | rot_x_robot(2, 1) = sin(-1.5708); 56 | rot_x_robot(2, 2) = cos(-1.5708); 57 | rot_x_robot(3, 3) = 1; 58 | 59 | // rotation of -90 60 | rot_z_robot(0, 0) = cos(-1.5708); 61 | rot_z_robot(0, 1) = -sin(-1.5708); 62 | rot_z_robot(1, 0) = sin(-1.5708); 63 | rot_z_robot(1, 1) = cos(-1.5708); 64 | rot_z_robot(2, 2) = 1; 65 | rot_z_robot(3, 3) = 1; 66 | 67 | // translation of -10cm in x, 10cm in y and -5cm in z 68 | // translation_cam(0,0) = 1; 69 | // translation_cam(0,3) = 0; 70 | // translation_cam(1,1) = 1; 71 | // translaton_cam(1,3) = -0.1; 72 | // translation_cam(2,2) = 1; 73 | // translation_cam(2,3) = 0.0; 74 | // translation_cam(3,3) = 1; 75 | 76 | // transformation from robot to world 77 | T_robot_world(0, 0) = cos(yaw) * cos(pitch); 78 | T_robot_world(0, 1) = 79 | cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll); 80 | T_robot_world(0, 2) = 81 | cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(pitch); 82 | 83 | T_robot_world(1, 0) = sin(yaw) * cos(pitch); 84 | T_robot_world(1, 1) = 85 | sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll); 86 | T_robot_world(1, 2) = 87 | sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll); 88 | 89 | T_robot_world(2, 0) = -sin(pitch); 90 | T_robot_world(2, 1) = cos(pitch) * sin(roll); 91 | T_robot_world(2, 2) = cos(pitch) * cos(roll); 92 | T_robot_world(3, 3) = 1; 93 | 94 | // fill the x, y and z variables over here if there is a fixed transform 95 | // between the camera and the world frame 96 | // T_robot_world(0,3) = x; 97 | // T_robot_world(1,3) = y; 98 | // T_robot_world(2,3) = z; 99 | // T_robot_world(3,3) = 1; 100 | 101 | transformation_mat = T_robot_world * rot_z_robot * rot_x_robot * rot_x_cam; 102 | } 103 | 104 | void transformPoseFromCameraToRobot(Eigen::Matrix4f &transformation_mat, 105 | const float real_sense_pitch_angle) { 106 | 107 | Eigen::Matrix4f rot_x_cam, rot_x_robot, rot_z_robot, translation_cam; 108 | rot_x_cam.setZero(4, 4), rot_x_robot.setZero(4, 4), 109 | rot_z_robot.setZero(4, 4), translation_cam.setZero(4, 4); 110 | 111 | rot_x_cam(0, 0) = 1; 112 | rot_x_cam(1, 1) = cos(-real_sense_pitch_angle); 113 | rot_x_cam(1, 2) = -sin(-real_sense_pitch_angle); 114 | rot_x_cam(2, 1) = sin(-real_sense_pitch_angle); 115 | rot_x_cam(2, 2) = cos(-real_sense_pitch_angle); 116 | rot_x_cam(3, 3) = 1; 117 | 118 | // rotation of -90 119 | rot_x_robot(0, 0) = 1; 120 | rot_x_robot(1, 1) = cos(-1.5708); 121 | rot_x_robot(1, 2) = -sin(-1.5708); 122 | rot_x_robot(2, 1) = sin(-1.5708); 123 | rot_x_robot(2, 2) = cos(-1.5708); 124 | rot_x_robot(3, 3) = 1; 125 | 126 | // rotation of -90 127 | rot_z_robot(0, 0) = cos(-1.5708); 128 | rot_z_robot(0, 1) = -sin(-1.5708); 129 | rot_z_robot(1, 0) = sin(-1.5708); 130 | rot_z_robot(1, 1) = cos(-1.5708); 131 | rot_z_robot(2, 2) = 1; 132 | rot_z_robot(3, 3) = 1; 133 | 134 | transformation_mat = rot_z_robot * rot_x_robot * rot_x_cam; 135 | } 136 | 137 | void transformIMUtoWorld(float ax, float ay, float az, 138 | Eigen::Matrix4f &transformation_mat) { 139 | Eigen::Matrix4f rot_x_imu, T_robot_world; 140 | double roll, pitch, yaw; 141 | 142 | // for now considering roll, pitch and yaw zero 143 | roll = pitch = yaw = 0; 144 | 145 | rot_x_imu.setZero(), T_robot_world.setZero(); 146 | 147 | // rotation of -180 to convert to robot frame 148 | rot_x_imu(0, 0) = 1; 149 | rot_x_imu(1, 1) = cos(-3.14); 150 | rot_x_imu(1, 2) = -sin(-3.14); 151 | rot_x_imu(2, 1) = sin(-3.14); 152 | rot_x_imu(2, 2) = cos(-3.14); 153 | rot_x_imu(3, 3) = 1; 154 | 155 | // rotation of the robot with respect to the world 156 | T_robot_world(0, 0) = cos(yaw) * cos(pitch); 157 | T_robot_world(0, 1) = 158 | cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll); 159 | T_robot_world(0, 2) = 160 | cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(pitch); 161 | 162 | T_robot_world(1, 0) = sin(yaw) * cos(pitch); 163 | T_robot_world(1, 1) = 164 | sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll); 165 | T_robot_world(1, 2) = 166 | sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll); 167 | 168 | T_robot_world(2, 0) = -sin(pitch); 169 | T_robot_world(2, 1) = cos(pitch) * sin(roll); 170 | T_robot_world(2, 2) = cos(pitch) * cos(roll); 171 | T_robot_world(3, 3) = 1; 172 | 173 | transformation_mat = T_robot_world * rot_x_imu; 174 | } 175 | 176 | void transformRobotToWorld(Eigen::VectorXf pose, 177 | Eigen::Matrix4f &transformation_mat) { 178 | Eigen::Matrix4f T_robot_world; 179 | float roll = pose(3); 180 | float pitch = pose(4); 181 | float yaw = pose(5); 182 | 183 | // rotation of the robot with respect to the world 184 | T_robot_world(0, 0) = cos(yaw) * cos(pitch); 185 | T_robot_world(0, 1) = 186 | cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll); 187 | T_robot_world(0, 2) = 188 | cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(pitch); 189 | 190 | T_robot_world(1, 0) = sin(yaw) * cos(pitch); 191 | T_robot_world(1, 1) = 192 | sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll); 193 | T_robot_world(1, 2) = 194 | sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll); 195 | 196 | T_robot_world(2, 0) = -sin(pitch); 197 | T_robot_world(2, 1) = cos(pitch) * sin(roll); 198 | T_robot_world(2, 2) = cos(pitch) * cos(roll); 199 | T_robot_world(3, 3) = 1; 200 | 201 | transformation_mat = T_robot_world; 202 | } 203 | 204 | void transformMapPointsToWorld(Eigen::VectorXf final_pose, 205 | Eigen::Matrix4f &transformation_mat, 206 | const float real_sense_pitch_angle) { 207 | Eigen::Matrix4f rot_x_cam, rot_x_robot, rot_z_robot, translation_cam, 208 | T_robot_world; 209 | rot_x_cam.setZero(4, 4), rot_x_robot.setZero(4, 4), 210 | rot_z_robot.setZero(4, 4), translation_cam.setZero(4, 4), 211 | T_robot_world.setZero(4, 4); 212 | 213 | float x, y, z, roll, pitch, yaw; 214 | 215 | x = final_pose(0); 216 | y = final_pose(1); 217 | z = final_pose(2); 218 | roll = final_pose(3); 219 | pitch = final_pose(4); 220 | yaw = final_pose(5); 221 | 222 | // pitch angle correction: 223 | // std::cout << "X: " << x << std::endl 224 | // << "Y: " << y << std::endl 225 | // << "Z: " << z << std::endl 226 | // << "roll: " << roll << std::endl 227 | // << "pitch: " << pitch << std::endl 228 | // << "yaw: " << yaw << std::endl; 229 | 230 | rot_x_cam(0, 0) = 1; 231 | rot_x_cam(1, 1) = cos(-real_sense_pitch_angle); 232 | rot_x_cam(1, 2) = -sin(-real_sense_pitch_angle); 233 | rot_x_cam(2, 1) = sin(-real_sense_pitch_angle); 234 | rot_x_cam(2, 2) = cos(-real_sense_pitch_angle); 235 | rot_x_cam(3, 3) = 1; 236 | 237 | // rotation of -90 238 | rot_x_robot(0, 0) = 1; 239 | rot_x_robot(1, 1) = cos(-1.5708); 240 | rot_x_robot(1, 2) = -sin(-1.5708); 241 | rot_x_robot(2, 1) = sin(-1.5708); 242 | rot_x_robot(2, 2) = cos(-1.5708); 243 | rot_x_robot(3, 3) = 1; 244 | 245 | // rotation of -90 246 | rot_z_robot(0, 0) = cos(-1.5708); 247 | rot_z_robot(0, 1) = -sin(-1.5708); 248 | rot_z_robot(1, 0) = sin(-1.5708); 249 | rot_z_robot(1, 1) = cos(-1.5708); 250 | rot_z_robot(2, 2) = 1; 251 | rot_z_robot(3, 3) = 1; 252 | 253 | // translation of -10cm in x, 10cm in y and -5cm in z 254 | // translation_cam(0,0) = 1; 255 | // translation_cam(0,3) = 0; 256 | // translation_cam(1,1) = 1; 257 | // translaton_cam(1,3) = -0.1; 258 | // translation_cam(2,2) = 1; 259 | // translation_cam(2,3) = 0.0; 260 | // translation_cam(3,3) = 1; 261 | 262 | // transformation from robot to world 263 | T_robot_world(0, 0) = cos(yaw) * cos(pitch); 264 | T_robot_world(0, 1) = 265 | cos(yaw) * sin(pitch) * sin(roll) - sin(yaw) * cos(roll); 266 | T_robot_world(0, 2) = 267 | cos(yaw) * sin(pitch) * cos(roll) + sin(yaw) * sin(pitch); 268 | T_robot_world(0, 3) = x; 269 | 270 | T_robot_world(1, 0) = sin(yaw) * cos(pitch); 271 | T_robot_world(1, 1) = 272 | sin(yaw) * sin(pitch) * sin(roll) + cos(yaw) * cos(roll); 273 | T_robot_world(1, 2) = 274 | sin(yaw) * sin(pitch) * cos(roll) - cos(yaw) * sin(roll); 275 | T_robot_world(1, 3) = y; 276 | 277 | T_robot_world(2, 0) = -sin(pitch); 278 | T_robot_world(2, 1) = cos(pitch) * sin(roll); 279 | T_robot_world(2, 2) = cos(pitch) * cos(roll); 280 | T_robot_world(2, 3) = z; 281 | T_robot_world(3, 3) = 1; 282 | 283 | // fill the x, y and z variables over here if there is a fixed transform 284 | // between the camera and the world frame 285 | // T_robot_world(0,3) = x; 286 | // T_robot_world(1,3) = y; 287 | // T_robot_world(2,3) = z; 288 | // T_robot_world(3,3) = 1; 289 | 290 | transformation_mat = T_robot_world * rot_z_robot * rot_x_robot * rot_x_cam; 291 | } 292 | 293 | inline float dist(float x1, float x2, float y1, float y2, float z1, 294 | float z2) { 295 | return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + 296 | (z2 - z1) * (z2 - z1)); 297 | } 298 | 299 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) { 300 | Eigen::Vector3d n = R.col(0); 301 | Eigen::Vector3d o = R.col(1); 302 | Eigen::Vector3d a = R.col(2); 303 | 304 | Eigen::Vector3d ypr(3); 305 | double y = atan2(n(1), n(0)); 306 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 307 | double r = 308 | atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 309 | ypr(0) = y; 310 | ypr(1) = p; 311 | ypr(2) = r; 312 | 313 | return ypr / M_PI * 180.0; 314 | } 315 | 316 | template 317 | static Eigen::Matrix 318 | ypr2R(const Eigen::MatrixBase &ypr) { 319 | typedef typename Derived::Scalar Scalar_t; 320 | 321 | Scalar_t y = ypr(0) / 180.0 * M_PI; 322 | Scalar_t p = ypr(1) / 180.0 * M_PI; 323 | Scalar_t r = ypr(2) / 180.0 * M_PI; 324 | 325 | Eigen::Matrix Rz; 326 | Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1; 327 | 328 | Eigen::Matrix Ry; 329 | Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p); 330 | 331 | Eigen::Matrix Rx; 332 | Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r); 333 | 334 | return Rz * Ry * Rx; 335 | } 336 | }; 337 | 338 | #endif 339 | -------------------------------------------------------------------------------- /launch/ps_slam_with_rovio_pose_yolo.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 | -------------------------------------------------------------------------------- /launch/ps_slam_with_rovio_pose_yolo_corridor.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 | -------------------------------------------------------------------------------- /launch/ps_slam_with_snap_pose_bucket_det_lab_data.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 | -------------------------------------------------------------------------------- /launch/ps_slam_with_snap_pose_bucket_det_lab_data_with_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 | 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 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /launch/semantic_graph_slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /msg/BoundingBox.msg: -------------------------------------------------------------------------------- 1 | string Class 2 | float64 probability 3 | int64 xmin 4 | int64 ymin 5 | int64 xmax 6 | int64 ymax 7 | 8 | -------------------------------------------------------------------------------- /msg/BoundingBoxes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Header image_header 3 | BoundingBox[] bounding_boxes 4 | -------------------------------------------------------------------------------- /msg/DetectedObjects.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | ObjectInfo[] objects 3 | 4 | -------------------------------------------------------------------------------- /msg/ObjectInfo.msg: -------------------------------------------------------------------------------- 1 | string type 2 | float32 prob 3 | int32 tl_x 4 | int32 tl_y 5 | int32 width 6 | int32 height 7 | -------------------------------------------------------------------------------- /msg/acl_msgs/ViconState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist twist 4 | geometry_msgs/Vector3 accel 5 | bool has_pose 6 | bool has_twist 7 | bool has_accel -------------------------------------------------------------------------------- /octomap.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hridaybavle/semantic_slam/5092b0934b06f1a0d6142376e509060d5df07cf8/octomap.gif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | semantic_SLAM 4 | 2.0.0 5 | 6 | This package is a general semantic-SLAM library 7 | 8 | Hriday Bavle 9 | Hriday Bavle 10 | BSD 11 | TODO 12 | 13 | catkin 14 | 15 | roscpp 16 | std_msgs 17 | sensor_msgs 18 | cv_bridge 19 | image_transport 20 | tf_conversions 21 | tf 22 | message_generation 23 | message_runtime 24 | pcl_ros 25 | pcl_msgs 26 | pcl_conversions 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rviz/graph_semantic_slam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /MarkerArray4/Namespaces1 10 | Splitter Ratio: 0.502118647 11 | Tree Height: 250 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679016 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 255; 255; 255 40 | Enabled: false 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Billboards 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 100 52 | Reference Frame: 53 | Value: false 54 | - Alpha: 1 55 | Autocompute Intensity Bounds: true 56 | Autocompute Value Bounds: 57 | Max Value: 10 58 | Min Value: -10 59 | Value: true 60 | Axis: Z 61 | Channel Name: intensity 62 | Class: rviz/PointCloud2 63 | Color: 255; 255; 255 64 | Color Transformer: RGB8 65 | Decay Time: 0 66 | Enabled: true 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Max Intensity: 4096 70 | Min Color: 0; 0; 0 71 | Min Intensity: 0 72 | Name: PointCloud2 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 3 77 | Size (m): 0.00999999978 78 | Style: Flat Squares 79 | Topic: /segmented_point_cloud 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Alpha: 0.100000001 85 | Arrow Length: 0.100000001 86 | Axes Length: 0.300000012 87 | Axes Radius: 0.00999999978 88 | Class: rviz/PoseArray 89 | Color: 255; 25; 0 90 | Enabled: true 91 | Head Length: 0.0700000003 92 | Head Radius: 0.0299999993 93 | Name: PoseArray 94 | Shaft Length: 0.230000004 95 | Shaft Radius: 0.00999999978 96 | Shape: Arrow (Flat) 97 | Topic: /keyframe_poses 98 | Unreliable: false 99 | Value: true 100 | - Alpha: 1 101 | Axes Length: 0.200000003 102 | Axes Radius: 0.00999999978 103 | Class: rviz/Pose 104 | Color: 105; 255; 24 105 | Enabled: true 106 | Head Length: 0.100000001 107 | Head Radius: 0.0500000007 108 | Name: Pose 109 | Shaft Length: 0.100000001 110 | Shaft Radius: 0.00999999978 111 | Shape: Axes 112 | Topic: /robot_pose 113 | Unreliable: true 114 | Value: true 115 | - Class: rviz/MarkerArray 116 | Enabled: false 117 | Marker Topic: /mapped_objects 118 | Name: MarkerArray 119 | Namespaces: 120 | {} 121 | Queue Size: 100 122 | Value: false 123 | - Class: rviz/Marker 124 | Enabled: false 125 | Marker Topic: /rovio/markers 126 | Name: Marker 127 | Namespaces: 128 | {} 129 | Queue Size: 100 130 | Value: false 131 | - Alpha: 1 132 | Axes Length: 1 133 | Axes Radius: 0.100000001 134 | Class: rviz/Pose 135 | Color: 255; 242; 60 136 | Enabled: true 137 | Head Length: 0.100000001 138 | Head Radius: 0.0500000007 139 | Name: Pose 140 | Shaft Length: 0.100000001 141 | Shaft Radius: 0.00999999978 142 | Shape: Arrow 143 | Topic: /corres_vo_pose 144 | Unreliable: false 145 | Value: true 146 | - Alpha: 1 147 | Buffer Length: 1 148 | Class: rviz/Path 149 | Color: 255; 0; 0 150 | Enabled: true 151 | Head Diameter: 0.300000012 152 | Head Length: 0.200000003 153 | Length: 0.300000012 154 | Line Style: Billboards 155 | Line Width: 0.100000001 156 | Name: Path 157 | Offset: 158 | X: 0 159 | Y: 0 160 | Z: 0 161 | Pose Color: 255; 85; 255 162 | Pose Style: None 163 | Radius: 0.0299999993 164 | Shaft Diameter: 0.100000001 165 | Shaft Length: 0.100000001 166 | Topic: /robot_path 167 | Unreliable: false 168 | Value: true 169 | - Alpha: 1 170 | Buffer Length: 1 171 | Class: rviz/Path 172 | Color: 255; 255; 0 173 | Enabled: true 174 | Head Diameter: 0.300000012 175 | Head Length: 0.200000003 176 | Length: 0.300000012 177 | Line Style: Billboards 178 | Line Width: 0.100000001 179 | Name: Path 180 | Offset: 181 | X: 0 182 | Y: 0 183 | Z: 0 184 | Pose Color: 255; 85; 255 185 | Pose Style: None 186 | Radius: 0.0299999993 187 | Shaft Diameter: 0.100000001 188 | Shaft Length: 0.100000001 189 | Topic: /corres_vo_path 190 | Unreliable: false 191 | Value: true 192 | - Alpha: 1 193 | Autocompute Intensity Bounds: true 194 | Autocompute Value Bounds: 195 | Max Value: 10 196 | Min Value: -10 197 | Value: true 198 | Axis: Z 199 | Channel Name: intensity 200 | Class: rviz/PointCloud2 201 | Color: 255; 255; 255 202 | Color Transformer: RGB8 203 | Decay Time: 0 204 | Enabled: false 205 | Invert Rainbow: false 206 | Max Color: 255; 255; 255 207 | Max Intensity: 4096 208 | Min Color: 0; 0; 0 209 | Min Intensity: 0 210 | Name: PointCloud2 211 | Position Transformer: XYZ 212 | Queue Size: 10 213 | Selectable: true 214 | Size (Pixels): 3 215 | Size (m): 0.00999999978 216 | Style: Flat Squares 217 | Topic: /mapped_points 218 | Unreliable: false 219 | Use Fixed Frame: true 220 | Use rainbow: true 221 | Value: false 222 | - Class: rviz/Image 223 | Enabled: false 224 | Image Topic: /image_processed/object_recognized_front_camera 225 | Max Value: 1 226 | Median window: 5 227 | Min Value: 0 228 | Name: Image 229 | Normalize Range: true 230 | Queue Size: 2 231 | Transport Hint: raw 232 | Unreliable: false 233 | Value: false 234 | - Alpha: 1 235 | Buffer Length: 1 236 | Class: rviz/Path 237 | Color: 85; 255; 255 238 | Enabled: true 239 | Head Diameter: 0.100000001 240 | Head Length: 0.100000001 241 | Length: 0.300000012 242 | Line Style: Lines 243 | Line Width: 0.0500000007 244 | Name: Path 245 | Offset: 246 | X: 0 247 | Y: 0 248 | Z: 0 249 | Pose Color: 85; 170; 255 250 | Pose Style: None 251 | Radius: 0.0299999993 252 | Shaft Diameter: 0.0199999996 253 | Shaft Length: 0.100000001 254 | Topic: /optitrack_path 255 | Unreliable: false 256 | Value: true 257 | - Class: rviz/MarkerArray 258 | Enabled: true 259 | Marker Topic: /mapped_landmarks 260 | Name: MarkerArray 261 | Namespaces: 262 | {} 263 | Queue Size: 100 264 | Value: true 265 | - Alpha: 1 266 | Axes Length: 1 267 | Axes Radius: 0.100000001 268 | Class: rviz/Pose 269 | Color: 85; 255; 255 270 | Enabled: true 271 | Head Length: 0.100000001 272 | Head Radius: 0.0500000007 273 | Name: Pose 274 | Shaft Length: 0.100000001 275 | Shaft Radius: 0.00999999978 276 | Shape: Arrow 277 | Topic: /optitrack_pose 278 | Unreliable: false 279 | Value: true 280 | - Alpha: 1 281 | Buffer Length: 1 282 | Class: rviz/Path 283 | Color: 25; 255; 0 284 | Enabled: true 285 | Head Diameter: 0.300000012 286 | Head Length: 0.200000003 287 | Length: 0.300000012 288 | Line Style: Lines 289 | Line Width: 0.0299999993 290 | Name: Path 291 | Offset: 292 | X: 0 293 | Y: 0 294 | Z: 0 295 | Pose Color: 255; 85; 255 296 | Pose Style: None 297 | Radius: 0.0299999993 298 | Shaft Diameter: 0.100000001 299 | Shaft Length: 0.100000001 300 | Topic: /orb_slam/path 301 | Unreliable: false 302 | Value: true 303 | - Alpha: 1 304 | Axes Length: 1 305 | Axes Radius: 0.100000001 306 | Class: rviz/Pose 307 | Color: 25; 255; 0 308 | Enabled: true 309 | Head Length: 0.100000001 310 | Head Radius: 0.0500000007 311 | Name: Pose 312 | Shaft Length: 0.100000001 313 | Shaft Radius: 0.00999999978 314 | Shape: Arrow 315 | Topic: /orb_slam/pose 316 | Unreliable: false 317 | Value: true 318 | - Class: rviz/MarkerArray 319 | Enabled: true 320 | Marker Topic: /detected_landmarks 321 | Name: MarkerArray 322 | Namespaces: 323 | {} 324 | Queue Size: 100 325 | Value: true 326 | - Class: rviz/Image 327 | Enabled: true 328 | Image Topic: /image_processed/object_recognized_front_camera 329 | Max Value: 1 330 | Median window: 5 331 | Min Value: 0 332 | Name: Image 333 | Normalize Range: true 334 | Queue Size: 2 335 | Transport Hint: raw 336 | Unreliable: false 337 | Value: true 338 | - Class: rviz/TF 339 | Enabled: true 340 | Frame Timeout: 15 341 | Frames: 342 | All Enabled: true 343 | Marker Scale: 1 344 | Name: TF 345 | Show Arrows: true 346 | Show Axes: true 347 | Show Names: true 348 | Tree: 349 | {} 350 | Update Interval: 0 351 | Value: true 352 | - Alpha: 0.699999988 353 | Class: octomap_rviz_plugin/OccupancyMap 354 | Color Scheme: map 355 | Draw Behind: false 356 | Enabled: false 357 | Max. Octree Depth: 16 358 | Name: OccupancyMap 359 | Octomap Binary Topic: /octomap_full 360 | Unreliable: true 361 | Use Timestamp: true 362 | Value: false 363 | - Class: rviz/MarkerArray 364 | Enabled: true 365 | Marker Topic: /occupied_cells_vis_array 366 | Name: MarkerArray 367 | Namespaces: 368 | {} 369 | Queue Size: 100 370 | Value: true 371 | Enabled: true 372 | Global Options: 373 | Background Color: 48; 48; 48 374 | Default Light: true 375 | Fixed Frame: map 376 | Frame Rate: 30 377 | Name: root 378 | Tools: 379 | - Class: rviz/Interact 380 | Hide Inactive Objects: true 381 | - Class: rviz/MoveCamera 382 | - Class: rviz/Select 383 | - Class: rviz/FocusCamera 384 | - Class: rviz/Measure 385 | - Class: rviz/SetInitialPose 386 | Topic: /initialpose 387 | - Class: rviz/SetGoal 388 | Topic: /move_base_simple/goal 389 | - Class: rviz/PublishPoint 390 | Single click: true 391 | Topic: /clicked_point 392 | Value: true 393 | Views: 394 | Current: 395 | Class: rviz/Orbit 396 | Distance: 35.2870789 397 | Enable Stereo Rendering: 398 | Stereo Eye Separation: 0.0599999987 399 | Stereo Focal Distance: 1 400 | Swap Stereo Eyes: false 401 | Value: false 402 | Focal Point: 403 | X: 4.68724632 404 | Y: -1.41772592 405 | Z: 15.6323862 406 | Focal Shape Fixed Size: true 407 | Focal Shape Size: 0.0500000007 408 | Invert Z Axis: false 409 | Name: Current View 410 | Near Clip Distance: 0.00999999978 411 | Pitch: 1.38979673 412 | Target Frame: 413 | Value: Orbit (rviz) 414 | Yaw: 2.89834809 415 | Saved: ~ 416 | Window Geometry: 417 | Displays: 418 | collapsed: false 419 | Height: 1056 420 | Hide Left Dock: false 421 | Hide Right Dock: false 422 | Image: 423 | collapsed: false 424 | QMainWindow State: 000000ff00000000fd00000004000000000000032100000402fc020000000ffb0000001200530065006c0065006300740069006f006e00000000000000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc000000000000013b000000d700fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000005c00fffffffb000000100044006900730070006c0061007900730100000000000001e20000016a00fffffffb0000000800540069006d006500000001050000022a0000003b00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000141000002c10000001600fffffffb0000000a0049006d0061006700650100000238000001860000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d0061006700650100000337000000cb0000000000000000fb0000000a0049006d0061006700650100000105000002fd0000000000000000fb0000000a0049006d00610067006501000000e60000031c0000000000000000000000010000010f00000402fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000402000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000004180000040200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 425 | Selection: 426 | collapsed: false 427 | Time: 428 | collapsed: false 429 | Tool Properties: 430 | collapsed: false 431 | Views: 432 | collapsed: false 433 | Width: 1855 434 | X: 65 435 | Y: 24 436 | -------------------------------------------------------------------------------- /rviz/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Imu1 10 | - /TF1 11 | - /TF1/Frames1 12 | Splitter Ratio: 0.5 13 | Tree Height: 775 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Acceleration properties: 55 | Acc. vector alpha: 1 56 | Acc. vector color: 255; 0; 0 57 | Acc. vector scale: 1 58 | Derotate acceleration: true 59 | Enable acceleration: false 60 | Axes properties: 61 | Axes scale: 1 62 | Enable axes: true 63 | Box properties: 64 | Box alpha: 1 65 | Box color: 255; 0; 0 66 | Enable box: false 67 | x_scale: 1 68 | y_scale: 1 69 | z_scale: 1 70 | Class: rviz_imu_plugin/Imu 71 | Enabled: false 72 | Name: Imu 73 | Topic: /imu/data 74 | Unreliable: false 75 | Value: false 76 | fixed_frame_orientation: true 77 | - Class: rviz/TF 78 | Enabled: true 79 | Frame Timeout: 15 80 | Frames: 81 | All Enabled: false 82 | camera_aligned_depth_to_color_frame: 83 | Value: false 84 | camera_aligned_depth_to_infra1_frame: 85 | Value: false 86 | camera_aligned_depth_to_infra2_frame: 87 | Value: false 88 | camera_color_frame: 89 | Value: false 90 | camera_color_optical_frame: 91 | Value: false 92 | camera_depth_frame: 93 | Value: false 94 | camera_depth_optical_frame: 95 | Value: false 96 | camera_imu_frame: 97 | Value: false 98 | camera_imu_optical_frame: 99 | Value: true 100 | camera_infra1_frame: 101 | Value: false 102 | camera_infra1_optical_frame: 103 | Value: false 104 | camera_infra2_frame: 105 | Value: false 106 | camera_infra2_optical_frame: 107 | Value: false 108 | camera_link: 109 | Value: false 110 | Marker Scale: 1 111 | Name: TF 112 | Show Arrows: true 113 | Show Axes: true 114 | Show Names: false 115 | Tree: 116 | camera_link: 117 | camera_aligned_depth_to_color_frame: 118 | camera_color_optical_frame: 119 | {} 120 | camera_aligned_depth_to_infra1_frame: 121 | camera_infra1_optical_frame: 122 | {} 123 | camera_aligned_depth_to_infra2_frame: 124 | camera_infra2_optical_frame: 125 | {} 126 | camera_color_frame: 127 | {} 128 | camera_depth_frame: 129 | camera_depth_optical_frame: 130 | {} 131 | camera_imu_frame: 132 | camera_imu_optical_frame: 133 | {} 134 | camera_infra1_frame: 135 | {} 136 | camera_infra2_frame: 137 | {} 138 | Update Interval: 0 139 | Value: true 140 | Enabled: true 141 | Global Options: 142 | Background Color: 48; 48; 48 143 | Default Light: true 144 | Fixed Frame: camera_link 145 | Frame Rate: 30 146 | Name: root 147 | Tools: 148 | - Class: rviz/Interact 149 | Hide Inactive Objects: true 150 | - Class: rviz/MoveCamera 151 | - Class: rviz/Select 152 | - Class: rviz/FocusCamera 153 | - Class: rviz/Measure 154 | - Class: rviz/SetInitialPose 155 | Topic: /initialpose 156 | - Class: rviz/SetGoal 157 | Topic: /move_base_simple/goal 158 | - Class: rviz/PublishPoint 159 | Single click: true 160 | Topic: /clicked_point 161 | Value: true 162 | Views: 163 | Current: 164 | Class: rviz/Orbit 165 | Distance: 0.661482632 166 | Enable Stereo Rendering: 167 | Stereo Eye Separation: 0.0599999987 168 | Stereo Focal Distance: 1 169 | Swap Stereo Eyes: false 170 | Value: false 171 | Focal Point: 172 | X: 0 173 | Y: 0 174 | Z: 0 175 | Focal Shape Fixed Size: true 176 | Focal Shape Size: 0.0500000007 177 | Invert Z Axis: false 178 | Name: Current View 179 | Near Clip Distance: 0.00999999978 180 | Pitch: 0.240398362 181 | Target Frame: 182 | Value: Orbit (rviz) 183 | Yaw: 5.13857651 184 | Saved: ~ 185 | Window Geometry: 186 | Displays: 187 | collapsed: false 188 | Height: 1056 189 | Hide Left Dock: false 190 | Hide Right Dock: false 191 | QMainWindow State: 000000ff00000000fd00000004000000000000032600000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074b0000003efc0100000002fb0000000800540069006d006501000000000000074b0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000030a0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 192 | Selection: 193 | collapsed: false 194 | Time: 195 | collapsed: false 196 | Tool Properties: 197 | collapsed: false 198 | Views: 199 | collapsed: false 200 | Width: 1867 201 | X: 53 202 | Y: 24 203 | -------------------------------------------------------------------------------- /rviz/semantic_slam_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /PointCloud21 11 | - /PoseArray1 12 | - /Pose1 13 | - /MarkerArray1 14 | - /Marker1 15 | - /Pose2 16 | - /Path1 17 | - /Path2 18 | - /PointCloud22 19 | - /Image1 20 | - /Image2 21 | - /Path3 22 | - /MarkerArray2 23 | - /Pose3 24 | - /Path4 25 | - /Pose4 26 | Splitter Ratio: 0.502118647 27 | Tree Height: 362 28 | - Class: rviz/Selection 29 | Name: Selection 30 | - Class: rviz/Tool Properties 31 | Expanded: 32 | - /2D Pose Estimate1 33 | - /2D Nav Goal1 34 | - /Publish Point1 35 | Name: Tool Properties 36 | Splitter Ratio: 0.588679016 37 | - Class: rviz/Views 38 | Expanded: 39 | - /Current View1 40 | Name: Views 41 | Splitter Ratio: 0.5 42 | - Class: rviz/Time 43 | Experimental: false 44 | Name: Time 45 | SyncMode: 0 46 | SyncSource: "" 47 | Visualization Manager: 48 | Class: "" 49 | Displays: 50 | - Alpha: 0.5 51 | Cell Size: 1 52 | Class: rviz/Grid 53 | Color: 160; 160; 164 54 | Enabled: true 55 | Line Style: 56 | Line Width: 0.0299999993 57 | Value: Lines 58 | Name: Grid 59 | Normal Cell Count: 0 60 | Offset: 61 | X: 0 62 | Y: 0 63 | Z: 0 64 | Plane: XY 65 | Plane Cell Count: 100 66 | Reference Frame: 67 | Value: true 68 | - Alpha: 1 69 | Autocompute Intensity Bounds: true 70 | Autocompute Value Bounds: 71 | Max Value: 10 72 | Min Value: -10 73 | Value: true 74 | Axis: Z 75 | Channel Name: intensity 76 | Class: rviz/PointCloud2 77 | Color: 255; 255; 255 78 | Color Transformer: RGB8 79 | Decay Time: 0 80 | Enabled: true 81 | Invert Rainbow: false 82 | Max Color: 255; 255; 255 83 | Max Intensity: 4096 84 | Min Color: 0; 0; 0 85 | Min Intensity: 0 86 | Name: PointCloud2 87 | Position Transformer: XYZ 88 | Queue Size: 10 89 | Selectable: true 90 | Size (Pixels): 3 91 | Size (m): 0.00999999978 92 | Style: Flat Squares 93 | Topic: /segmented_point_cloud 94 | Unreliable: false 95 | Use Fixed Frame: true 96 | Use rainbow: true 97 | Value: true 98 | - Alpha: 0.100000001 99 | Arrow Length: 0.100000001 100 | Axes Length: 0.300000012 101 | Axes Radius: 0.00999999978 102 | Class: rviz/PoseArray 103 | Color: 255; 25; 0 104 | Enabled: true 105 | Head Length: 0.0700000003 106 | Head Radius: 0.0299999993 107 | Name: PoseArray 108 | Shaft Length: 0.230000004 109 | Shaft Radius: 0.00999999978 110 | Shape: Arrow (Flat) 111 | Topic: /particle_poses 112 | Unreliable: false 113 | Value: true 114 | - Alpha: 1 115 | Axes Length: 0.200000003 116 | Axes Radius: 0.00999999978 117 | Class: rviz/Pose 118 | Color: 105; 255; 24 119 | Enabled: true 120 | Head Length: 0.100000001 121 | Head Radius: 0.0500000007 122 | Name: Pose 123 | Shaft Length: 0.100000001 124 | Shaft Radius: 0.00999999978 125 | Shape: Axes 126 | Topic: /final_pose 127 | Unreliable: true 128 | Value: true 129 | - Class: rviz/MarkerArray 130 | Enabled: false 131 | Marker Topic: /mapped_objects 132 | Name: MarkerArray 133 | Namespaces: 134 | {} 135 | Queue Size: 100 136 | Value: false 137 | - Class: rviz/Marker 138 | Enabled: true 139 | Marker Topic: visualization_marker 140 | Name: Marker 141 | Namespaces: 142 | {} 143 | Queue Size: 100 144 | Value: true 145 | - Alpha: 1 146 | Axes Length: 1 147 | Axes Radius: 0.100000001 148 | Class: rviz/Pose 149 | Color: 255; 242; 60 150 | Enabled: true 151 | Head Length: 0.100000001 152 | Head Radius: 0.0500000007 153 | Name: Pose 154 | Shaft Length: 0.100000001 155 | Shaft Radius: 0.00999999978 156 | Shape: Arrow 157 | Topic: /corres_vo_pose 158 | Unreliable: false 159 | Value: true 160 | - Alpha: 1 161 | Buffer Length: 1 162 | Class: rviz/Path 163 | Color: 255; 0; 0 164 | Enabled: true 165 | Head Diameter: 0.300000012 166 | Head Length: 0.200000003 167 | Length: 0.300000012 168 | Line Style: Lines 169 | Line Width: 0.0299999993 170 | Name: Path 171 | Offset: 172 | X: 0 173 | Y: 0 174 | Z: 0 175 | Pose Color: 255; 85; 255 176 | Pose Style: None 177 | Radius: 0.0299999993 178 | Shaft Diameter: 0.100000001 179 | Shaft Length: 0.100000001 180 | Topic: /final_path 181 | Unreliable: false 182 | Value: true 183 | - Alpha: 1 184 | Buffer Length: 1 185 | Class: rviz/Path 186 | Color: 255; 255; 0 187 | Enabled: true 188 | Head Diameter: 0.300000012 189 | Head Length: 0.200000003 190 | Length: 0.300000012 191 | Line Style: Lines 192 | Line Width: 0.0299999993 193 | Name: Path 194 | Offset: 195 | X: 0 196 | Y: 0 197 | Z: 0 198 | Pose Color: 255; 85; 255 199 | Pose Style: None 200 | Radius: 0.0299999993 201 | Shaft Diameter: 0.100000001 202 | Shaft Length: 0.100000001 203 | Topic: /corres_vo_path 204 | Unreliable: false 205 | Value: true 206 | - Alpha: 1 207 | Autocompute Intensity Bounds: true 208 | Autocompute Value Bounds: 209 | Max Value: 10 210 | Min Value: -10 211 | Value: true 212 | Axis: Z 213 | Channel Name: intensity 214 | Class: rviz/PointCloud2 215 | Color: 255; 255; 255 216 | Color Transformer: RGB8 217 | Decay Time: 0 218 | Enabled: true 219 | Invert Rainbow: false 220 | Max Color: 255; 255; 255 221 | Max Intensity: 4096 222 | Min Color: 0; 0; 0 223 | Min Intensity: 0 224 | Name: PointCloud2 225 | Position Transformer: XYZ 226 | Queue Size: 10 227 | Selectable: true 228 | Size (Pixels): 3 229 | Size (m): 0.00999999978 230 | Style: Flat Squares 231 | Topic: /mapped_points 232 | Unreliable: false 233 | Use Fixed Frame: true 234 | Use rainbow: true 235 | Value: true 236 | - Class: rviz/Image 237 | Enabled: false 238 | Image Topic: /image_processed/object_recognized_front_camera 239 | Max Value: 1 240 | Median window: 5 241 | Min Value: 0 242 | Name: Image 243 | Normalize Range: true 244 | Queue Size: 2 245 | Transport Hint: raw 246 | Unreliable: false 247 | Value: false 248 | - Class: rviz/Image 249 | Enabled: true 250 | Image Topic: /image_processed/object_recognized_front_camera 251 | Max Value: 1 252 | Median window: 5 253 | Min Value: 0 254 | Name: Image 255 | Normalize Range: true 256 | Queue Size: 2 257 | Transport Hint: raw 258 | Unreliable: false 259 | Value: true 260 | - Alpha: 1 261 | Buffer Length: 1 262 | Class: rviz/Path 263 | Color: 85; 255; 255 264 | Enabled: true 265 | Head Diameter: 0.100000001 266 | Head Length: 0.100000001 267 | Length: 0.300000012 268 | Line Style: Lines 269 | Line Width: 0.0299999993 270 | Name: Path 271 | Offset: 272 | X: 0 273 | Y: 0 274 | Z: 0 275 | Pose Color: 85; 170; 255 276 | Pose Style: None 277 | Radius: 0.0299999993 278 | Shaft Diameter: 0.0199999996 279 | Shaft Length: 0.100000001 280 | Topic: /optitrack_path 281 | Unreliable: false 282 | Value: true 283 | - Class: rviz/MarkerArray 284 | Enabled: true 285 | Marker Topic: /detected_objects 286 | Name: MarkerArray 287 | Namespaces: 288 | my_namespace: true 289 | Queue Size: 100 290 | Value: true 291 | - Alpha: 1 292 | Axes Length: 1 293 | Axes Radius: 0.100000001 294 | Class: rviz/Pose 295 | Color: 85; 255; 255 296 | Enabled: true 297 | Head Length: 0.100000001 298 | Head Radius: 0.0500000007 299 | Name: Pose 300 | Shaft Length: 0.100000001 301 | Shaft Radius: 0.00999999978 302 | Shape: Arrow 303 | Topic: /optitrack_pose 304 | Unreliable: false 305 | Value: true 306 | - Alpha: 1 307 | Buffer Length: 1 308 | Class: rviz/Path 309 | Color: 25; 255; 0 310 | Enabled: true 311 | Head Diameter: 0.300000012 312 | Head Length: 0.200000003 313 | Length: 0.300000012 314 | Line Style: Lines 315 | Line Width: 0.0299999993 316 | Name: Path 317 | Offset: 318 | X: 0 319 | Y: 0 320 | Z: 0 321 | Pose Color: 255; 85; 255 322 | Pose Style: None 323 | Radius: 0.0299999993 324 | Shaft Diameter: 0.100000001 325 | Shaft Length: 0.100000001 326 | Topic: /orb_slam/path 327 | Unreliable: false 328 | Value: true 329 | - Alpha: 1 330 | Axes Length: 1 331 | Axes Radius: 0.100000001 332 | Class: rviz/Pose 333 | Color: 25; 255; 0 334 | Enabled: true 335 | Head Length: 0.100000001 336 | Head Radius: 0.0500000007 337 | Name: Pose 338 | Shaft Length: 0.100000001 339 | Shaft Radius: 0.00999999978 340 | Shape: Arrow 341 | Topic: /orb_slam/pose 342 | Unreliable: false 343 | Value: true 344 | Enabled: true 345 | Global Options: 346 | Background Color: 48; 48; 48 347 | Default Light: true 348 | Fixed Frame: map 349 | Frame Rate: 30 350 | Name: root 351 | Tools: 352 | - Class: rviz/Interact 353 | Hide Inactive Objects: true 354 | - Class: rviz/MoveCamera 355 | - Class: rviz/Select 356 | - Class: rviz/FocusCamera 357 | - Class: rviz/Measure 358 | - Class: rviz/SetInitialPose 359 | Topic: /initialpose 360 | - Class: rviz/SetGoal 361 | Topic: /move_base_simple/goal 362 | - Class: rviz/PublishPoint 363 | Single click: true 364 | Topic: /clicked_point 365 | Value: true 366 | Views: 367 | Current: 368 | Class: rviz/Orbit 369 | Distance: 11.4623852 370 | Enable Stereo Rendering: 371 | Stereo Eye Separation: 0.0599999987 372 | Stereo Focal Distance: 1 373 | Swap Stereo Eyes: false 374 | Value: false 375 | Focal Point: 376 | X: 3.49668312 377 | Y: 0.808902681 378 | Z: -0.100616023 379 | Focal Shape Fixed Size: true 380 | Focal Shape Size: 0.0500000007 381 | Invert Z Axis: false 382 | Name: Current View 383 | Near Clip Distance: 0.00999999978 384 | Pitch: 0.614796519 385 | Target Frame: 386 | Value: Orbit (rviz) 387 | Yaw: 3.30213904 388 | Saved: ~ 389 | Window Geometry: 390 | Displays: 391 | collapsed: false 392 | Height: 1028 393 | Hide Left Dock: false 394 | Hide Right Dock: true 395 | Image: 396 | collapsed: false 397 | QMainWindow State: 000000ff00000000fd00000004000000000000024a000003befc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000028000001ab000000d700fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000005c00fffffffb000000100044006900730070006c0061007900730100000000000001e20000016a00fffffffb0000000800540069006d006500000001050000022a0000003b00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001d90000020d0000001600fffffffb0000000a0049006d0061006700650100000238000001860000000000000000fb0000000a0049006d00610067006501000002f4000000ca0000000000000000fb0000000a0049006d0061006700650100000337000000cb0000000000000000fb0000000a0049006d0061006700650100000105000002fd0000000000000000000000010000010f000003befc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003be000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000001fb0000000800540069006d0065010000000000000450000000000000000000000530000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 398 | Selection: 399 | collapsed: false 400 | Time: 401 | collapsed: false 402 | Tool Properties: 403 | collapsed: false 404 | Views: 405 | collapsed: true 406 | Width: 1920 407 | X: 1920 408 | Y: 24 409 | -------------------------------------------------------------------------------- /semantic.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hridaybavle/semantic_slam/5092b0934b06f1a0d6142376e509060d5df07cf8/semantic.gif -------------------------------------------------------------------------------- /semantic_slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hridaybavle/semantic_slam/5092b0934b06f1a0d6142376e509060d5df07cf8/semantic_slam.png -------------------------------------------------------------------------------- /src/ps_graph_slam/graph_slam.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | G2O_USE_OPTIMIZATION_LIBRARY(csparse) 28 | 29 | namespace g2o { 30 | // G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane) 31 | // G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY) 32 | // G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ) 33 | } 34 | 35 | namespace ps_graph_slam { 36 | 37 | /** 38 | * @brief constructor 39 | */ 40 | GraphSLAM::GraphSLAM(bool verbose) { 41 | 42 | verbose_ = verbose; 43 | graph.reset(new g2o::SparseOptimizer()); 44 | 45 | std::cout << "construct solver... " << std::endl; 46 | 47 | // typedef g2o::BlockSolver > 48 | // BlockSolver_3_3; BlockSolver_3_3::LinearSolverType* linearSolver 49 | // = new 50 | // g2o::LinearSolverCSparse(); 51 | // BlockSolver_3_3* blockSolver 52 | // = new BlockSolver_3_3(linearSolver); 53 | // g2o::OptimizationAlgorithmGaussNewton* solver 54 | // = new g2o::OptimizationAlgorithmGaussNewton(blockSolver); 55 | // graph->setAlgorithm(solver); 56 | 57 | // Create the block solver - the dimensions are specified because 58 | // 3D observations marginalise to a 6D estimate 59 | // std::unique_ptr linearSolver (new 60 | // g2o::LinearSolverCSparse()); 61 | // std::unique_ptr solver_ptr (new 62 | // g2o::BlockSolverX(std::move(linearSolver))); 63 | // g2o::OptimizationAlgorithmLevenberg * solver = new 64 | // g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr)); 65 | // graph->setAlgorithm(solver); 66 | 67 | std::string g2o_solver_name = "lm_var"; 68 | g2o::OptimizationAlgorithmFactory *solver_factory = 69 | g2o::OptimizationAlgorithmFactory::instance(); 70 | g2o::OptimizationAlgorithmProperty solver_property; 71 | g2o::OptimizationAlgorithm *solver = 72 | solver_factory->construct(g2o_solver_name, solver_property); 73 | graph->setAlgorithm(solver); 74 | 75 | g2o::ParameterSE3Offset *cameraOffset(new g2o::ParameterSE3Offset); 76 | Eigen::Quaterniond quat; 77 | quat.setIdentity(); 78 | Eigen::Vector3d trans; 79 | trans.setZero(); 80 | g2o::SE3Quat offset_quat(quat, trans); 81 | cameraOffset->setId(0); 82 | cameraOffset->setOffset(offset_quat); 83 | graph->addParameter(cameraOffset); 84 | 85 | if (!graph->solver()) { 86 | std::cerr << std::endl; 87 | std::cerr << "error : failed to allocate solver!!" << std::endl; 88 | // solver_factory->listSolvers(std::cerr); 89 | std::cerr << "-------------" << std::endl; 90 | std::cin.ignore(1); 91 | return; 92 | } 93 | std::cout << "done" << std::endl; 94 | 95 | // floor_plane_node = add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0)); 96 | // floor_plane_node->setFixed(true); 97 | } 98 | 99 | /** 100 | * @brief destructor 101 | */ 102 | GraphSLAM::~GraphSLAM() { graph.reset(); } 103 | 104 | g2o::VertexSE3 *GraphSLAM::add_se3_node(const Eigen::Isometry3d &pose) { 105 | g2o::VertexSE3 *vertex(new g2o::VertexSE3()); 106 | vertex->setId(graph->vertices().size()); 107 | vertex->setEstimate(pose); 108 | // fixing the first node pose 109 | if (graph->vertices().size() == 0) { 110 | vertex->setFixed(true); 111 | } 112 | graph->addVertex(vertex); 113 | 114 | return vertex; 115 | } 116 | 117 | // g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& 118 | // plane_coeffs) { 119 | // g2o::VertexPlane* vertex(new g2o::VertexPlane()); 120 | // vertex->setId(graph->vertices().size()); 121 | // vertex->setEstimate(plane_coeffs); 122 | // graph->addVertex(vertex); 123 | 124 | // return vertex; 125 | //} 126 | 127 | g2o::VertexPointXYZ *GraphSLAM::add_point_xyz_node(const Eigen::Vector3d &xyz) { 128 | g2o::VertexPointXYZ *vertex(new g2o::VertexPointXYZ()); 129 | vertex->setId(graph->vertices().size()); 130 | vertex->setEstimate(xyz); 131 | graph->addVertex(vertex); 132 | 133 | return vertex; 134 | } 135 | 136 | g2o::EdgeSE3 * 137 | GraphSLAM::add_se3_edge(g2o::VertexSE3 *v1, g2o::VertexSE3 *v2, 138 | const Eigen::Isometry3d &relative_pose, 139 | const Eigen::MatrixXd &information_matrix) { 140 | g2o::EdgeSE3 *edge(new g2o::EdgeSE3()); 141 | edge->setMeasurement(relative_pose); 142 | edge->setInformation(information_matrix); 143 | edge->vertices()[0] = v1; 144 | edge->vertices()[1] = v2; 145 | graph->addEdge(edge); 146 | 147 | return edge; 148 | } 149 | 150 | g2o::EdgeSE3PointXYZ *GraphSLAM::add_se3_point_xyz_edge( 151 | g2o::VertexSE3 *v_se3, g2o::VertexPointXYZ *v_xyz, 152 | const Eigen::Vector3d &xyz, const Eigen::MatrixXd &information_matrix) { 153 | g2o::EdgeSE3PointXYZ *edge(new g2o::EdgeSE3PointXYZ()); 154 | 155 | g2o::RobustKernelDCS *dcs_robust_kernel; 156 | 157 | edge->setMeasurement(xyz); 158 | edge->setInformation(information_matrix); 159 | edge->vertices()[0] = v_se3; 160 | edge->vertices()[1] = v_xyz; 161 | edge->setRobustKernel(dcs_robust_kernel); 162 | edge->setParameterId(0, 0); 163 | graph->addEdge(edge); 164 | 165 | return edge; 166 | } 167 | 168 | g2o::EdgePointXYZ *GraphSLAM::add_point_xyz_point_xyz_edge( 169 | g2o::VertexPointXYZ *v1_xyz, g2o::VertexPointXYZ *v2_xyz, 170 | const Eigen::Vector3d &xyz, const Eigen::MatrixXd &information_matrix) { 171 | g2o::EdgePointXYZ *edge(new g2o::EdgePointXYZ()); 172 | edge->setMeasurement(xyz); 173 | edge->setInformation(information_matrix); 174 | edge->vertices()[0] = v1_xyz; 175 | edge->vertices()[1] = v2_xyz; 176 | edge->setParameterId(0, 0); 177 | graph->addEdge(edge); 178 | 179 | return edge; 180 | } 181 | 182 | bool GraphSLAM::optimize() { 183 | 184 | if (graph->edges().size() < 10) { 185 | return false; 186 | } 187 | 188 | if (verbose_) { 189 | std::cout << std::endl; 190 | std::cout << "--- pose graph optimization ---" << std::endl; 191 | std::cout << "nodes: " << graph->vertices().size() 192 | << " edges: " << graph->edges().size() << std::endl; 193 | std::cout << "optimizing... " << std::flush; 194 | } 195 | 196 | // graph->setComputeBatchStatistics(true); 197 | // graph->computeActiveErrors(); 198 | 199 | graph->initializeOptimization(); 200 | graph->setVerbose(false); 201 | 202 | double chi2 = graph->chi2(); 203 | 204 | auto t1 = ros::Time::now(); 205 | int iterations = graph->optimize(1024); 206 | 207 | auto t2 = ros::Time::now(); 208 | 209 | if (verbose_) { 210 | std::cout << "done" << std::endl; 211 | std::cout << "iterations: " << iterations << std::endl; 212 | std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() 213 | << std::endl; 214 | std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() 215 | << "[sec]" << std::endl; 216 | } 217 | 218 | return true; 219 | } 220 | 221 | bool GraphSLAM::computeLandmarkMarginals( 222 | g2o::SparseBlockMatrix &spinv, 223 | std::vector> vert_pairs_vec) { 224 | 225 | if (graph->computeMarginals(spinv, vert_pairs_vec)) { 226 | if (verbose_) 227 | std::cout << "computed marginals " << std::endl; 228 | return true; 229 | } else { 230 | if (verbose_) 231 | std::cout << "not computing marginals " << std::endl; 232 | return false; 233 | } 234 | } 235 | 236 | void GraphSLAM::save(const std::string &filename) { 237 | std::ofstream ofs(filename); 238 | graph->save(ofs); 239 | } 240 | 241 | } // namespace ps_graph_slam 242 | -------------------------------------------------------------------------------- /src/ps_graph_slam/information_matrix_calculator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace ps_graph_slam { 7 | 8 | InformationMatrixCalculator::InformationMatrixCalculator() { 9 | ros::param::get("~use_const_inf_matrix", use_const_inf_matrix); 10 | 11 | ros::param::get("~const_stddev_x", const_stddev_x); 12 | if(const_stddev_x == 0) 13 | const_stddev_x = 0.0667; 14 | 15 | ros::param::get("~const_stddev_q", const_stddev_q); 16 | if(const_stddev_q == 0) 17 | const_stddev_q = 0.0667; 18 | 19 | std::cout << "use_const_inf_matrix: " << use_const_inf_matrix << std::endl; 20 | std::cout << "const_stddev_x: " << const_stddev_x << std::endl; 21 | std::cout << "const_stddev_q: " << const_stddev_q << std::endl; 22 | } 23 | 24 | InformationMatrixCalculator::~InformationMatrixCalculator() { 25 | 26 | } 27 | 28 | Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix() const { 29 | if(use_const_inf_matrix) { 30 | Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); 31 | inf.topLeftCorner(3, 3).array() /= const_stddev_x; 32 | inf.bottomRightCorner(3, 3).array() /= const_stddev_q; 33 | //inf(2,2) = 9; 34 | return inf; 35 | } 36 | 37 | double fitness_score = 0.9 ;//calc_fitness_score(cloud1, cloud2, relpose); 38 | 39 | double min_var_x = std::pow(min_stddev_x, 2); 40 | double max_var_x = std::pow(max_stddev_x, 2); 41 | double min_var_q = std::pow(min_stddev_q, 2); 42 | double max_var_q = std::pow(max_stddev_q, 2); 43 | 44 | float w_x = weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score); 45 | float w_q = weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score); 46 | 47 | Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); 48 | inf.topLeftCorner(3, 3).array() /= w_x; 49 | inf.bottomRightCorner(3, 3).array() /= w_q; 50 | return inf; 51 | } 52 | 53 | } 54 | 55 | -------------------------------------------------------------------------------- /src/ps_graph_slam/keyframe.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace ps_graph_slam { 9 | 10 | KeyFrame::KeyFrame(const ros::Time &stamp, const Eigen::Isometry3d &odom, 11 | const Eigen::Isometry3d &robot_pose, 12 | const Eigen::MatrixXf &odom_cov, double accum_distance, 13 | const sensor_msgs::PointCloud2 &cloud_msg, 14 | const pcl::PointCloud::Ptr &cloud, 15 | std::vector &obj_info) 16 | : stamp(stamp), odom(odom), robot_pose(robot_pose), odom_cov(odom_cov), 17 | accum_distance(accum_distance), cloud_msg(cloud_msg), cloud(cloud), 18 | obj_info(obj_info), node(nullptr) {} 19 | 20 | KeyFrame::~KeyFrame() {} 21 | 22 | void KeyFrame::dump(const std::string &directory) { 23 | if (!boost::filesystem::is_directory(directory)) { 24 | boost::filesystem::create_directory(directory); 25 | } 26 | 27 | std::ofstream ofs(directory + "/data"); 28 | ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n"; 29 | 30 | ofs << "odom\n"; 31 | ofs << odom.matrix() << "\n"; 32 | 33 | ofs << "accum_distance " << accum_distance << "\n"; 34 | 35 | if (node) { 36 | ofs << "id " << node->id() << "\n"; 37 | } 38 | 39 | // pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud); 40 | } 41 | 42 | } // namespace ps_graph_slam 43 | -------------------------------------------------------------------------------- /src/ps_graph_slam/semantic_graph_slam.cpp: -------------------------------------------------------------------------------- 1 | #include "ps_graph_slam/semantic_graph_slam.h" 2 | 3 | semantic_graph_slam::semantic_graph_slam() { 4 | std::cout << "semantic graph slam constructor " << std::endl; 5 | } 6 | 7 | semantic_graph_slam::~semantic_graph_slam() { 8 | std::cout << "semantic graph slam destructor " << std::endl; 9 | } 10 | 11 | void semantic_graph_slam::init(bool verbose) { 12 | verbose_ = verbose; 13 | // default values 14 | object_detection_available_ = false; 15 | point_cloud_available_ = false; 16 | first_key_added_ = false; 17 | update_keyframes_using_detections_ = false; 18 | max_keyframes_per_update_ = 10; 19 | new_keyframes_.clear(); 20 | seg_obj_vec_.clear(); 21 | 22 | ros::param::param("~update_key_using_det", 23 | update_keyframes_using_detections_, false); 24 | ros::param::param("~camera_angle", cam_angled_, 0); 25 | ros::param::param("~add_first_lan", add_first_lan_, false); 26 | ros::param::param("~first_lan_x", first_lan_x_, 1.8); 27 | ros::param::param("~first_lan_y", first_lan_y_, 0); 28 | ros::param::param("~first_lan_z", first_lan_z_, 0.3); 29 | cam_angle_ = static_cast(cam_angled_) * (M_PI / 180); 30 | 31 | pc_seg_obj_.reset(new point_cloud_segmentation(verbose_)); 32 | data_ass_obj_.reset(new data_association(verbose_)); 33 | graph_slam_.reset(new ps_graph_slam::GraphSLAM(verbose_)); 34 | keyframe_updater_.reset(new ps_graph_slam::KeyframeUpdater()); 35 | inf_calclator_.reset(new ps_graph_slam::InformationMatrixCalculator()); 36 | // semantic_mapping_obj_ = new mapping(cam_angle_); 37 | // semantic_mapping_th_ = new 38 | // std::thread(&mapping::generateMap,semantic_mapping_obj_); 39 | // semantic_mapping_opt_th_ = new 40 | // std::thread(&mapping::opitmizeMap,semantic_mapping_obj_); 41 | 42 | trans_odom2map_.setIdentity(); 43 | landmarks_vec_.clear(); 44 | robot_pose_.setIdentity(); 45 | vio_pose_.setIdentity(); 46 | prev_odom_.setIdentity(); 47 | map2odom_.setIdentity(); 48 | 49 | std::cout << "camera angle in radians: " << cam_angle_ << std::endl; 50 | std::cout << "update keyframe every detection: " 51 | << update_keyframes_using_detections_ << std::endl; 52 | std::cout << "add first landmark: " << add_first_lan_ << std::endl; 53 | 54 | if (add_first_lan_) 55 | this->addFirstPoseAndLandmark(); 56 | } 57 | 58 | bool semantic_graph_slam::run() { 59 | // add keyframe nodes to graph if keyframes available 60 | if (empty_keyframe_queue()) { 61 | // add semantic data nodes to graph if available 62 | for (int i = 0; i < new_keyframes_.size(); ++i) { 63 | if (!new_keyframes_[i]->obj_info.empty()) { 64 | // segmenting and matching keyframes 65 | std::vector current_landmarks_vec = 66 | this->semantic_data_ass(new_keyframes_[i]); 67 | // add the segmented landmarks to the graph for the current keyframe 68 | this->empty_landmark_queue(current_landmarks_vec, new_keyframes_[i]); 69 | } 70 | } 71 | 72 | // pass the keyframe to the mapping module 73 | // semantic_mapping_obj_->setKeyframes(new_keyframes_); 74 | 75 | // graph slam opitimization 76 | std::copy(new_keyframes_.begin(), new_keyframes_.end(), 77 | std::back_inserter(keyframes_)); 78 | new_keyframes_.clear(); 79 | 80 | // optimizing the graph 81 | if (graph_slam_->optimize()) { 82 | if (verbose_) 83 | std::cout << "optimizing the graph " << std::endl; 84 | 85 | // give the optimized keypoints to the mapping 86 | // semantic_mapping_obj_->setoptimizedKeyframes(keyframes_); 87 | 88 | // get and set the landmark covariances 89 | this->getAndSetLandmarkCov(); 90 | 91 | // getting the optimized pose 92 | const auto &keyframe = keyframes_.back(); 93 | 94 | robot_pose_ = keyframe->node->estimate(); 95 | map2odom_ = keyframe->node->estimate() * keyframe->odom.inverse(); 96 | } 97 | 98 | first_key_added_ = true; 99 | return true; 100 | } else 101 | return false; 102 | } 103 | 104 | bool semantic_graph_slam::empty_keyframe_queue() { 105 | std::lock_guard lock(keyframe_queue_mutex); 106 | 107 | if (keyframe_queue_.empty()) { 108 | return false; 109 | } 110 | 111 | int num_processed = 0; 112 | for (int i = 0; 113 | i < std::min(keyframe_queue_.size(), max_keyframes_per_update_); 114 | i++) { 115 | num_processed = i; 116 | 117 | const auto &keyframe = keyframe_queue_[i]; 118 | new_keyframes_.push_back(keyframe); 119 | 120 | Eigen::Isometry3d odom = keyframe->odom; 121 | keyframe->node = graph_slam_->add_se3_node(odom); 122 | // keyframe_hash_[keyframe->stamp] = keyframe; 123 | if (verbose_) 124 | std::cout << "added new keyframe to the graph" << std::endl; 125 | 126 | if (i == 0 && keyframes_.empty()) { 127 | continue; 128 | } 129 | 130 | // add edge between keyframes 131 | const auto &prev_keyframe = 132 | i == 0 ? keyframes_.back() : keyframe_queue_[i - 1]; 133 | 134 | Eigen::Isometry3d relative_pose = 135 | prev_keyframe->odom.inverse() * keyframe->odom; 136 | Eigen::MatrixXd information = 137 | inf_calclator_ 138 | ->calc_information_matrix(); /*keyframe->odom_cov.inverse().cast(); 139 | */ 140 | graph_slam_->add_se3_edge(prev_keyframe->node, keyframe->node, 141 | relative_pose, information); 142 | if (verbose_) 143 | std::cout << "added new odom measurement to the graph" << std::endl; 144 | } 145 | 146 | keyframe_queue_.erase(keyframe_queue_.begin(), 147 | keyframe_queue_.begin() + num_processed + 1); 148 | 149 | return true; 150 | } 151 | 152 | void semantic_graph_slam::empty_landmark_queue( 153 | std::vector current_lan_queue, const auto current_keyframe) { 154 | if (current_lan_queue.empty()) 155 | return; 156 | 157 | for (int i = 0; i < current_lan_queue.size(); ++i) { 158 | // adding new landmark vertex if only the landmark is seen once 159 | if (current_lan_queue[i].is_new_landmark) { 160 | current_lan_queue[i].node = graph_slam_->add_point_xyz_node( 161 | current_lan_queue[i].pose.cast()); 162 | current_lan_queue[i].is_new_landmark = false; 163 | data_ass_obj_->assignLandmarkNode(current_lan_queue[i].id, 164 | current_lan_queue[i].node); 165 | if (verbose_) 166 | std::cout << "added the landmark position node " << std::endl; 167 | } 168 | 169 | // add an edge between landmark and the current keyframe 170 | Eigen::Matrix3f information = current_lan_queue[i].covariance.inverse(); 171 | graph_slam_->add_se3_point_xyz_edge( 172 | current_keyframe->node, current_lan_queue[i].node, 173 | current_lan_queue[i].local_pose.cast(), 174 | information.cast()); 175 | if (verbose_) 176 | std::cout << "added an edge between the landmark and it keyframe " 177 | << std::endl; 178 | } 179 | } 180 | 181 | void semantic_graph_slam::getAndSetLandmarkCov() { 182 | std::vector l; 183 | data_ass_obj_->getMappedLandmarks(l); 184 | g2o::SparseBlockMatrix lan_spinv_vec; 185 | 186 | std::vector> vert_pairs_vec; 187 | for (int i = 0; i < l.size(); ++i) { 188 | l[i].node->unlockQuadraticForm(); 189 | vert_pairs_vec.push_back( 190 | std::make_pair(l[i].node->hessianIndex(), l[i].node->hessianIndex())); 191 | } 192 | 193 | if (graph_slam_->computeLandmarkMarginals((lan_spinv_vec), vert_pairs_vec)) { 194 | for (int i = 0; i < l.size(); ++i) { 195 | // std::cout << "landmark spinv block " << 196 | // lan_spinv_vec.block(l[i].node->hessianIndex(),l[i].node->hessianIndex())->eval() 197 | // << std::endl; 198 | data_ass_obj_->setLandmarkCovs( 199 | i, lan_spinv_vec 200 | .block(l[i].node->hessianIndex(), l[i].node->hessianIndex()) 201 | ->eval() 202 | .cast()); 203 | } 204 | } 205 | } 206 | 207 | std::vector semantic_graph_slam::semantic_data_ass( 208 | const ps_graph_slam::KeyFrame::Ptr curr_keyframe) { 209 | std::vector object_info = curr_keyframe->obj_info; 210 | sensor_msgs::PointCloud2 point_cloud_msg = curr_keyframe->cloud_msg; 211 | Eigen::VectorXf current_robot_pose = ps_graph_slam::matrix2vector( 212 | curr_keyframe->robot_pose.matrix().cast()); 213 | if (verbose_) 214 | std::cout << "current robot pose " << current_robot_pose << std::endl; 215 | 216 | std::vector seg_obj_vec = 217 | pc_seg_obj_->segmentallPointCloudData(current_robot_pose, cam_angle_, 218 | object_info, point_cloud_msg); 219 | if (verbose_) 220 | std::cout << "seg_obj_vec size " << seg_obj_vec.size() << std::endl; 221 | 222 | std::vector current_landmarks_vec = 223 | data_ass_obj_->find_matches(seg_obj_vec, current_robot_pose, cam_angle_); 224 | 225 | if (verbose_) 226 | std::cout << "current_landmarks_vec size " << current_landmarks_vec.size() 227 | << std::endl; 228 | // this->publishDetectedLandmarks(current_robot_pose, seg_obj_vec); 229 | this->setDetectedObjectsPose(seg_obj_vec); 230 | 231 | return current_landmarks_vec; 232 | } 233 | 234 | void semantic_graph_slam::VIOCallback(const ros::Time &stamp, 235 | Eigen::Isometry3d odom, 236 | Eigen::MatrixXf odom_cov) { 237 | // dont update keyframes if the keyframe time and distance is less or no 238 | // detection is available 239 | if (update_keyframes_using_detections_) { 240 | if (!keyframe_updater_->update(odom, stamp) && 241 | !object_detection_available_) { 242 | if (first_key_added_) { 243 | Eigen::Isometry3d pose_inc = prev_odom_.inverse() * odom; 244 | robot_pose_ = robot_pose_ * pose_inc; 245 | } 246 | 247 | this->setVIOPose(odom); 248 | prev_odom_ = odom; 249 | return; 250 | } 251 | } else { 252 | if (!keyframe_updater_->update(odom, stamp)) { 253 | if (first_key_added_) { 254 | Eigen::Isometry3d pose_inc = prev_odom_.inverse() * odom; 255 | robot_pose_ = robot_pose_ * pose_inc; 256 | } 257 | 258 | this->setVIOPose(odom); 259 | prev_odom_ = odom; 260 | return; 261 | } 262 | } 263 | 264 | sensor_msgs::PointCloud2 cloud_msg; 265 | this->getPointCloudData(cloud_msg); 266 | pcl::PointCloud::Ptr cloud( 267 | new pcl::PointCloud()); 268 | 269 | std::vector obj_info; 270 | obj_info.clear(); 271 | if (object_detection_available_) 272 | this->getDetectedObjectInfo(obj_info); 273 | 274 | double accum_d = keyframe_updater_->get_accum_distance(); 275 | ps_graph_slam::KeyFrame::Ptr keyframe(new ps_graph_slam::KeyFrame( 276 | stamp, odom, robot_pose_, odom_cov, accum_d, cloud_msg, cloud, obj_info)); 277 | 278 | std::lock_guard lock(keyframe_queue_mutex); 279 | keyframe_queue_.push_back(keyframe); 280 | if (verbose_) 281 | std::cout << "added keyframe in queue" << std::endl; 282 | 283 | this->setVIOPose(odom); 284 | prev_odom_ = odom; 285 | 286 | return; 287 | } 288 | 289 | void semantic_graph_slam::addFirstPoseAndLandmark() { 290 | std::vector first_lan_vec; 291 | landmark first_landmark; 292 | first_landmark.is_new_landmark = true; 293 | first_landmark.id = 0; 294 | first_landmark.type = "bucket"; 295 | first_landmark.plane_type = "vertical"; 296 | first_landmark.pose << first_lan_x_, first_lan_y_, first_lan_z_; 297 | first_landmark.local_pose = first_landmark.pose; 298 | first_landmark.normal_orientation << -0.4, 0.86, 0, 0; 299 | first_landmark.covariance << 0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1; 300 | 301 | first_lan_vec.push_back(first_landmark); 302 | data_ass_obj_->addFirstLandmark(first_landmark); 303 | 304 | Eigen::Isometry3d odom; 305 | odom.setIdentity(); 306 | Eigen::MatrixXf odom_cov; 307 | odom_cov.setIdentity(6, 6); 308 | double accum_d = 0; 309 | sensor_msgs::PointCloud2 cloud_msg; 310 | pcl::PointCloud::Ptr cloud( 311 | new pcl::PointCloud()); 312 | std::vector obj_info; 313 | 314 | ps_graph_slam::KeyFrame::Ptr keyframe( 315 | new ps_graph_slam::KeyFrame(ros::Time::now(), odom, robot_pose_, odom_cov, 316 | accum_d, cloud_msg, cloud, obj_info)); 317 | keyframe_queue_.push_back(keyframe); 318 | 319 | if (empty_keyframe_queue()) { 320 | for (int i = 0; i < new_keyframes_.size(); ++i) 321 | empty_landmark_queue(first_lan_vec, new_keyframes_[i]); 322 | 323 | std::copy(new_keyframes_.begin(), new_keyframes_.end(), 324 | std::back_inserter(keyframes_)); 325 | new_keyframes_.clear(); 326 | } 327 | 328 | if (verbose_) 329 | std::cout << "add the first landmark and keyframe pose " << std::endl; 330 | 331 | return; 332 | } 333 | 334 | void semantic_graph_slam::setVIOPose(Eigen::Isometry3d vio_pose) { 335 | vio_pose_ = vio_pose; 336 | } 337 | 338 | void semantic_graph_slam::getVIOPose(Eigen::Isometry3d &vio_pose) { 339 | vio_pose = vio_pose_; 340 | } 341 | 342 | void semantic_graph_slam::setPointCloudData( 343 | sensor_msgs::PointCloud2 point_cloud) { 344 | point_cloud_available_ = true; 345 | this->point_cloud_msg_ = point_cloud; 346 | } 347 | 348 | void semantic_graph_slam::getPointCloudData( 349 | sensor_msgs::PointCloud2 &point_cloud) { 350 | point_cloud_available_ = false; 351 | point_cloud = this->point_cloud_msg_; 352 | } 353 | 354 | void semantic_graph_slam::setDetectedObjectInfo( 355 | std::vector object_info) { 356 | object_detection_available_ = true; 357 | this->object_info_ = object_info; 358 | } 359 | 360 | void semantic_graph_slam::getDetectedObjectInfo( 361 | std::vector &object_info) { 362 | object_detection_available_ = false; 363 | object_info = this->object_info_; 364 | } 365 | 366 | void semantic_graph_slam::getMappedLandmarks(std::vector &l_vec) { 367 | data_ass_obj_->getMappedLandmarks(l_vec); 368 | } 369 | 370 | void semantic_graph_slam::getKeyframes( 371 | std::vector &keyframes) { 372 | keyframes = keyframes_; 373 | } 374 | 375 | void semantic_graph_slam::getRobotPose(Eigen::Isometry3d &robot_pose) { 376 | robot_pose = robot_pose_; 377 | } 378 | 379 | void semantic_graph_slam::getMap2OdomTrans(Eigen::Isometry3d &map2odom){ 380 | map2odom = map2odom_; 381 | } 382 | 383 | void semantic_graph_slam::setDetectedObjectsPose( 384 | std::vector seg_obj_vec) { 385 | seg_obj_vec_ = seg_obj_vec; 386 | } 387 | 388 | void semantic_graph_slam::getDetectedObjectsPose( 389 | std::vector &seg_obj_vec) { 390 | seg_obj_vec = seg_obj_vec_; 391 | } 392 | 393 | void semantic_graph_slam::saveGraph(std::string save_graph_path) { 394 | graph_slam_->save(save_graph_path); 395 | std::cout << "saved the graph at " << save_graph_path << std::endl; 396 | } 397 | -------------------------------------------------------------------------------- /src/semantic_graph_SLAM_node.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_graph_slam_ros.h" 2 | 3 | int main(int argc, char **argv) { 4 | 5 | ros::init(argc, argv, "semantic_graph_slam"); 6 | ros::NodeHandle n; 7 | 8 | semantic_graph_slam_ros mySemanticGraphSLAM; 9 | mySemanticGraphSLAM.open(n); 10 | 11 | ros::Rate r(30); 12 | { 13 | 14 | while (ros::ok()) { 15 | // updating all the ros msgs 16 | ros::spinOnce(); 17 | // running the filter 18 | mySemanticGraphSLAM.run(); 19 | r.sleep(); 20 | } 21 | 22 | mySemanticGraphSLAM.computeATE(); 23 | mySemanticGraphSLAM.saveGraph(); 24 | } 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /src/semantic_graph_slam_ros.cpp: -------------------------------------------------------------------------------- 1 | #include "semantic_graph_slam_ros.h" 2 | 3 | semantic_graph_slam_ros::semantic_graph_slam_ros() 4 | // : sync(SyncPolicy(10)) 5 | { 6 | std::cout << "Intialializing semantic graph slam ros node " << std::endl; 7 | } 8 | 9 | semantic_graph_slam_ros::~semantic_graph_slam_ros() { 10 | std::cout << "Destructing semantic graph slam ros node " << std::endl; 11 | } 12 | 13 | void semantic_graph_slam_ros::init(ros::NodeHandle n) { 14 | counter_ = false; 15 | first_gt_pose_ = false; 16 | first_jack_pose_ = false; 17 | verbose_ = false; 18 | 19 | robot_pose_array_.poses.clear(); 20 | vio_pose_array_.poses.clear(); 21 | 22 | robot_pose_vec_.clear(); 23 | vio_key_pose_vec_.clear(); 24 | vio_pose_vec_.clear(); 25 | orb_slam_pose_vec_.clear(); 26 | 27 | jack_yaw_transform_ = -1.57; 28 | 29 | ros::param::param("~verbose", verbose_, false); 30 | ros::param::param("~save_graph", save_graph_, false); 31 | ros::param::param("~save_graph_path", save_graph_path_, 32 | "semantic_graph.g2o"); 33 | ros::param::param("~use_snap_pose", use_snap_pose_, true); 34 | ros::param::param("~use_orb_slam_odom", use_orb_slam_odom_, false); 35 | ros::param::param("~use_rovio_odom", use_rovio_odom_, true); 36 | ros::param::param("~use_rtab_map_odom", use_rtab_map_odom_, false); 37 | ros::param::param("~compute_txt_for_ate", compute_txt_for_ate_, false); 38 | 39 | std::cout << "should save graph: " << save_graph_ << std::endl; 40 | std::cout << "saving graph path: " << save_graph_path_ << std::endl; 41 | std::cout << "using snap pose: " << use_snap_pose_ << std::endl; 42 | std::cout << "using orb slam odom " << use_orb_slam_odom_ << std::endl; 43 | std::cout << "using rovio odom " << use_rovio_odom_ << std::endl; 44 | std::cout << "using rtab map odom " << use_rtab_map_odom_ << std::endl; 45 | std::cout << "computing txt for ate " << compute_txt_for_ate_ << std::endl; 46 | 47 | semantic_gslam_obj_.reset(new semantic_graph_slam()); 48 | semantic_gslam_obj_->init(verbose_); 49 | } 50 | 51 | void semantic_graph_slam_ros::run() { 52 | if (semantic_gslam_obj_->run()) { 53 | if (use_rtab_map_odom_) this->transformListener(); 54 | this->publishLandmarks(); 55 | this->publishKeyframePoses(); 56 | this->publishDetectedLandmarks(); 57 | } 58 | 59 | this->publishCorresVIOPose(); 60 | this->publishRobotPose(); 61 | this->publishMap2Odom(); 62 | return; 63 | } 64 | 65 | void semantic_graph_slam_ros::open(ros::NodeHandle n) { 66 | init(n); 67 | 68 | // subscribers 69 | rvio_odom_pose_sub_ = n.subscribe( 70 | "/rovio/odometry", 1, &semantic_graph_slam_ros::rovioVIOCallback, this); 71 | snap_odom_pose_sub_ = 72 | n.subscribe("/SQ04/snap_vislam/vislam/pose", 1, 73 | &semantic_graph_slam_ros::snapVIOCallback, this); 74 | orb_slam_pose_sub_ = n.subscribe( 75 | "orb_slam/pose", 1, &semantic_graph_slam_ros::orbslamPoseCallback, this); 76 | jackal_odom_pose_sub_ = 77 | n.subscribe("/JA01/odometry/filtered", 1, 78 | &semantic_graph_slam_ros::jackalOdomCallback, this); 79 | cloud_sub_ = n.subscribe("/depth_registered/points", 1, 80 | &semantic_graph_slam_ros::PointCloudCallback, this); 81 | detected_object_sub_ = n.subscribe( 82 | "/darknet_ros/bounding_boxes", 1, 83 | &semantic_graph_slam_ros::detectedObjectDarknetCallback, this); 84 | simple_detected_object_sub_ = 85 | n.subscribe("/image_processed/bounding_boxes", 1, 86 | &semantic_graph_slam_ros::detectedObjectSimpleCallback, this); 87 | optitrack_pose_sub_ = 88 | n.subscribe("/vrpn_client_node/realsense/pose", 1, 89 | &semantic_graph_slam_ros::optitrackPoseCallback, this); 90 | vicon_pose_sub_ = n.subscribe( 91 | "/SQ04/vicon", 1, &semantic_graph_slam_ros::viconPoseSubCallback, this); 92 | 93 | // publishers 94 | keyframe_pose_pub_ = 95 | n.advertise("keyframe_poses", 1); 96 | landmarks_pub_ = 97 | n.advertise("mapped_landmarks", 1); 98 | detected_lans_pub_ = 99 | n.advertise("detected_landmarks", 1); 100 | robot_pose_pub_ = n.advertise("robot_pose", 1); 101 | robot_transform_pub_ = 102 | n.advertise("robot_transform", 1); 103 | keyframe_path_pub_ = n.advertise("robot_path", 1); 104 | optitrack_pose_pub_ = 105 | n.advertise("optitrack_pose", 1); 106 | optitrack_path_pub_ = n.advertise("optitrack_path", 1); 107 | corres_vio_pose_pub_ = 108 | n.advertise("corres_vo_pose", 1); 109 | corres_vio_path_ = n.advertise("corres_vo_path", 1); 110 | map_points_pub_ = n.advertise("map_points", 1); 111 | } 112 | 113 | void semantic_graph_slam_ros::rovioVIOCallback( 114 | const nav_msgs::Odometry::ConstPtr& odom_msg) { 115 | const ros::Time& stamp = odom_msg->header.stamp; 116 | Eigen::Isometry3d odom; 117 | Eigen::MatrixXf odom_cov; 118 | 119 | if (use_rovio_odom_) { 120 | odom = ps_graph_slam::odom2isometry(odom_msg); 121 | odom_cov = ps_graph_slam::arrayToMatrix(odom_msg); 122 | } else if (use_rtab_map_odom_) { 123 | nav_msgs::OdometryPtr odom_msg_conv; 124 | odom_msg_conv = ps_graph_slam::PoseCam2Robot(odom_msg); 125 | odom = ps_graph_slam::odom2isometry(odom_msg); 126 | odom_cov = ps_graph_slam::arrayToMatrix(odom_msg); 127 | } 128 | 129 | semantic_gslam_obj_->VIOCallback(stamp, odom, odom_cov); 130 | 131 | return; 132 | } 133 | 134 | void semantic_graph_slam_ros::snapVIOCallback( 135 | const geometry_msgs::PoseStamped& pose_msg) { 136 | const ros::Time& stamp = ros::Time::now(); 137 | Eigen::Isometry3d odom; 138 | Eigen::MatrixXf odom_cov; 139 | odom_cov.setIdentity(6, 6); // manually adding pose cov 140 | 141 | if (use_snap_pose_) { 142 | geometry_msgs::PoseStamped pose_enu = ps_graph_slam::poseNED2ENU(pose_msg); 143 | odom = ps_graph_slam::pose2isometry(pose_enu); 144 | } else if (use_orb_slam_odom_) { 145 | odom = ps_graph_slam::pose2isometry(pose_msg); 146 | } 147 | 148 | semantic_gslam_obj_->VIOCallback(stamp, odom, odom_cov); 149 | 150 | return; 151 | } 152 | 153 | void semantic_graph_slam_ros::orbslamPoseCallback( 154 | const geometry_msgs::PoseStamped& pose_msg) { 155 | orb_slam_pose_vec_.push_back(pose_msg); 156 | 157 | return; 158 | } 159 | 160 | void semantic_graph_slam_ros::jackalOdomCallback( 161 | const nav_msgs::OdometryConstPtr& odom_msg) { 162 | const ros::Time& stamp = odom_msg->header.stamp; 163 | nav_msgs::OdometryPtr odom_msg_convert = 164 | ps_graph_slam::RotPoseZ(odom_msg, jack_yaw_transform_); 165 | if (!first_jack_pose_) { 166 | jack_x_transform_ = odom_msg_convert->pose.pose.position.x; 167 | jack_y_transform_ = odom_msg_convert->pose.pose.position.y; 168 | jack_z_transform_ = odom_msg_convert->pose.pose.position.z; 169 | 170 | first_jack_pose_ = true; 171 | } 172 | 173 | nav_msgs::OdometryPtr odom_msg_orig = 174 | ps_graph_slam::navMsgsToOrigin(odom_msg_convert, jack_x_transform_, 175 | jack_y_transform_, jack_z_transform_); 176 | Eigen::Isometry3d odom = ps_graph_slam::odom2isometry(odom_msg_orig); 177 | Eigen::MatrixXf odom_cov; 178 | odom_cov.setIdentity(6, 6); // jackal pose covariance is bad 179 | 180 | semantic_gslam_obj_->VIOCallback(stamp, odom, odom_cov); 181 | 182 | return; 183 | } 184 | 185 | void semantic_graph_slam_ros::PointCloudCallback( 186 | const sensor_msgs::PointCloud2& msg) { 187 | semantic_gslam_obj_->setPointCloudData(msg); 188 | pc_stamp_ = msg.header.stamp; 189 | } 190 | 191 | void semantic_graph_slam_ros::detectedObjectDarknetCallback( 192 | const semantic_SLAM::BoundingBoxes& msg) { 193 | std::vector object_info; 194 | object_info.resize(msg.bounding_boxes.size()); 195 | 196 | for (int i = 0; i < msg.bounding_boxes.size(); ++i) { 197 | object_info[i].type = msg.bounding_boxes[i].Class; 198 | object_info[i].tl_x = msg.bounding_boxes[i].xmin; 199 | object_info[i].tl_y = msg.bounding_boxes[i].ymin; 200 | object_info[i].height = 201 | abs(msg.bounding_boxes[i].ymax - msg.bounding_boxes[i].ymin); 202 | object_info[i].width = 203 | abs(msg.bounding_boxes[i].xmax - msg.bounding_boxes[i].xmin); 204 | object_info[i].prob = msg.bounding_boxes[i].probability; 205 | } 206 | 207 | semantic_gslam_obj_->setDetectedObjectInfo(object_info); 208 | } 209 | 210 | void semantic_graph_slam_ros::detectedObjectSimpleCallback( 211 | const semantic_SLAM::DetectedObjects& msg) { 212 | std::vector object_info; 213 | object_info.resize(msg.objects.size()); 214 | 215 | for (int i = 0; i < msg.objects.size(); ++i) { 216 | object_info[i].type = msg.objects[i].type; 217 | object_info[i].tl_x = msg.objects[i].tl_x; 218 | object_info[i].tl_y = msg.objects[i].tl_y; 219 | object_info[i].height = msg.objects[i].height; 220 | object_info[i].width = msg.objects[i].width; 221 | object_info[i].prob = msg.objects[i].prob; 222 | } 223 | 224 | semantic_gslam_obj_->setDetectedObjectInfo(object_info); 225 | } 226 | 227 | void semantic_graph_slam_ros::publishLandmarks() { 228 | visualization_msgs::MarkerArray marker_arrays; 229 | int marker_id = 0; 230 | std::vector l_vec; 231 | semantic_gslam_obj_->getMappedLandmarks(l_vec); 232 | 233 | for (int i = 0; i < l_vec.size(); ++i) { 234 | Eigen::Vector3d lan_node_pose = l_vec[i].node->estimate(); 235 | 236 | visualization_msgs::Marker marker; 237 | marker.header.stamp = ros::Time::now(); 238 | marker.header.frame_id = "map"; 239 | marker.ns = "my_namespace"; 240 | marker.id = marker_id; 241 | marker.action = visualization_msgs::Marker::ADD; 242 | marker.type = visualization_msgs::Marker::CUBE; 243 | marker.pose.position.x = lan_node_pose(0); 244 | marker.pose.position.y = lan_node_pose(1); 245 | marker.pose.position.z = lan_node_pose(2); 246 | marker.pose.orientation.x = 0.0; 247 | marker.pose.orientation.y = 0.0; 248 | marker.pose.orientation.z = 0.0; 249 | marker.pose.orientation.w = 1.0; 250 | 251 | if (l_vec[i].plane_type == "horizontal") { 252 | marker.scale.x = 0.3; 253 | marker.scale.y = 0.3; 254 | marker.scale.z = 0.01; 255 | } else if (l_vec[i].plane_type == "vertical") { 256 | marker.scale.x = 0.01; 257 | marker.scale.y = 0.3; 258 | marker.scale.z = 0.3; 259 | } 260 | 261 | if (l_vec[i].type == "chair") { 262 | marker.color.a = 1.0; // Don't forget to set the alpha! 263 | marker.color.r = 0.0; 264 | marker.color.g = 1.0; 265 | marker.color.b = 0.0; 266 | } else if (l_vec[i].type == "tvmonitor") { 267 | marker.color.a = 1.0; // Don't forget to set the alpha! 268 | marker.color.r = 1.0; 269 | marker.color.g = 0.0; 270 | marker.color.b = 0.0; 271 | } else if (l_vec[i].type == "laptop") { 272 | marker.color.a = 1.0; // Don't forget to set the alpha! 273 | marker.color.r = 1.0; 274 | marker.color.g = 1.0; 275 | marker.color.b = 0.0; 276 | } else if (l_vec[i].type == "keyboard") { 277 | marker.color.a = 1.0; // Don't forget to set the alpha! 278 | marker.color.r = 1.0; 279 | marker.color.g = 0.0; 280 | marker.color.b = 1.0; 281 | } else if (l_vec[i].type == "book") { 282 | marker.color.a = 1.0; // Don't forget to set the alpha! 283 | marker.color.r = 0.0; 284 | marker.color.g = 0.0; 285 | marker.color.b = 1.0; 286 | } else if (l_vec[i].type == "bucket") { 287 | marker.color.a = 1.0; // Don't forget to set the alpha! 288 | marker.color.r = 0.0; 289 | marker.color.g = 0.0; 290 | marker.color.b = 1.0; 291 | } else if (l_vec[i].type == "car") { 292 | marker.color.a = 1.0; // Don't forget to set the alpha! 293 | marker.color.r = 1.0; 294 | marker.color.g = 1.0; 295 | marker.color.b = 0.0; 296 | } 297 | 298 | marker_arrays.markers.push_back(marker); 299 | marker_id++; 300 | } 301 | 302 | landmarks_pub_.publish(marker_arrays); 303 | } 304 | 305 | void semantic_graph_slam_ros::publishDetectedLandmarks() { 306 | std::vector det_obj_info; 307 | semantic_gslam_obj_->getDetectedObjectsPose(det_obj_info); 308 | 309 | visualization_msgs::MarkerArray marker_arrays; 310 | int marker_id = 0; 311 | 312 | for (int i = 0; i < det_obj_info.size(); ++i) { 313 | visualization_msgs::Marker marker; 314 | marker.header.stamp = ros::Time::now(); 315 | marker.header.frame_id = "map"; 316 | marker.ns = "my_namespace"; 317 | marker.id = marker_id; 318 | marker.action = visualization_msgs::Marker::ADD; 319 | marker.type = visualization_msgs::Marker::CUBE; 320 | marker.pose.position.x = det_obj_info[i].world_pose(0); 321 | marker.pose.position.y = det_obj_info[i].world_pose(1); 322 | marker.pose.position.z = det_obj_info[i].world_pose(2); 323 | marker.pose.orientation.x = 0.0; 324 | marker.pose.orientation.y = 0.0; 325 | marker.pose.orientation.z = 0.0; 326 | marker.pose.orientation.w = 1.0; 327 | marker.color.a = 1.0; // Don't forget to set the alpha! 328 | marker.color.r = 1.0; 329 | marker.color.g = 1.0; 330 | marker.color.b = 1.0; 331 | 332 | if (det_obj_info[i].plane_type == "horizontal") { 333 | marker.scale.x = 0.3; 334 | marker.scale.y = 0.3; 335 | marker.scale.z = 0.01; 336 | } else if (det_obj_info[i].plane_type == "vertical") { 337 | marker.scale.x = 0.01; 338 | marker.scale.y = 0.3; 339 | marker.scale.z = 0.3; 340 | } 341 | 342 | marker_arrays.markers.push_back(marker); 343 | marker_id++; 344 | } 345 | 346 | detected_lans_pub_.publish(marker_arrays); 347 | } 348 | 349 | void semantic_graph_slam_ros::publishKeyframePoses() { 350 | ros::Time current_time = ros::Time::now(); 351 | 352 | geometry_msgs::PoseArray pose_array; 353 | pose_array.header.stamp = current_time; 354 | pose_array.header.frame_id = "map"; 355 | 356 | nav_msgs::Path final_path; 357 | final_path.header.stamp = current_time; 358 | final_path.header.frame_id = "map"; 359 | 360 | // copying the new keyframes for publishing 361 | std::vector keyframes; 362 | semantic_gslam_obj_->getKeyframes(keyframes); 363 | geometry_msgs::Pose key_pose; 364 | geometry_msgs::PoseStamped key_pose_stamped; 365 | geometry_msgs::PoseStamped vio_key_pose_stamped; 366 | 367 | vio_key_pose_vec_.clear(); 368 | robot_pose_vec_.clear(); 369 | for (int i = 0; i < keyframes.size(); ++i) { 370 | key_pose = ps_graph_slam::matrix2pose( 371 | ros::Time::now(), keyframes[i]->node->estimate().matrix().cast(), 372 | "map"); 373 | 374 | key_pose_stamped.header.stamp = keyframes[i]->stamp; 375 | key_pose_stamped.pose = key_pose; 376 | 377 | pose_array.poses.push_back(key_pose); 378 | final_path.poses.push_back(key_pose_stamped); 379 | robot_pose_vec_.push_back(key_pose_stamped); 380 | 381 | vio_key_pose_stamped = ps_graph_slam::matrix2posestamped( 382 | keyframes[i]->stamp, keyframes[i]->odom.matrix().cast(), "map"); 383 | vio_key_pose_vec_.push_back(vio_key_pose_stamped); 384 | } 385 | 386 | keyframe_pose_pub_.publish(pose_array); 387 | keyframe_path_pub_.publish(final_path); 388 | } 389 | 390 | void semantic_graph_slam_ros::publishRobotPose() { 391 | Eigen::Isometry3d robot_pose; 392 | semantic_gslam_obj_->getRobotPose(robot_pose); 393 | 394 | geometry_msgs::PoseStamped robot_pose_msg = ps_graph_slam::matrix2posestamped( 395 | ros::Time::now(), robot_pose.matrix().cast(), "map"); 396 | // this->publishRobotPoseTF(robot_pose_msg); 397 | 398 | geometry_msgs::TransformStamped robot_transform; 399 | robot_transform.header.stamp = pc_stamp_; 400 | robot_transform.header.frame_id = "map"; 401 | robot_transform.child_frame_id = "base_link"; 402 | robot_transform.transform.translation.x = robot_pose_msg.pose.position.x; 403 | robot_transform.transform.translation.y = robot_pose_msg.pose.position.y; 404 | robot_transform.transform.translation.z = robot_pose_msg.pose.position.z; 405 | robot_transform.transform.rotation = robot_pose_msg.pose.orientation; 406 | 407 | robot_transform_pub_.publish(robot_transform); 408 | robot_pose_pub_.publish(robot_pose_msg); 409 | } 410 | 411 | void semantic_graph_slam_ros::publishRobotPoseTF( 412 | geometry_msgs::PoseStamped robot_pose) { 413 | static tf::TransformBroadcaster br; 414 | tf::Transform transform; 415 | transform.setOrigin(tf::Vector3(robot_pose.pose.position.x, 416 | robot_pose.pose.position.y, 417 | robot_pose.pose.position.z)); 418 | tf::Quaternion tf_quat; 419 | tf_quat[0] = robot_pose.pose.orientation.x; 420 | tf_quat[1] = robot_pose.pose.orientation.y; 421 | tf_quat[2] = robot_pose.pose.orientation.z; 422 | tf_quat[3] = robot_pose.pose.orientation.w; 423 | transform.setRotation(tf_quat); 424 | br.sendTransform(tf::StampedTransform(transform, robot_pose.header.stamp, 425 | "map", "base_link")); 426 | } 427 | 428 | void semantic_graph_slam_ros::publishMap2Odom() { 429 | Eigen::Isometry3d map2odom; 430 | semantic_gslam_obj_->getMap2OdomTrans(map2odom); 431 | 432 | geometry_msgs::PoseStamped map2odom_msg = ps_graph_slam::matrix2posestamped( 433 | ros::Time::now(), map2odom.matrix().cast(), "map"); 434 | this->publishMap2OdomTF(map2odom_msg); 435 | } 436 | 437 | void semantic_graph_slam_ros::publishMap2OdomTF( 438 | geometry_msgs::PoseStamped map2odom_pose) { 439 | static tf::TransformBroadcaster br; 440 | tf::Transform transform; 441 | transform.setOrigin(tf::Vector3(map2odom_pose.pose.position.x, 442 | map2odom_pose.pose.position.y, 443 | map2odom_pose.pose.position.z)); 444 | tf::Quaternion tf_quat; 445 | tf_quat[0] = map2odom_pose.pose.orientation.x; 446 | tf_quat[1] = map2odom_pose.pose.orientation.y; 447 | tf_quat[2] = map2odom_pose.pose.orientation.z; 448 | tf_quat[3] = map2odom_pose.pose.orientation.w; 449 | transform.setRotation(tf_quat); 450 | br.sendTransform(tf::StampedTransform(transform, map2odom_pose.header.stamp, 451 | "map", "odom")); 452 | } 453 | 454 | void semantic_graph_slam_ros::optitrackPoseCallback( 455 | const nav_msgs::Odometry& msg) { 456 | geometry_msgs::PoseStamped optitrack_pose; 457 | optitrack_pose.header.stamp = ros::Time::now(); 458 | optitrack_pose.header.frame_id = "map"; 459 | 460 | optitrack_pose.pose = msg.pose.pose; 461 | optitrack_pose_pub_.publish(optitrack_pose); 462 | 463 | optitrack_pose_vec_.push_back(optitrack_pose); 464 | nav_msgs::Path optitrack_path; 465 | optitrack_path.header.stamp = msg.header.stamp; 466 | optitrack_path.header.frame_id = "map"; 467 | optitrack_path.poses = optitrack_pose_vec_; 468 | optitrack_path_pub_.publish(optitrack_path); 469 | } 470 | 471 | void semantic_graph_slam_ros::viconPoseSubCallback( 472 | const semantic_SLAM::ViconState& msg) { 473 | if (!first_gt_pose_) { 474 | gt_x_transform_ = -msg.pose.position.x; 475 | gt_y_transform_ = -msg.pose.position.y; 476 | gt_z_transform_ = -msg.pose.position.z; 477 | first_gt_pose_ = true; 478 | } 479 | 480 | geometry_msgs::PoseStamped vicon_pose; 481 | vicon_pose.header.stamp = ros::Time::now(); 482 | vicon_pose.header.frame_id = "map"; 483 | 484 | vicon_pose.pose.position.x = msg.pose.position.x + gt_x_transform_; 485 | vicon_pose.pose.position.y = msg.pose.position.y + gt_y_transform_; 486 | vicon_pose.pose.position.z = msg.pose.position.z + gt_z_transform_; 487 | vicon_pose.pose.orientation = msg.pose.orientation; 488 | 489 | optitrack_pose_pub_.publish(vicon_pose); 490 | optitrack_pose_vec_.push_back(vicon_pose); 491 | 492 | nav_msgs::Path vicon_path; 493 | vicon_path.header.stamp = msg.header.stamp; 494 | vicon_path.header.frame_id = "map"; 495 | vicon_path.poses = optitrack_pose_vec_; 496 | optitrack_path_pub_.publish(vicon_path); 497 | } 498 | 499 | void semantic_graph_slam_ros::transformListener() { 500 | tf::StampedTransform transform; 501 | try { 502 | gt_pose_listener_.lookupTransform("/world", "/openni_camera", ros::Time(0), 503 | transform); 504 | } catch (tf::TransformException& ex) { 505 | ROS_ERROR("%s", ex.what()); 506 | // ros::Duration(1.0).sleep(); 507 | } 508 | 509 | geometry_msgs::PoseStamped vicon_pose; 510 | vicon_pose.header.stamp = transform.stamp_; 511 | vicon_pose.header.frame_id = "map"; 512 | 513 | vicon_pose.pose.position.x = transform.getOrigin().x(); 514 | vicon_pose.pose.position.y = transform.getOrigin().y(); 515 | vicon_pose.pose.position.z = transform.getOrigin().z(); 516 | vicon_pose.pose.orientation.x = transform.getRotation().x(); 517 | vicon_pose.pose.orientation.y = transform.getRotation().y(); 518 | vicon_pose.pose.orientation.z = transform.getRotation().z(); 519 | vicon_pose.pose.orientation.w = transform.getRotation().w(); 520 | 521 | optitrack_pose_pub_.publish(vicon_pose); 522 | optitrack_pose_vec_.push_back(vicon_pose); 523 | 524 | nav_msgs::Path vicon_path; 525 | vicon_path.header.stamp = ros::Time::now(); 526 | vicon_path.header.frame_id = "map"; 527 | vicon_path.poses = optitrack_pose_vec_; 528 | optitrack_path_pub_.publish(vicon_path); 529 | } 530 | 531 | void semantic_graph_slam_ros::publishCorresVIOPose() { 532 | Eigen::Isometry3d vio_pose; 533 | semantic_gslam_obj_->getVIOPose(vio_pose); 534 | 535 | ros::Time current_time = ros::Time::now(); 536 | geometry_msgs::PoseStamped corres_vio_pose_msg = 537 | ps_graph_slam::matrix2posestamped( 538 | current_time, vio_pose.matrix().cast(), "odom"); 539 | 540 | corres_vio_pose_pub_.publish(corres_vio_pose_msg); 541 | this->publishVIOTF(corres_vio_pose_msg); 542 | 543 | nav_msgs::Path vio_path; 544 | vio_path.header.stamp = current_time; 545 | vio_path.header.frame_id = "odom"; 546 | 547 | vio_pose_vec_.push_back(corres_vio_pose_msg); 548 | vio_path.poses = vio_pose_vec_; 549 | corres_vio_path_.publish(vio_path); 550 | } 551 | 552 | void semantic_graph_slam_ros::publishVIOTF( 553 | geometry_msgs::PoseStamped vio_pose) { 554 | static tf::TransformBroadcaster br; 555 | tf::Transform transform; 556 | transform.setOrigin(tf::Vector3(vio_pose.pose.position.x, 557 | vio_pose.pose.position.y, 558 | vio_pose.pose.position.z)); 559 | tf::Quaternion tf_quat; 560 | tf_quat[0] = vio_pose.pose.orientation.x; 561 | tf_quat[1] = vio_pose.pose.orientation.y; 562 | tf_quat[2] = vio_pose.pose.orientation.z; 563 | tf_quat[3] = vio_pose.pose.orientation.w; 564 | transform.setRotation(tf_quat); 565 | br.sendTransform(tf::StampedTransform(transform, vio_pose.header.stamp, 566 | "odom", "base_link")); 567 | } 568 | 569 | void semantic_graph_slam_ros::saveGraph() { 570 | if (save_graph_) semantic_gslam_obj_->saveGraph(save_graph_path_); 571 | } 572 | 573 | void semantic_graph_slam_ros::computeATE() { 574 | if (compute_txt_for_ate_) { 575 | // writing the txt for robot 576 | std::ofstream robot_data; 577 | robot_data.open("/home/hriday/Desktop/robot_pose.txt", 578 | std::ios::out | std::ios::ate | std::ios::app); 579 | robot_data << "#timestamp ,tx,ty,tz,qx,qy,qz,qw" << std::endl; 580 | robot_data.close(); 581 | 582 | for (int i = 0; i < robot_pose_vec_.size(); ++i) { 583 | robot_data.open("/home/hriday/Desktop/robot_pose.txt", 584 | std::ios::out | std::ios::ate | std::ios::app); 585 | robot_data << robot_pose_vec_[i].header.stamp << " " 586 | << robot_pose_vec_[i].pose.position.x << " " 587 | << robot_pose_vec_[i].pose.position.y << " " 588 | << robot_pose_vec_[i].pose.position.z << " " 589 | << robot_pose_vec_[i].pose.orientation.x << " " 590 | << robot_pose_vec_[i].pose.orientation.y << " " 591 | << robot_pose_vec_[i].pose.orientation.z << " " 592 | << robot_pose_vec_[i].pose.orientation.w << std::endl; 593 | robot_data.close(); 594 | } 595 | 596 | // writing the text for vio data 597 | std::ofstream vio_data; 598 | vio_data.open("/home/hriday/Desktop/vio_pose.txt", 599 | std::ios::out | std::ios::ate | std::ios::app); 600 | vio_data << "#timestamp ,tx,ty,tz,qx,qy,qz,qw" << std::endl; 601 | vio_data.close(); 602 | 603 | for (int i = 0; i < vio_key_pose_vec_.size(); ++i) { 604 | vio_data.open("/home/hriday/Desktop/vio_pose.txt", 605 | std::ios::out | std::ios::ate | std::ios::app); 606 | vio_data << vio_key_pose_vec_[i].header.stamp << " " 607 | << vio_key_pose_vec_[i].pose.position.x << " " 608 | << vio_key_pose_vec_[i].pose.position.y << " " 609 | << vio_key_pose_vec_[i].pose.position.z << " " 610 | << vio_key_pose_vec_[i].pose.orientation.x << " " 611 | << vio_key_pose_vec_[i].pose.orientation.y << " " 612 | << vio_key_pose_vec_[i].pose.orientation.z << " " 613 | << vio_key_pose_vec_[i].pose.orientation.w << std::endl; 614 | vio_data.close(); 615 | } 616 | 617 | // writing the text for gt data 618 | std::ofstream gt_data; 619 | gt_data.open("/home/hriday/Desktop/gt_pose.txt", 620 | std::ios::out | std::ios::ate | std::ios::app); 621 | gt_data << "#timestamp ,tx,ty,tz,qx,qy,qz,qw" << std::endl; 622 | gt_data.close(); 623 | 624 | for (int i = 0; i < optitrack_pose_vec_.size(); ++i) { 625 | gt_data.open("/home/hriday/Desktop/gt_pose.txt", 626 | std::ios::out | std::ios::ate | std::ios::app); 627 | gt_data << optitrack_pose_vec_[i].header.stamp << " " 628 | << optitrack_pose_vec_[i].pose.position.x << " " 629 | << optitrack_pose_vec_[i].pose.position.y << " " 630 | << optitrack_pose_vec_[i].pose.position.z << " " 631 | << optitrack_pose_vec_[i].pose.orientation.x << " " 632 | << optitrack_pose_vec_[i].pose.orientation.y << " " 633 | << optitrack_pose_vec_[i].pose.orientation.z << " " 634 | << optitrack_pose_vec_[i].pose.orientation.w << std::endl; 635 | gt_data.close(); 636 | } 637 | 638 | // writing orb slam data for comparision 639 | std::ofstream orb_slam_data; 640 | orb_slam_data.open("/home/hriday/Desktop/orb_slam_pose.txt", 641 | std::ios::out | std::ios::ate | std::ios::app); 642 | orb_slam_data << "#timestamp ,tx,ty,tz,qx,qy,qz,qw" << std::endl; 643 | orb_slam_data.close(); 644 | 645 | for (int i = 0; i < orb_slam_pose_vec_.size(); ++i) { 646 | orb_slam_data.open("/home/hriday/Desktop/orb_slam_pose.txt", 647 | std::ios::out | std::ios::ate | std::ios::app); 648 | orb_slam_data << orb_slam_pose_vec_[i].header.stamp << " " 649 | << orb_slam_pose_vec_[i].pose.position.x << " " 650 | << orb_slam_pose_vec_[i].pose.position.y << " " 651 | << orb_slam_pose_vec_[i].pose.position.z << " " 652 | << orb_slam_pose_vec_[i].pose.orientation.x << " " 653 | << orb_slam_pose_vec_[i].pose.orientation.y << " " 654 | << orb_slam_pose_vec_[i].pose.orientation.z << " " 655 | << orb_slam_pose_vec_[i].pose.orientation.w << std::endl; 656 | orb_slam_data.close(); 657 | } 658 | } 659 | } -------------------------------------------------------------------------------- /travis_build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | mkdir -p /root/semantic_slam_build_ws/src 4 | cd /root/semantic_slam_build_ws/src 5 | git clone https://github.com/hridaybavle/semantic_slam.git 6 | source /opt/ros/kinetic/setup.bash 7 | cd .. && catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release 8 | --------------------------------------------------------------------------------