diff --git a/.gitignore b/.gitignore index 5caa5d8..f758c83 100644 --- a/.gitignore +++ b/.gitignore @@ -59,4 +59,4 @@ open3d_catkin/tmp /open3d_slam_ros/data/* !/open3d_slam_ros/data/.gitkeep documentation/build - +ros/examples_standalone/open3d_rosbag_mapper_mesher/data/ diff --git a/open3d_catkin/CMakeLists.txt b/open3d_catkin/CMakeLists.txt index 076cab7..ac930b3 100644 --- a/open3d_catkin/CMakeLists.txt +++ b/open3d_catkin/CMakeLists.txt @@ -4,162 +4,138 @@ project(open3d_catkin) find_package(catkin REQUIRED) set(CATKIN_PACKAGE_DEPENDENCIES - roscpp - roslib + roscpp + roslib ) find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} + ${CATKIN_PACKAGE_DEPENDENCIES} ) # Check whether open3d can be found find_package(Open3D QUIET) # Color -if(NOT WIN32) - string(ASCII 27 Esc) - set(ColourReset "${Esc}[m") - set(BoldMagenta "${Esc}[1;35m") - set(Magenta "${Esc}[35m") -endif() +if (NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(BoldMagenta "${Esc}[1;35m") + set(Magenta "${Esc}[35m") +endif () # 1 - Check whether variable is set ----------------------------------------------------------- -if(DEFINED ENV{Open3D_DIR}) - set(Open3D_DIR ENV{Open3D_DIR}) - find_package(Open3D CONFIG REQUIRED) - message("${BoldMagenta}INFO: Found manually set path to Open3D. Using version located at $ENV{Open3D_DIR}.${ColourReset}") - set(Open3D_INCLUDE_DIR_MAN "${Open3D_DIR}/../../../include/") - message("Open3D include path: ${Open3D_INCLUDE_DIRS} , ${Open3D_INCLUDE_DIR_MAN}") - message("Open3D libs: ${Open3D_LIBRARIES}") - catkin_package( - INCLUDE_DIRS - ${Open3D_INCLUDE_DIRS} - ${Open3D_INCLUDE_DIR_MAN} - LIBRARIES - ${Open3D_LIBRARIES} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Open3D - ) -# 2 - Check whether open3d is installed ------------------------------------------------------- -elseif(Open3D_FOUND) - message("${BoldMagenta}INFO: Found Open3D.${ColourReset}") - - # Path differs whether installed from PPA or (locally) from source - if(EXISTS "${Open3D_DIR}/../../../../include/") - set(Open3D_INCLUDE_DIR_MAN "${Open3D_DIR}/../../../../include/") - else() +if (DEFINED ENV{Open3D_DIR}) + set(Open3D_DIR ENV{Open3D_DIR}) + find_package(Open3D CONFIG REQUIRED) + message("${BoldMagenta}INFO: Found manually set path to Open3D. Using version located at $ENV{Open3D_DIR}.${ColourReset}") set(Open3D_INCLUDE_DIR_MAN "${Open3D_DIR}/../../../include/") - endif() - - message("Open3D libraries: ${Open3D_LIBRARIES}") - message("Open3D include path: ${Open3D_INCLUDE_DIRS} , ${Open3D_INCLUDE_DIR_MAN}") - - # FMT - find_package(fmt QUIET) - if(fmt_FOUND) - message("Linking fmt into open3d_catkin") - catkin_package( - INCLUDE_DIRS - ${Open3D_INCLUDE_DIRS} - ${Open3D_INCLUDE_DIR_MAN} - LIBRARIES - ${Open3D_LIBRARIES} - fmt::fmt - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Open3D + message("Open3D include path: ${Open3D_INCLUDE_DIRS} , ${Open3D_INCLUDE_DIR_MAN}") + message("Open3D libs: ${Open3D_LIBRARIES}") + catkin_package( + INCLUDE_DIRS + ${Open3D_INCLUDE_DIRS} + ${Open3D_INCLUDE_DIR_MAN} + LIBRARIES + ${Open3D_LIBRARIES} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + Open3D ) - else() - message("fmt not found. If you get linking errors try installing libfmt with sudo apt install libfmt-dev") - catkin_package( - INCLUDE_DIRS - ${Open3D_INCLUDE_DIRS} - ${Open3D_INCLUDE_DIR_MAN} - LIBRARIES - ${Open3D_LIBRARIES} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Open3D - ) - endif() - - # Linking other packages - catkin_package( - INCLUDE_DIRS - ${Open3D_INCLUDE_DIRS} - ${Open3D_INCLUDE_DIR_MAN} - LIBRARIES - ${Open3D_LIBRARIES} - CATKIN_DEPENDS - ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - Open3D - ) - - -# 3 - Compile in catkin workspace ------------------------------------------------------------- -else() - message("${BoldMagenta}INFO: Neither variable Open3d_DIR is set, nor could Open3D be found, hence compiling in workspace.${ColourReset}") - message("${BoldMagenta}INFO: This automatic compilation from source requires CMAKE>3.18.${ColourReset}") - - # Newer version of CMake is required - cmake_minimum_required(VERSION 3.18) - - # Catkinization of open3D - include(ExternalProject) - file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) - - ExternalProject_Add(open3d - GIT_REPOSITORY "https://github.com/isl-org/Open3D" - GIT_TAG "v0.15.1" - GIT_PROGRESS "true" - CMAKE_CACHE_ARGS "-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=true" - PREFIX "${CMAKE_SOURCE_DIR}" - SOURCE_DIR "${CMAKE_SOURCE_DIR}/open3d" - BINARY_DIR "${CMAKE_SOURCE_DIR}/tmp" - CMAKE_ARGS - -DBUILD_SHARED_LIBS=ON - -DCMAKE_BUILD_TYPE=RelWithDebInfo - -DUSE_SYSTEM_EIGEN3=OFF - -DGLIBCXX_USE_CXX11_ABI=ON - -DBUILD_PYTHON_MODULE=OFF - -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} - INSTALL_COMMAND make install && bash "-c" "cp -r ${CMAKE_SOURCE_DIR}/tmp/fmt/include/ ${CATKIN_DEVEL_PREFIX} && cp ${CMAKE_SOURCE_DIR}/tmp/fmt/lib/*.a ${CATKIN_DEVEL_PREFIX}/lib/" - ) - - catkin_package( - INCLUDE_DIRS - ${CATKIN_DEVEL_PREFIX}/include - LIBRARIES - ${CATKIN_DEVEL_PREFIX}/lib/libOpen3D.so - ${CATKIN_DEVEL_PREFIX}/lib/libfmt.a - CATKIN_DEPENDS - ) - - ############# - ## INSTALL ## - ############# - - install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - ) - - install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/ - DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} - ) - - if(${MAKE_LOCAL_INSTALL}) - message("${BoldMagenta}INFO: Making a local install.${ColourReset}") - add_custom_command (OUTPUT run_install_script___OK - COMMAND ${CMAKE_SOURCE_DIR}/local_install.sh "${CMAKE_SOURCE_DIR}" - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/) - add_custom_target( - RunInstallScript ALL - DEPENDS run_install_script___OK) - endif() - -endif() + # 2 - Check whether open3d is installed ------------------------------------------------------- +elseif (Open3D_FOUND) + message("${BoldMagenta}INFO: Found Open3D.${ColourReset}") + + # Path differs whether installed from PPA or (locally) from source + if (EXISTS "${Open3D_DIR}/../../../../include/") + set(Open3D_INCLUDE_DIR_MAN "${Open3D_DIR}/../../../../include/") + else () + set(Open3D_INCLUDE_DIR_MAN "${Open3D_DIR}/../../../include/") + endif () + + message("Open3D libraries: ${Open3D_LIBRARIES}") + message("Open3D include path: ${Open3D_INCLUDE_DIRS} , ${Open3D_INCLUDE_DIR_MAN}") + + # FMT + find_package(fmt QUIET) + if (fmt_FOUND) + message("Found fmt.") + else () + message("fmt not found. If you get linking errors try installing libfmt with sudo apt install libfmt-dev") + endif () + + # Catkinization of open3D + catkin_package( + INCLUDE_DIRS + ${Open3D_INCLUDE_DIRS} + ${Open3D_INCLUDE_DIR_MAN} + LIBRARIES + ${Open3D_LIBRARIES} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + Open3D + ) + + # 3 - Compile in catkin workspace ------------------------------------------------------------- +else () + message("${BoldMagenta}INFO: Neither variable Open3d_DIR is set, nor could Open3D be found, hence compiling in workspace.${ColourReset}") + message("${BoldMagenta}INFO: This automatic compilation from source requires CMAKE>3.18.${ColourReset}") + + # Newer version of CMake is required + cmake_minimum_required(VERSION 3.18) + + # Catkinization of open3D + include(ExternalProject) + file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) + + ExternalProject_Add(open3d + GIT_REPOSITORY "https://github.com/isl-org/Open3D" + GIT_TAG "v0.15.1" + GIT_PROGRESS "true" + CMAKE_CACHE_ARGS "-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=true" + PREFIX "${CMAKE_SOURCE_DIR}" + SOURCE_DIR "${CMAKE_SOURCE_DIR}/open3d" + BINARY_DIR "${CMAKE_SOURCE_DIR}/tmp" + CMAKE_ARGS + -DBUILD_SHARED_LIBS=ON + -DCMAKE_BUILD_TYPE=RelWithDebInfo + -DUSE_SYSTEM_EIGEN3=OFF + -DGLIBCXX_USE_CXX11_ABI=ON + -DBUILD_PYTHON_MODULE=OFF + -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} + INSTALL_COMMAND make install && bash "-c" "cp -r ${CMAKE_SOURCE_DIR}/tmp/fmt/include/ ${CATKIN_DEVEL_PREFIX} && cp ${CMAKE_SOURCE_DIR}/tmp/fmt/lib/*.a ${CATKIN_DEVEL_PREFIX}/lib/" + ) + + catkin_package( + INCLUDE_DIRS + ${CATKIN_DEVEL_PREFIX}/include + LIBRARIES + ${CATKIN_DEVEL_PREFIX}/lib/libOpen3D.so + ${CATKIN_DEVEL_PREFIX}/lib/libfmt.a + CATKIN_DEPENDS + ) + + ############# + ## INSTALL ## + ############# + + install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + ) + + install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/ + DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} + ) + + if (${MAKE_LOCAL_INSTALL}) + message("${BoldMagenta}INFO: Making a local install.${ColourReset}") + add_custom_command(OUTPUT run_install_script___OK + COMMAND ${CMAKE_SOURCE_DIR}/local_install.sh "${CMAKE_SOURCE_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/) + add_custom_target( + RunInstallScript ALL + DEPENDS run_install_script___OK) + endif () + +endif () diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/CMakeLists.txt b/ros/examples_standalone/open3d_rosbag_mapper_mesher/CMakeLists.txt new file mode 100644 index 0000000..087ace7 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.5) +project(open3d_rosbag_mapper_mesher) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +add_compile_options(-O3) + +# Source Files in this Project +set(SRC_FILES + src/lib/RosbagMapperRos.cpp + src/lib/MapMesher.cpp +) + +# Catkin Dependencies +set(CATKIN_PACKAGE_DEPENDENCIES + roscpp + roslib + open3d_conversions + open3d_slam + open3d_slam_ros + eigen_conversions + tf + tf2 + tf2_msgs + tf2_ros + tf2_geometry_msgs + rosbag + interactive_markers + open3d_slam_lua_io +) + +# CMake Dependencies +find_package(Eigen3 REQUIRED) +find_package(OpenMP QUIET) +if (OpenMP_FOUND) + add_compile_options("${OpenMP_CXX_FLAGS}") + add_definitions(-Dopen3d_slam_ros_OPENMP_FOUND=${OpenMP_FOUND}) +endif () +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +# Main Catkin Package +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + yaml-cpp + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS +) + +# Include Directories +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${YAML_CPP_INCLUDE_DIR} + ${OpenMP_CXX_INCLUDE_DIRS} +) + +# Main Library +add_library(${PROJECT_NAME} + ${SRC_FILES} +) + +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + yaml-cpp + ${OpenMP_CXX_LIBRARIES} +) + +# Main Node +add_executable(${PROJECT_NAME}_node + src/main_node.cpp) +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} + ${PROJECT_NAME} +) diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/MapMesher.h b/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/MapMesher.h new file mode 100644 index 0000000..a74eb4e --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/MapMesher.h @@ -0,0 +1,12 @@ +/* +* MapMesher.h +* +* Created on: Nov 19, 2024 +* Author: nubertj + */ + +#pragma once + +namespace o3d_slam { + +} // namespace o3d_slam diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/RosbagMapperRos.h b/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/RosbagMapperRos.h new file mode 100644 index 0000000..4ee837e --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/RosbagMapperRos.h @@ -0,0 +1,69 @@ +/* + * RosbagMapper.h + * + * Created on: Nov 19, 2024 + * Author: nubertj + */ + +#pragma once + +// C++ +#include + +// Ros +#include +#include +#include +#include +#include + +// Open3D SLAM +#include "open3d_slam/SlamWrapper.hpp" +#include "open3d_slam/Submap.hpp" +#include "open3d_slam_ros/DataProcessorRos.hpp" + +namespace o3d_slam { + +class RosbagMapperRos : public DataProcessorRos { + using BASE = DataProcessorRos; + + public: + RosbagMapperRos(ros::NodeHandlePtr nh); + ~RosbagMapperRos() override = default; + + void initialize() override; + void startProcessing() override; + void processMeasurement(const PointCloud& cloud, const Time& timestamp, + const std::optional& transform = std::nullopt) override; + + private: + void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg, const geometry_msgs::TransformStamped& transform); + void readRosbags(const std::vector>& pcBagVector, const rosbag::Bag& tfBag, const rosbag::Bag& tfStaticBag); + + // Members + // Parameters + SlamParameters params_; + + // Full Names + std::string tfStaticRosbagFilename_; + std::string tfRosbagFilename_; + // Directory name and prefix + std::string pcRosbagDirectoryName_; + std::string pcRosbagPrefix_; + // Frame Names + std::string worldFrame_; + std::string lidarFrame_; + // Submap + Submap submap_; + // Publish Rate + int publishMapEveryNScans_; + + // Saving of the map + std::string mapSavingFolderPath_; + std::string mapSavingFilename_; + + // Publisher + ros::Publisher denseCloudPub_; +}; + +} // namespace o3d_slam diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/file_utils.h b/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/file_utils.h new file mode 100644 index 0000000..51796ce --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/include/open3d_rosbag_mapper_mesher/file_utils.h @@ -0,0 +1,41 @@ +/* + * file_utils.h + * + * Created on: Nov 19, 2024 + * Author: nubertj + */ + +#include +#include +#include +#include + +#pragma once + +namespace o3d_slam { + +std::vector getFilesWithPrefix(const std::string& directory, const std::string& prefix) { + std::vector matching_files; + + // Check if the directory exists + if (!std::filesystem::exists(directory) || !std::filesystem::is_directory(directory)) { + throw std::runtime_error("Invalid directory: " + directory); + } + + // Iterate through directory entries + for (const auto& entry : std::filesystem::directory_iterator(directory)) { + if (entry.is_regular_file()) { // Ensure it's a file + const std::string file_name = entry.path().filename().string(); + if (file_name.rfind(prefix, 0) == 0) { // Check if the filename starts with the prefix + matching_files.push_back(entry.path().string()); + } + } + } + + // Sort the files + std::sort(matching_files.begin(), matching_files.end()); + + return matching_files; +} + +} // namespace o3d_slam diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/launch/create_map_from_rosbag.launch b/ros/examples_standalone/open3d_rosbag_mapper_mesher/launch/create_map_from_rosbag.launch new file mode 100644 index 0000000..c800a8b --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/launch/create_map_from_rosbag.launch @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/package.xml b/ros/examples_standalone/open3d_rosbag_mapper_mesher/package.xml new file mode 100644 index 0000000..d755cd0 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/package.xml @@ -0,0 +1,31 @@ + + + open3d_rosbag_mapper_mesher + 0.0.0 + Open3D map and mesh generation + + + Julian Nubert + Edo Jelavic + + + BSD-3-Clause + + + catkin + roscpp + roslib + open3d_conversions + open3d_slam + open3d_slam_ros + eigen_conversions + tf + tf2 + tf2_msgs + tf2_ros + tf2_geometry_msgs + interactive_markers + rosbag + open3d_slam_lua_io + + diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/default/default_parameters.lua b/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/default/default_parameters.lua new file mode 100644 index 0000000..07a9615 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/default/default_parameters.lua @@ -0,0 +1,16 @@ +include "parameter_structure_definitions.lua" + + +DEFAULT_PARAMETERS = { + odometry = deepcopy(ODOMETRY_PARAMETERS), + submap = deepcopy(SUBMAP_PARAMETERS), + map_builder = deepcopy(MAP_BUILDER_PARAMETERS), + dense_map_builder = deepcopy(MAP_BUILDER_PARAMETERS), + mapper_localizer = deepcopy(MAPPER_LOCALIZER_PARAMETERS), + saving = deepcopy(SAVING_PARAMETERS), + visualization = deepcopy(VISUALIZATION_PARAMETERS), + motion_compensation = deepcopy(MOTION_COMPENSATION_PARAMETERS), + global_optimization = deepcopy(GLOBAL_OPTIMIZATION_PARAMETERS), + map_initializer = deepcopy(MAP_INITIALIZER_PARAMETERS), + place_recognition = deepcopy(PLACE_RECOGNITION_PARAMETERS), +} \ No newline at end of file diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/default/parameter_structure_definitions.lua b/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/default/parameter_structure_definitions.lua new file mode 100644 index 0000000..854ec17 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/default/parameter_structure_definitions.lua @@ -0,0 +1,174 @@ +-- need this to deep copy all the tables +function deepcopy(orig, copies) + copies = copies or {} + local orig_type = type(orig) + local copy + if orig_type == 'table' then + if copies[orig] then + copy = copies[orig] + else + copy = {} + copies[orig] = copy + for orig_key, orig_value in next, orig, nil do + copy[deepcopy(orig_key, copies)] = deepcopy(orig_value, copies) + end + setmetatable(copy, deepcopy(getmetatable(orig), copies)) + end + else -- number, string, boolean, etc + copy = orig + end + return copy +end + + + +SAVING_PARAMETERS = { + save_at_mission_end = true, + save_map = false, + save_submaps = false, + save_dense_submaps = false, +} + +MOTION_COMPENSATION_PARAMETERS = { + is_undistort_scan = false, + is_spinning_clockwise = true, + scan_duration = 0.1, + num_poses_vel_estimation = 3, +} + +VISUALIZATION_PARAMETERS = { + assembled_map_voxel_size = 0.3, + submaps_voxel_size = 0.3, + visualize_every_n_msec = 300.0, +} + +GLOBAL_OPTIMIZATION_PARAMETERS = { + edge_prune_threshold = 0.2, + loop_closure_preference = 2.0, + max_correspondence_distance = 1000.0, + reference_node = 0, +} + +SCAN_CROPPING_PARAMETERS = { + cropping_radius_max= 30.0, + cropping_radius_min= 2.0, + min_z= -50.0, + max_z= 50.0, + cropper_type= "MinMaxRadius", -- options are Cylinder, MaxRadius, MinRadius, MinMaxRadius +} + +SCAN_PROCESSING_PARAMETERS = { + voxel_size = 0.1, + downsampling_ratio = 0.3, + point_cloud_buffer_size = 1, -- the scan processing buffer size. 1 means no buffering. + scan_cropping = deepcopy(SCAN_CROPPING_PARAMETERS), +} + +ICP_PARAMETERS = { + max_correspondence_dist= 1.0, + knn= 20, + max_distance_knn= 3.0, + max_n_iter= 50, +} + +SCAN_MATCHING_PARAMETERS = { + icp = deepcopy(ICP_PARAMETERS), + cloud_registration_type = "GeneralizedIcp", -- options GeneralizedIcp, PointToPointIcp, PointToPlaneIcp +} + +ODOMETRY_PARAMETERS = { + is_publish_odometry_msgs = true, + odometry_buffer_size = 1, -- the scan2scan odometry buffer size. 1 means no buffering. + scan_matching = deepcopy(SCAN_MATCHING_PARAMETERS), + scan_processing = deepcopy(SCAN_PROCESSING_PARAMETERS), +} + +SUBMAP_PARAMETERS = { + submap_size = 20, -- meters + min_num_range_data = 10, + adjacency_based_revisiting_min_fitness = 0.5, + submaps_num_scan_overlap = 10, +} + +SPACE_CARVING_PARAMETERS = { + voxel_size= 0.2, + max_raytracing_length = 20.0, + truncation_distance = 0.3, + carve_space_every_n_scans= 10.0, +} + + +MAP_BUILDER_PARAMETERS = { + map_voxel_size = 0.1, --meters + scan_cropping = deepcopy(SCAN_CROPPING_PARAMETERS), + space_carving = deepcopy(SPACE_CARVING_PARAMETERS), +} + +SCAN_TO_MAP_REGISTRATION_PARAMETERS = { + min_refinement_fitness = 0.7, + scan_to_map_refinement_type = "GeneralizedIcp", -- options GeneralizedIcp, PointToPointIcp, PointToPlaneIcp + icp = deepcopy(ICP_PARAMETERS), + scan_processing = deepcopy(SCAN_PROCESSING_PARAMETERS), +} + + +MAPPER_LOCALIZER_PARAMETERS = { + is_print_timing_information = true, + is_build_dense_map = false, + is_attempt_loop_closures = true, + is_use_map_initialization = false, + is_merge_scans_into_map = false, + dump_submaps_to_file_before_after_lc = false, + is_refine_odometry_constraints_between_submaps = false, + min_movement_between_mapping_steps = 0.0, + ignore_minimum_refinement_fitness = false, + mapping_buffer_size = 1, -- the scan2map odometry buffer size. 1 means no buffering. + scan_to_map_registration = deepcopy(SCAN_TO_MAP_REGISTRATION_PARAMETERS), +} + +POSE = { + x = 0.0, + y = 0.0, + z = 0.0, + roll = 0.0, --roll, pitch ,yaw in degrees!!!!! + pitch = 0.0, + yaw = 0.0, +} + +MAP_INITIALIZER_PARAMETERS = { + is_initialize_interactively = false, + frame_id = "map_o3d", + pcd_file_path = "", + init_pose = POSE, +} + +LOOP_CLOSURE_CONSISTENCY_CHECK_PARAMETERS = { + max_drift_roll = 30.0, --deg + max_drift_pitch = 30.0, --deg + max_drift_yaw = 30.0, --deg + max_drift_x = 80.0, --meters + max_drift_y = 80.0, --meters + max_drift_z = 40.0, --meters +} + +PLACE_RECOGNITION_PARAMETERS = { + feature_map_normal_estimation_radius = 2.0, + feature_voxel_size = 0.5, + feature_radius = 2.5, + feature_knn = 100, + feature_normal_knn = 20, + ransac_num_iter = 10000000, + ransac_probability = 0.999, + ransac_model_size = 3, + ransac_max_correspondence_dist = 0.75, + ransac_correspondence_checker_distance = 0.8, + ransac_correspondence_checker_edge_length = 0.6, + ransac_min_corresondence_set_size = 25, + max_icp_correspondence_distance = 0.3, + min_icp_refinement_fitness = 0.7, -- the more aliasing, the higher this should be + dump_aligned_place_recognitions_to_file = false , --useful for debugging + min_submaps_between_loop_closures = 2, + loop_closure_search_radius = 20.0, + consistency_check = deepcopy(LOOP_CLOSURE_CONSISTENCY_CHECK_PARAMETERS), +} + diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/param_robosense_rs16.lua b/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/param_robosense_rs16.lua new file mode 100644 index 0000000..78bfde2 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/param/param_robosense_rs16.lua @@ -0,0 +1,68 @@ +include "default/default_parameters.lua" + + +params = deepcopy(DEFAULT_PARAMETERS) + +--ODOMETRY +params.odometry.scan_processing.voxel_size = 0.05 +params.odometry.scan_processing.downsampling_ratio = 1.0 +params.odometry.scan_processing.scan_cropping.cropping_radius_max = 40.0 + + +--MAPPER_LOCALIZER +params.mapper_localizer.is_merge_scans_into_map = false +params.mapper_localizer.is_build_dense_map = false +params.mapper_localizer.is_use_map_initialization = false +params.mapper_localizer.is_print_timing_information = true +params.mapper_localizer.scan_to_map_registration.scan_processing.voxel_size = 0.08 +params.mapper_localizer.scan_to_map_registration.scan_processing.downsampling_ratio = 0.25 +params.mapper_localizer.scan_to_map_registration.scan_processing.scan_cropping.cropping_radius_max = 40.0 +params.mapper_localizer.scan_to_map_registration.icp.max_correspondence_dist = 0.8 + +--MAP_INITIALIZER +params.map_initializer.pcd_file_path = "" +params.map_initializer.init_pose.x = 0.0 +params.map_initializer.init_pose.y = 0.0 +params.map_initializer.init_pose.z = 0.0 +params.map_initializer.init_pose.roll = 0.0 +params.map_initializer.init_pose.pitch = 0.0 +params.map_initializer.init_pose.yaw = 0.0 + +--SUBMAP +params.submap.submap_size = 20.0 --meters + + +--MAP_BUILDER +params.map_builder.map_voxel_size = 0.1 +params.map_builder.scan_cropping.cropping_radius_max = 40.0 +params.map_builder.space_carving.carve_space_every_n_scans = 10 + +--DENSE_MAP_BUILDER +params.dense_map_builder.map_voxel_size = 0.05 +params.dense_map_builder.scan_cropping.cropping_radius_max = 32.0 +params.dense_map_builder.space_carving.carve_space_every_n_scans = 2 +params.dense_map_builder.space_carving.truncation_distance = 0.1 +params.dense_map_builder.space_carving.max_raytracing_length = 40.0 + +--PLACE_RECOGNITION +params.place_recognition.ransac_min_corresondence_set_size = 40 +params.place_recognition.max_icp_correspondence_distance = 0.3 +params.place_recognition.min_icp_refinement_fitness = 0.7 +params.place_recognition.dump_aligned_place_recognitions_to_file = false +params.place_recognition.min_submaps_between_loop_closures = 2 +params.place_recognition.loop_closure_search_radius = 20.0 +params.place_recognition.consistency_check.max_drift_roll = 30.0 --deg +params.place_recognition.consistency_check.max_drift_pitch = 30.0 --deg +params.place_recognition.consistency_check.max_drift_yaw = 30.0 --deg +params.place_recognition.consistency_check.max_drift_x = 80.0 --m +params.place_recognition.consistency_check.max_drift_y = 80.0 --m +params.place_recognition.consistency_check.max_drift_z = 40.0 --m + +--MOTION_COMPENSATION +params.motion_compensation.is_undistort_scan = true + +--SAVING +params.saving.save_map = false + + +return params \ No newline at end of file diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/rviz/map.rviz b/ros/examples_standalone/open3d_rosbag_mapper_mesher/rviz/map.rviz new file mode 100644 index 0000000..1985799 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/rviz/map.rviz @@ -0,0 +1,230 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /AssembledMap1 + Splitter Ratio: 0.6278260946273804 + Tree Height: 585 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: AssembledMap + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 7.946328639984131 + Min Value: -14.672446250915527 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 239; 41; 41 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: AssembledMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 1 + Size (m): 0.20000000298023224 + Style: Points + Topic: /create_map_from_rosbag_node/dense_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 8.856701850891113 + Min Value: -1.5836607217788696 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: SubmapsCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.10000000149011612 + Style: Points + Topic: /mapping_node/submaps + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /mapping_node/submap_origins + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: jsk_rviz_plugin/TFTrajectory + Enabled: true + Name: TFTrajectoryOdom + Value: true + color: 173; 127; 168 + duration: 500 + frame: raw_odom_o3d + line_width: 0.15000000596046448 + - Class: jsk_rviz_plugin/TFTrajectory + Enabled: true + Name: TFTrajectoryMap + Value: true + color: 239; 41; 41 + duration: 500 + frame: raw_rs_o3d + line_width: 0.15000000596046448 + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: false + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Update Topic: /initialization_pose/update + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: gt_world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 976 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000024100000310fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000310000000c900fffffffb0000000a00560069006500770073000000093600000140000000a400fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000010c0000028b0000000000000000000000010000015f00000310fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000310000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000060fc0100000002fb0000000800540069006d0065010000000000000a00000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000006540000031000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 27 diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/lib/MapMesher.cpp b/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/lib/MapMesher.cpp new file mode 100644 index 0000000..d8b4819 --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/lib/MapMesher.cpp @@ -0,0 +1,8 @@ +/* +* MapMesher.cpp +* +* Created on: Nov 19, 2024 +* Author: nubertj + */ + +#include "open3d_rosbag_mapper_mesher/MapMesher.h" \ No newline at end of file diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/lib/RosbagMapperRos.cpp b/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/lib/RosbagMapperRos.cpp new file mode 100644 index 0000000..27b322d --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/lib/RosbagMapperRos.cpp @@ -0,0 +1,257 @@ +/* + * RosbagMapper.cpp + * + * Created on: Nov 19, 2024 + * Author: nubertj + */ + +// Implementation +#include "open3d_rosbag_mapper_mesher/RosbagMapperRos.h" + +// ROS +#include +#include +#include +#include +#include +#include + +// Open3D SLAM +#include "open3d_conversions/open3d_conversions.h" +#include "open3d_slam/frames.hpp" +#include "open3d_slam/output.hpp" +#include "open3d_slam/time.hpp" +#include "open3d_slam_lua_io/parameter_loaders.hpp" +#include "open3d_slam_ros/SlamWrapperRos.hpp" +#include "open3d_slam_ros/helpers_ros.hpp" + +// Package +#include "open3d_rosbag_mapper_mesher/file_utils.h" + +namespace o3d_slam { + +RosbagMapperRos::RosbagMapperRos(ros::NodeHandlePtr nh) : BASE(nh), submap_(0, 0) { + denseCloudPub_ = nh_->advertise("dense_cloud", 1, true); +} + +void RosbagMapperRos::initialize() { + initCommonRosStuff(); + + // Load the rosbag filepaths + tfStaticRosbagFilename_ = tryGetParam("tf_static_rosbag_filepath", *nh_); + tfRosbagFilename_ = tryGetParam("tf_rosbag_filepath", *nh_); + pcRosbagDirectoryName_ = tryGetParam("pc_rosbag_directory_name", *nh_); + pcRosbagPrefix_ = tryGetParam("pc_rosbag_prefix", *nh_); + std::cout << "TF static rosbag filepath: " << tfStaticRosbagFilename_ << std::endl; + std::cout << "TF rosbag filepath: " << tfRosbagFilename_ << std::endl; + std::cout << "PC rosbag directory name: " << pcRosbagDirectoryName_ << std::endl; + std::cout << "PC rosbag prefix: " << pcRosbagPrefix_ << std::endl; + // Frame names + worldFrame_ = tryGetParam("world_frame", *nh_); + lidarFrame_ = tryGetParam("lidar_frame", *nh_); + std::cout << "World frame: " << worldFrame_ << std::endl; + std::cout << "Lidar frame: " << lidarFrame_ << std::endl; + // Publish rate + publishMapEveryNScans_ = tryGetParam("publish_map_every_n_scans", *nh_); + // Saving of the map + mapSavingFolderPath_ = tryGetParam("map_saving_folder_path", *nh_); + mapSavingFilename_ = tryGetParam("map_saving_filename", *nh_); + + // Load the parameters + const std::string paramFolderPath = nh_->param("parameter_folder_path", ""); + const std::string paramFilename = nh_->param("parameter_filename", ""); + io_lua::loadParameters(paramFolderPath, paramFilename, ¶ms_); + // Set the parameters + std::cout << "Voxel size before setting parameters: " << submap_.getDenseMap().getVoxelSize() << std::endl; + submap_.setParameters(params_.mapper_); + std::cout << "Voxel size: " << submap_.getDenseMap().getVoxelSize() << std::endl; +} + +void RosbagMapperRos::startProcessing() { + // Open the rosbags + // TF static + rosbag::Bag tfStaticBag; + std::cout << "Opening TF static rosbag: " << tfStaticRosbagFilename_ << "\n"; + tfStaticBag.open(tfStaticRosbagFilename_, rosbag::bagmode::Read); + std::cout << "...done. \n"; + // TF + rosbag::Bag tfBag; + std::cout << "Opening TF rosbag: " << tfRosbagFilename_ << "\n"; + tfBag.open(tfRosbagFilename_, rosbag::bagmode::Read); + std::cout << "...done. \n"; + // Pointcloud + std::vector> pointCloudBagPtrVector = {}; + // Get all files in the directory starting with pcRosbagFilename_ + std::vector fileNames = getFilesWithPrefix(pcRosbagDirectoryName_, pcRosbagPrefix_); + for (const auto& fileName : fileNames) { + std::shared_ptr bagPtr = std::make_shared(); + std::cout << "Opening pointcloud rosbag: " << fileName << "\n"; + bagPtr->open(fileName, rosbag::bagmode::Read); + pointCloudBagPtrVector.push_back(bagPtr); + } + std::cout << "...done. \n"; + // Read the rosbag + readRosbags(pointCloudBagPtrVector, tfBag, tfStaticBag); + ros::spin(); +} + +void RosbagMapperRos::readRosbags(const std::vector>& pcBagVector, const rosbag::Bag& tfBag, + const rosbag::Bag& tfStaticBag) { + // Rosbag + rosbag::View tfStaticBagView(tfStaticBag, rosbag::TopicQuery("/tf_static")); + rosbag::View tfBagView(tfBag, rosbag::TopicQuery("/tf")); + Timer rosbagTimer; + ros::Time lastTimestamp; + // Start + bool isFirstMessage = true; + Timer rosbagProcessingTimer; + + // Create a tf2 buffer and listener + tf2_ros::Buffer tfBuffer(ros::Duration(10000.0)); + tf2_ros::TransformListener tfListener(tfBuffer); + + // Earliest and last timestamp + ros::Time earliest_time = ros::Time::now(); // Initialize to now (a large value) + ros::Time latest_time = ros::Time(0); // Initialize to 0 (a small value) + + // Step 1: Preload static transforms + for (const rosbag::MessageInstance& msg : tfStaticBagView) { + if (msg.getTopic() == "/tf_static") { + auto tfStaticMsg = msg.instantiate(); + if (tfStaticMsg != nullptr) { + for (const auto& transform : tfStaticMsg->transforms) { + try { + tfBuffer.setTransform(transform, "bag_reader", true); // `true` for static + } catch (tf2::TransformException& ex) { + ROS_WARN_STREAM("Error setting static transform: " << ex.what()); + throw std::runtime_error("Error setting static transform"); + } + } + } + } + } + + // Step 2: Loading dynamic transforms + for (const rosbag::MessageInstance& msg : tfBagView) { + if (msg.getTopic() == "/tf") { + auto tfMsg = msg.instantiate(); + if (tfMsg != nullptr) { + for (const auto& transform : tfMsg->transforms) { + try { + tfBuffer.setTransform(transform, "bag_reader"); + if (transform.header.stamp < earliest_time) { + earliest_time = transform.header.stamp; + } else if (transform.header.stamp > latest_time) { + latest_time = transform.header.stamp; + } + } catch (tf2::TransformException& ex) { + ROS_WARN_STREAM("Error updating dynamic transform: " << ex.what()); + throw std::runtime_error("Error updating dynamic transform"); + } + } + } + } + } + + // Print + std::cout << "TF buffer preloaded with static and dynamic transforms. Earliest time: " << earliest_time << " Latest time: " << latest_time + << std::endl; + + // Step 2: Main loop for reading the rosbags + for (const auto& pcBag : pcBagVector) { + std::cout << "Processing pointcloud rosbag: " << pcBag->getFileName() << "\n"; + rosbag::View pcView(*pcBag, rosbag::TopicQuery(cloudTopic_)); + for (const rosbag::MessageInstance& msg : pcView) { + // Process Cloud + if (msg.getTopic() == cloudTopic_ || ("/" + msg.getTopic() == cloudTopic_)) { + sensor_msgs::PointCloud2::ConstPtr cloud = msg.instantiate(); + if (cloud != nullptr) { + if (isFirstMessage) { + isFirstMessage = false; + lastTimestamp = cloud->header.stamp; + } + // Looking Up the TF + ros::Time msgTime = cloud->header.stamp; + geometry_msgs::TransformStamped transform; + try { + transform = tfBuffer.lookupTransform(worldFrame_, lidarFrame_, msgTime); + } catch (tf2::TransformException& ex) { + ROS_WARN_STREAM("Could not get transform: " << ex.what()); + std::cout << "Skipping the cloud message \n"; + continue; + } + // Process the cloud + cloudCallback(cloud, transform); + + // Logging + const double elapsedWallTime = rosbagProcessingTimer.elapsedSec(); + if (elapsedWallTime > 15.0) { + const double elapsedRosbagTime = (cloud->header.stamp - lastTimestamp).toSec(); + std::cout << "ROSBAG PLAYER: Rosbag messages pulsed at: " << 100.0 * elapsedRosbagTime / elapsedWallTime + << " % realtime speed \n"; + rosbagProcessingTimer.reset(); + lastTimestamp = cloud->header.stamp; + } + } // end if checking for the null ptr + } // end if checking for the right topic + // Shut Down if ROS is not OK + if (!ros::ok()) { + return; + } + } // end foreach + } // end through all rosbags + + // Saving the map + std::cout << "Saving the map to: " << mapSavingFolderPath_ + "/" + mapSavingFilename_ << std::endl; + createDirectoryOrNoActionIfExists(mapSavingFolderPath_); + saveToFile(mapSavingFolderPath_ + "/" + mapSavingFilename_, submap_.getDenseMap().toPointCloud()); + std::cout << "Map saved. \n"; + + // a bit of a hack, this extra thread listens to ros shutdown + // otherwise we might get stuck in a loop + bool isProcessingFinished = false; + std::thread rosSpinner([&]() { + ros::Rate r(20.0); + while (true) { + if (!ros::ok()) { + break; + } + if (isProcessingFinished) { + break; + } + r.sleep(); + } + }); + isProcessingFinished = true; + rosSpinner.join(); +} + +void RosbagMapperRos::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg, const geometry_msgs::TransformStamped& transform) { + open3d::geometry::PointCloud cloud; + open3d_conversions::rosToOpen3d(msg, cloud, false); + const Time timestamp = fromRos(msg->header.stamp); + // Convert transform to Eigen + Transform T_W_L = tf2::transformToEigen(transform.transform); + // Transform the cloud to the world frame + // cloud.Transform(T_W_L.matrix()); + accumulateAndProcessRangeData(cloud, timestamp, T_W_L); +} + +void RosbagMapperRos::processMeasurement(const PointCloud& cloud, const Time& timestamp, const std::optional& transform) { + // Counter + static int publishCounter = 0; + // Transform + Transform T_W_L = transform.value_or(Transform::Identity()); + // Add to submap + bool isPerformCarving = false; + submap_.insertScanDenseMap(cloud, T_W_L, timestamp, isPerformCarving); + // Publish the dense cloud + const bool isCloudEmpty = submap_.getDenseMap().empty(); + if (!isCloudEmpty && !(publishCounter % publishMapEveryNScans_)) { + std::cout << "Publishing the dense cloud in frame: " << worldFrame_ << std::endl; + o3d_slam::publishCloud(submap_.getDenseMap().toPointCloud(), worldFrame_, toRos(timestamp), denseCloudPub_); + } + publishCounter++; +} + +} // namespace o3d_slam \ No newline at end of file diff --git a/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/main_node.cpp b/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/main_node.cpp new file mode 100644 index 0000000..223b89d --- /dev/null +++ b/ros/examples_standalone/open3d_rosbag_mapper_mesher/src/main_node.cpp @@ -0,0 +1,56 @@ +/* + * mapping_node.cpp + * + * Created on: Sep 1, 2021 + * Author: jelavice + */ + +// Open3D +#include + +// Open3D SLAM +#include "open3d_slam/Parameters.hpp" +#include "open3d_slam_lua_io/parameter_loaders.hpp" +#include "open3d_slam_ros/SlamMapInitializer.hpp" +#include "open3d_slam_ros/creators.hpp" +#include "open3d_slam_ros/helpers_ros.hpp" + +// Package +#include "open3d_rosbag_mapper_mesher/RosbagMapperRos.h" + +int main(int argc, char** argv) { + using namespace o3d_slam; + + // ROS + ros::init(argc, argv, "open3d_slam"); + ros::NodeHandlePtr nh(new ros::NodeHandle("~")); + + // ROS configuration + // Paths + const std::string paramFolderPath = tryGetParam("parameter_folder_path", *nh); + const std::string paramFilename = o3d_slam::tryGetParam("parameter_filename", *nh); + const std::string mapSavingFolderPath = tryGetParam("map_saving_folder_path", *nh); + const std::string mapSavingFilename = tryGetParam("map_saving_filename", *nh); + // Bools + const bool isProcessAsFastAsPossible = o3d_slam::tryGetParam("is_read_from_rosbag", *nh); + + // The LUA parameters are loaded twice. This is the first time. Solely because we need to know if we are using a map for initialization. + // SlamParameters params; + // io_lua::loadParameters(paramFolderPath, paramFilename, ¶ms); + + // This is where the initial class is constructed and passed on + std::shared_ptr dataProcessorPtr; + if (isProcessAsFastAsPossible) { + dataProcessorPtr = std::make_shared(nh); + } else { + throw std::logic_error("Not implemented yet. Can only read from rosbag."); + } + dataProcessorPtr->initialize(); + + // Start processing + dataProcessorPtr->startProcessing(); + + // Wrap up + std::cout << "Finished processing. Exiting... \n"; + return 0; +} diff --git a/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp b/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp index 01f44fb..c44a89d 100644 --- a/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp +++ b/ros/open3d_slam_ros/include/open3d_slam_ros/DataProcessorRos.hpp @@ -21,8 +21,9 @@ class DataProcessorRos { virtual void initialize() = 0; virtual void startProcessing() = 0; - virtual void processMeasurement(const PointCloud& cloud, const Time& timestamp); - void accumulateAndProcessRangeData(const PointCloud& cloud, const Time& timestamp); + virtual void processMeasurement(const PointCloud& cloud, const Time& timestamp, const std::optional& transform = std::nullopt); + void accumulateAndProcessRangeData(const PointCloud& cloud, const Time& timestamp, + const std::optional& transform = std::nullopt); void initCommonRosStuff(); std::shared_ptr getSlamPtr(); diff --git a/ros/open3d_slam_ros/include/open3d_slam_ros/OnlineRangeDataProcessorRos.hpp b/ros/open3d_slam_ros/include/open3d_slam_ros/OnlineRangeDataProcessorRos.hpp index 910897b..f0a275a 100644 --- a/ros/open3d_slam_ros/include/open3d_slam_ros/OnlineRangeDataProcessorRos.hpp +++ b/ros/open3d_slam_ros/include/open3d_slam_ros/OnlineRangeDataProcessorRos.hpp @@ -25,7 +25,8 @@ class OnlineRangeDataProcessorRos : public DataProcessorRos { void initialize() override; void startProcessing() override; - void processMeasurement(const PointCloud& cloud, const Time& timestamp) override; + void processMeasurement(const PointCloud& cloud, const Time& timestamp, + const std::optional& transform = std::nullopt) override; private: void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); diff --git a/ros/open3d_slam_ros/include/open3d_slam_ros/RosbagRangeDataProcessorRos.hpp b/ros/open3d_slam_ros/include/open3d_slam_ros/RosbagRangeDataProcessorRos.hpp index 7fb0c73..4fd079b 100644 --- a/ros/open3d_slam_ros/include/open3d_slam_ros/RosbagRangeDataProcessorRos.hpp +++ b/ros/open3d_slam_ros/include/open3d_slam_ros/RosbagRangeDataProcessorRos.hpp @@ -26,7 +26,8 @@ class RosbagRangeDataProcessorRos : public DataProcessorRos { void initialize() override; void startProcessing() override; - void processMeasurement(const PointCloud& cloud, const Time& timestamp) override; + void processMeasurement(const PointCloud& cloud, const Time& timestamp, + const std::optional& transform = std::nullopt) override; private: void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); diff --git a/ros/open3d_slam_ros/src/DataProcessorRos.cpp b/ros/open3d_slam_ros/src/DataProcessorRos.cpp index e4b9fc8..2e60125 100644 --- a/ros/open3d_slam_ros/src/DataProcessorRos.cpp +++ b/ros/open3d_slam_ros/src/DataProcessorRos.cpp @@ -23,7 +23,7 @@ void DataProcessorRos::initCommonRosStuff() { std::cout << "Num accumulated range data: " << numAccumulatedRangeDataDesired_ << std::endl; } -void DataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp) { +void DataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp, const std::optional& transform) { std::cout << "Warning you have not implemented processMeasurement!!! \n"; } @@ -31,7 +31,7 @@ std::shared_ptr DataProcessorRos::getSlamPtr() { return slam_; } -void DataProcessorRos::accumulateAndProcessRangeData(const PointCloud& cloud, const Time& timestamp) { +void DataProcessorRos::accumulateAndProcessRangeData(const PointCloud& cloud, const Time& timestamp, const std::optional& transform) { const size_t minNumCloudsReceived = magic::skipFirstNPointClouds; if (numPointCloudsReceived_ < minNumCloudsReceived) { ++numPointCloudsReceived_; @@ -48,11 +48,11 @@ void DataProcessorRos::accumulateAndProcessRangeData(const PointCloud& cloud, co } if (accumulatedCloud_.IsEmpty()) { - std::cout << "Trying to insert and empyt cloud!!! Skipping the measurement \n"; + std::cout << "Trying to insert and empty cloud!!! Skipping the measurement \n"; return; } - processMeasurement(accumulatedCloud_, timestamp); + processMeasurement(accumulatedCloud_, timestamp, transform); numAccumulatedRangeDataCount_ = 0; accumulatedCloud_.Clear(); diff --git a/ros/open3d_slam_ros/src/OnlineRangeDataProcessorRos.cpp b/ros/open3d_slam_ros/src/OnlineRangeDataProcessorRos.cpp index 891d36a..ed64b76 100644 --- a/ros/open3d_slam_ros/src/OnlineRangeDataProcessorRos.cpp +++ b/ros/open3d_slam_ros/src/OnlineRangeDataProcessorRos.cpp @@ -30,7 +30,8 @@ void OnlineRangeDataProcessorRos::startProcessing() { slam_->stopWorkers(); } -void OnlineRangeDataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp) { +void OnlineRangeDataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp, + const std::optional& transform) { slam_->addRangeScan(cloud, timestamp); o3d_slam::publishCloud(cloud, o3d_slam::frames::rangeSensorFrame, toRos(timestamp), rawCloudPub_); } diff --git a/ros/open3d_slam_ros/src/RosbagRangeDataProcessorRos.cpp b/ros/open3d_slam_ros/src/RosbagRangeDataProcessorRos.cpp index 7db3d4f..d8df8c0 100644 --- a/ros/open3d_slam_ros/src/RosbagRangeDataProcessorRos.cpp +++ b/ros/open3d_slam_ros/src/RosbagRangeDataProcessorRos.cpp @@ -40,7 +40,8 @@ void RosbagRangeDataProcessorRos::startProcessing() { slam_->stopWorkers(); } -void RosbagRangeDataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp) { +void RosbagRangeDataProcessorRos::processMeasurement(const PointCloud& cloud, const Time& timestamp, + const std::optional& transform) { slam_->addRangeScan(cloud, timestamp); std::pair cloudTimePair = slam_->getLatestRegisteredCloudTimestampPair(); const bool isCloudEmpty = cloudTimePair.first.IsEmpty(); @@ -92,7 +93,7 @@ void RosbagRangeDataProcessorRos::readRosbag(const rosbag::Bag& bag) { } ros::spinOnce(); } // end if checking for the null ptr - } // end if checking for the right topic + } // end if checking for the right topic if (!ros::ok()) { slam_->stopWorkers(); return; diff --git a/ros/open3d_slam_ros/src/SlamWrapperRos.cpp b/ros/open3d_slam_ros/src/SlamWrapperRos.cpp index 89cfef3..7b33e4d 100644 --- a/ros/open3d_slam_ros/src/SlamWrapperRos.cpp +++ b/ros/open3d_slam_ros/src/SlamWrapperRos.cpp @@ -187,9 +187,8 @@ void SlamWrapperRos::loadParametersAndInitialize() { // const std::string paramFolderPath = nh_->param("parameter_folder_path", ""); const std::string paramFilename = nh_->param("parameter_filename", ""); - SlamParameters params; - io_lua::loadParameters(paramFolderPath, paramFilename, ¶ms_); + io_lua::loadParameters(paramFolderPath, paramFilename, ¶ms_); BASE::loadParametersAndInitialize(); } diff --git a/ros/open3d_slam_ros/src/mapping_node.cpp b/ros/open3d_slam_ros/src/mapping_node.cpp index 5f6adbf..576af81 100644 --- a/ros/open3d_slam_ros/src/mapping_node.cpp +++ b/ros/open3d_slam_ros/src/mapping_node.cpp @@ -29,17 +29,21 @@ int main(int argc, char** argv) { std::cout << "Is use a map for initialization: " << std::boolalpha << params.mapper_.isUseInitialMap_ << "\n"; // This is where the initial class is constructed and passed on. - std::shared_ptr dataProcessor = dataProcessorFactory(nh, isProcessAsFastAsPossible); - dataProcessor->initialize(); + std::shared_ptr dataProcessorPtr = dataProcessorFactory(nh, isProcessAsFastAsPossible); + dataProcessorPtr->initialize(); + // Main Slam std::shared_ptr slamMapInitializer; if (params.mapper_.isUseInitialMap_) { - std::shared_ptr slam = dataProcessor->getSlamPtr(); - slamMapInitializer = std::make_shared(slam, nh); + std::shared_ptr slamWrapperPtr = dataProcessorPtr->getSlamPtr(); + slamMapInitializer = std::make_shared(slamWrapperPtr, nh); slamMapInitializer->initialize(params.mapper_.mapInit_); } - dataProcessor->startProcessing(); + // Start processing + dataProcessorPtr->startProcessing(); + // Wrap up + std::cout << "Finished processing. Exiting... \n"; return 0; }