├── CMakeLists.txt ├── src ├── meas_models │ ├── src │ │ └── multibeam_simple.cpp │ ├── CMakeLists.txt │ └── include │ │ └── meas_models │ │ └── multibeam_simple.hpp ├── bathy_slam │ ├── include │ │ └── bathy_slam │ │ │ └── bathy_slam.hpp │ ├── CMakeLists.txt │ └── src │ │ └── bathy_slam.cpp ├── apps │ ├── CMakeLists.txt │ └── src │ │ ├── read_auv_data.cpp │ │ └── test_slam_real.cpp ├── submaps_tools │ ├── CMakeLists.txt │ ├── include │ │ └── submaps_tools │ │ │ └── submaps.hpp │ └── src │ │ └── submaps.cpp ├── graph_optimization │ ├── src │ │ ├── read_g2o.cpp │ │ ├── types.cpp │ │ ├── utils_g2o.cpp │ │ ├── graph_construction.cpp │ │ └── ceres_optimizer.cpp │ ├── CMakeLists.txt │ └── include │ │ └── graph_optimization │ │ ├── utils_g2o.hpp │ │ ├── ceres_optimizer.hpp │ │ ├── graph_construction.hpp │ │ ├── types.h │ │ ├── pose_graph_3d_error_term.h │ │ └── read_g2o.h ├── registration │ ├── CMakeLists.txt │ ├── include │ │ └── registration │ │ │ ├── utils_visualization.hpp │ │ │ ├── gicp_reg.hpp │ │ │ └── utils_registration.hpp │ └── src │ │ ├── utils_registration.cpp │ │ ├── gicp_reg.cpp │ │ └── utils_visualization.cpp └── CMakeLists.txt ├── img └── graph_borno.png ├── .gitignore ├── config.yaml ├── LICENSE ├── scripts ├── plot_results.py └── naviedit_export_to_std_data.py └── README.md /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | add_subdirectory(src) -------------------------------------------------------------------------------- /src/meas_models/src/multibeam_simple.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | -------------------------------------------------------------------------------- /img/graph_borno.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ignaciotb/bathymetric_slam/HEAD/img/graph_borno.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | *.pyc 3 | *.autosave 4 | 5 | # Folders 6 | experiments/* 7 | bin/* 8 | build/* 9 | install/* 10 | 11 | # Qt files 12 | *.config 13 | *.creator 14 | *.creator.user 15 | *.files 16 | *.includes 17 | *.cflags 18 | *.cxxflags 19 | 20 | # G2O files 21 | *.g2o 22 | 23 | # Images 24 | #*.png 25 | 26 | *.cereal 27 | -------------------------------------------------------------------------------- /src/bathy_slam/include/bathy_slam/bathy_slam.hpp: -------------------------------------------------------------------------------- 1 | #ifndef BATHY_SLAM_HPP 2 | #define BATHY_SLAM_HPP 3 | 4 | #include "submaps_tools/submaps.hpp" 5 | 6 | #include "graph_optimization/graph_construction.hpp" 7 | #include "graph_optimization/utils_g2o.hpp" 8 | 9 | #include "registration/gicp_reg.hpp" 10 | 11 | class BathySlam{ 12 | 13 | public: 14 | 15 | GraphConstructor* graph_obj_; 16 | SubmapRegistration* gicp_reg_; 17 | 18 | BathySlam(GraphConstructor& graph_obj, SubmapRegistration& gicp_reg); 19 | ~BathySlam(); 20 | 21 | SubmapsVec runOffline(SubmapsVec &submaps_gt, GaussianGen &transSampler, GaussianGen &rotSampler, YAML::Node config); 22 | }; 23 | 24 | 25 | #endif //BATHY_SLAM_HPP 26 | -------------------------------------------------------------------------------- /src/apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(slam_ws_apps) 2 | 3 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin/) 4 | 5 | include_directories(include) 6 | 7 | ## APPS 8 | 9 | add_executable(bathy_slam_real 10 | src/test_slam_real.cpp 11 | ) 12 | 13 | target_link_libraries(bathy_slam_real 14 | gicp_registration 15 | utils_g2o 16 | submaps_tools 17 | graph_construction 18 | ceres_optimizer 19 | read_g2o 20 | data_transforms 21 | benchmark 22 | std_data 23 | slam_solver 24 | ${YAML_CPP_LIBRARIES} 25 | ) 26 | 27 | add_executable(read_auv_data 28 | src/read_auv_data.cpp 29 | ) 30 | 31 | target_link_libraries(read_auv_data 32 | std_data navi_data gsf_data all_data xtf_data csv_data data_transforms benchmark submaps_tools utils_visualization) 33 | 34 | -------------------------------------------------------------------------------- /config.yaml: -------------------------------------------------------------------------------- 1 | # Benchmark imap size 2 | benchmark_nbr_rows: 200 3 | benchmark_nbr_cols: 1000 4 | 5 | # Point cloud downsampling (m) 6 | downsampling_leaf_x: .2 7 | downsampling_leaf_y: .2 8 | downsampling_leaf_z: .1 9 | 10 | # Bathy SLAM params 11 | submap_size: 500 # nbr_pings per submap 12 | add_gaussian_noise: false 13 | 14 | # Submap overlaps 15 | overlap_coverage: 0.9 # Reduce submap area to look for overlap by this factor 16 | 17 | 18 | # GICP registration params (meters) 19 | gicp_max_correspondence_distance: 20 20 | gicp_maximum_iterations: 200 21 | gicp_transform_epsilon: 1e-3 22 | gicp_euclidean_fitness_epsilon: 1e-3 23 | gicp_ransac_outlier_rejection_threshold: 1 24 | gicp_normal_use_knn_search: true 25 | gicp_normal_search_radius: 20 # used if gicp_normal_use_knn_search = false 26 | gicp_normal_search_k_neighbours: 20 # used if gicp_normal_use_knn_search = true 27 | gicp_info_diag: [10000.0, 10000.0, 10000.0, 1000.0] 28 | 29 | # DR weights - uncertainty on vehicle navigation across submap 30 | dr_noise_x: 5. 31 | dr_noise_y: 5. 32 | dr_noise_z: 1. 33 | dr_noise_roll: 1e-4 34 | dr_noise_pitch: 1e-4 35 | dr_noise_yaw: 1e-2 36 | 37 | # Loop closure edge covariance computation method 38 | # 0 = fixed external value 39 | # 1 = manual inputs (#TODO: factor out to YAML file) 40 | # 2 = covariances from GICP 41 | # 3 = covariances loaded from external file 42 | lc_edge_covs_type: 2 43 | -------------------------------------------------------------------------------- /src/meas_models/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(meas_models) 2 | 3 | #set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin/submaps) 4 | 5 | include_directories(include) 6 | 7 | add_library(multibeam_simple 8 | src/multibeam_simple.cpp 9 | ) 10 | 11 | #Define headers for this library. PUBLIC headers are used for 12 | #compiling the library, and will be added to consumers' build 13 | #paths. 14 | target_include_directories(multibeam_simple PUBLIC 15 | $ 16 | $ 17 | PRIVATE src 18 | ) 19 | 20 | target_link_libraries(multibeam_simple PUBLIC 21 | submaps_tools 22 | ${PCL_LIBRARIES} 23 | ) 24 | 25 | # 'make install' to the correct locations (provided by GNUInstallDirs). 26 | install(TARGETS multibeam_simple EXPORT MeasModelsConfig 27 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 28 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 29 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows 30 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 31 | 32 | # This makes the project importable from the install directory 33 | # Put config file in per-project dir (name MUST match), can also 34 | # just go into 'cmake'. 35 | install(EXPORT MeasModelsConfig DESTINATION share/MeasModels/cmake) 36 | 37 | # This makes the project importable from the build directory 38 | export(TARGETS multibeam_simple FILE MeasModelsConfig.cmake) 39 | -------------------------------------------------------------------------------- /src/bathy_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(bathy_slam) 2 | 3 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin) 4 | 5 | include_directories(include) 6 | 7 | # Add libraries and executables 8 | 9 | add_library(slam_solver 10 | src/bathy_slam.cpp 11 | ) 12 | 13 | # Define headers for this library. PUBLIC headers are used for 14 | # compiling the library, and will be added to consumers' build 15 | # paths. 16 | target_include_directories(slam_solver PUBLIC 17 | $ 18 | $ 19 | PRIVATE src) 20 | 21 | target_link_libraries(slam_solver PUBLIC 22 | submaps_tools 23 | graph_construction 24 | gicp_registration 25 | ) 26 | 27 | # 'make install' to the correct locations (provided by GNUInstallDirs). 28 | install(TARGETS slam_solver EXPORT BathySlamConfig 29 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 30 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 31 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows 32 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 33 | 34 | # This makes the project importable from the install directory 35 | # Put config file in per-project dir (name MUST match), can also 36 | # just go into 'cmake'. 37 | install(EXPORT BathySlamConfig DESTINATION share/BathySlam/cmake) 38 | 39 | # This makes the project importable from the build directory 40 | export(TARGETS slam_solver FILE BathySlamConfig.cmake) 41 | -------------------------------------------------------------------------------- /src/submaps_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(submaps_tools) 2 | 3 | # set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin/submaps) 4 | 5 | include_directories(include) 6 | 7 | add_library(submaps_tools 8 | src/submaps.cpp 9 | ) 10 | 11 | # Define headers for this library. PUBLIC headers are used for 12 | # compiling the library, and will be added to consumers' build 13 | # paths. 14 | target_include_directories(submaps_tools PUBLIC 15 | $ 16 | $ 17 | PRIVATE src) 18 | 19 | 20 | target_link_libraries(submaps_tools 21 | std_data 22 | navi_data 23 | data_transforms 24 | ${PCL_LIBRARIES} 25 | ) 26 | 27 | # 'make install' to the correct locations (provided by GNUInstallDirs). 28 | install(TARGETS submaps_tools EXPORT SubmapsToolsConfig 29 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 30 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 31 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows 32 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 33 | 34 | # This makes the project importable from the install directory 35 | # Put config file in per-project dir (name MUST match), can also 36 | # just go into 'cmake'. 37 | install(EXPORT SubmapsToolsConfig DESTINATION share/SubmapsTools/cmake) 38 | 39 | # This makes the project importable from the build directory 40 | export(TARGETS submaps_tools FILE SubmapsToolsConfig.cmake) 41 | -------------------------------------------------------------------------------- /src/graph_optimization/src/read_g2o.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "graph_optimization/read_g2o.h" 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, ignaciotb 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /src/registration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(registration) 2 | 3 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin) 4 | 5 | include_directories(include) 6 | 7 | # Add libraries and executables 8 | 9 | # add_library(utils_registration 10 | # src/utils_registration.cpp 11 | # ) 12 | 13 | add_library(utils_visualization 14 | src/utils_visualization.cpp 15 | ) 16 | 17 | add_library(gicp_registration 18 | src/gicp_reg.cpp 19 | ) 20 | 21 | # Define headers for this library. PUBLIC headers are used for 22 | # compiling the library, and will be added to consumers' build 23 | # paths. 24 | # target_include_directories(utils_registration PUBLIC 25 | # $ 26 | # $ 27 | # PRIVATE src) 28 | 29 | target_include_directories(utils_visualization PUBLIC 30 | $ 31 | $ 32 | PRIVATE src) 33 | 34 | target_include_directories(gicp_registration PUBLIC 35 | $ 36 | $ 37 | PRIVATE src) 38 | 39 | 40 | # Link the libraries 41 | # target_link_libraries(utils_registration PUBLIC 42 | # submaps_tools 43 | # ${PCL_LIBRARIES} 44 | # ) 45 | 46 | target_link_libraries(utils_visualization PUBLIC 47 | submaps_tools 48 | graph_construction 49 | ceres_optimizer 50 | ${PCL_LIBRARIES} 51 | ) 52 | 53 | target_link_libraries(gicp_registration PUBLIC 54 | utils_visualization 55 | submaps_tools 56 | benchmark 57 | ${PCL_LIBRARIES} 58 | ${YAML_CPP_LIBRARIES} 59 | ) 60 | 61 | # 'make install' to the correct locations (provided by GNUInstallDirs). 62 | install(TARGETS utils_visualization gicp_registration EXPORT SubmapRegistrationConfig 63 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 64 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 65 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows 66 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 67 | 68 | # This makes the project importable from the install directory 69 | # Put config file in per-project dir (name MUST match), can also 70 | # just go into 'cmake'. 71 | install(EXPORT SubmapRegistrationConfig DESTINATION share/SubmapRegistration/cmake) 72 | 73 | # This makes the project importable from the build directory 74 | export(TARGETS utils_visualization gicp_registration FILE SubmapRegistrationConfig.cmake) 75 | -------------------------------------------------------------------------------- /src/graph_optimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(graph_optimization) 2 | 3 | # set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin/g2o_tools) 4 | 5 | #include_directories(include) 6 | 7 | # Add libraries and executables 8 | add_library(utils_g2o 9 | src/utils_g2o.cpp 10 | ) 11 | 12 | add_library(graph_construction 13 | src/graph_construction.cpp 14 | ) 15 | 16 | add_library(read_g2o 17 | src/read_g2o.cpp 18 | ) 19 | 20 | add_library(ceres_optimizer 21 | src/ceres_optimizer.cpp 22 | src/types.cpp 23 | ) 24 | 25 | # Define headers for this library. PUBLIC headers are used for 26 | # compiling the library, and will be added to consumers' build 27 | # paths. 28 | target_include_directories(utils_g2o PUBLIC 29 | $ 30 | $ 31 | PRIVATE src) 32 | 33 | target_include_directories(graph_construction PUBLIC 34 | $ 35 | $ 36 | PRIVATE src) 37 | 38 | target_include_directories(read_g2o PUBLIC 39 | $ 40 | $ 41 | PRIVATE src) 42 | 43 | target_include_directories(ceres_optimizer PUBLIC 44 | $ 45 | $ 46 | PRIVATE src) 47 | 48 | target_link_libraries(utils_g2o 49 | submaps_tools 50 | ${G2O_LIBS} 51 | ) 52 | 53 | target_link_libraries(graph_construction 54 | submaps_tools 55 | utils_g2o 56 | ${G2O_LIBS} 57 | ) 58 | 59 | target_link_libraries(ceres_optimizer 60 | ceres 61 | submaps_tools 62 | ${GFLAGS_LIBRARIES} 63 | ) 64 | 65 | target_link_libraries(read_g2o 66 | ceres 67 | ${GFLAGS_LIBRARIES} 68 | ) 69 | 70 | # 'make install' to the correct locations (provided by GNUInstallDirs). 71 | install(TARGETS utils_g2o graph_construction ceres_optimizer read_g2o EXPORT GraphOptimizationConfig 72 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 73 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 74 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows 75 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 76 | 77 | # This makes the project importable from the install directory 78 | # Put config file in per-project dir (name MUST match), can also 79 | # just go into 'cmake'. 80 | install(EXPORT GraphOptimizationConfig DESTINATION share/GraphOptimization/cmake) 81 | 82 | # This makes the project importable from the build directory 83 | export(TARGETS utils_g2o graph_construction ceres_optimizer read_g2o FILE GraphOptimizationConfig.cmake) 84 | -------------------------------------------------------------------------------- /src/graph_optimization/include/graph_optimization/utils_g2o.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef UTILS_G2O_HPP 13 | #define UTILS_G2O_HPP 14 | 15 | #include 16 | #include 17 | 18 | #include "g2o/stuff/sampler.h" 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "submaps_tools/submaps.hpp" 27 | 28 | //#include "graph_optimization/graph_construction.hpp" 29 | 30 | using namespace std; 31 | using namespace g2o; 32 | 33 | typedef pcl::PointCloud PointCloudT; 34 | typedef pcl::PointXYZ PointT; 35 | typedef g2o::GaussianSampler GaussianGen; 36 | 37 | Matrix generateGaussianNoise(GaussianGen& transSampler, 38 | GaussianGen& rotSampler); 39 | 40 | void addNoiseToSubmap(GaussianGen& transSampler, 41 | GaussianGen& rotSampler, 42 | SubmapObj& submap); 43 | 44 | void addNoiseToMap(GaussianGen& transSampler, 45 | GaussianGen& rotSampler, 46 | SubmapsVec& submap_set); 47 | 48 | #endif // UTILS_G2O_HPP 49 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(uw_slam_projects) 3 | 4 | # check c++14 / c++0x 5 | include(CheckCXXCompilerFlag) 6 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 7 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 8 | if(COMPILER_SUPPORTS_CXX14) 9 | set(CMAKE_CXX_FLAGS "-std=c++14 -O3 -fPIC") 10 | add_compile_options(-std=c++14 -Wall -Wextra) 11 | elseif(COMPILER_SUPPORTS_CXX0X) 12 | set(CMAKE_CXX_FLAGS "-std=c++11 -O3 -fPIC") 13 | add_compile_options(-std=c++11 -Wall -Wextra) 14 | else() 15 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 or C++11 support. Please use a different C++ compiler.") 16 | endif() 17 | 18 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -fPIC") 19 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99 -O3 -fPIC") 20 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 21 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 22 | set( CMAKE_BUILD_TYPE "Release" ) 23 | 24 | # Must use GNUInstallDirs to install libraries into correct 25 | # locations on all platforms. 26 | include(GNUInstallDirs) 27 | 28 | find_package(Ceres REQUIRED) 29 | find_package(Eigen3 REQUIRED) 30 | find_package(PCL 1.8 REQUIRED) 31 | find_package(OpenCV REQUIRED) 32 | find_package(g2o REQUIRED) 33 | find_package(yaml-cpp) 34 | 35 | # AUVLIB stuff 36 | # find_package(LIBIGL REQUIRED QUIET) 37 | find_package(LibXTF REQUIRED) 38 | find_package(LibGSF REQUIRED) 39 | find_package(EigenCereal REQUIRED) 40 | find_package(DataTools REQUIRED) 41 | find_package(SonarTracing REQUIRED) 42 | find_package(AuvlibGlad REQUIRED) 43 | find_package(BathyMaps REQUIRED) 44 | 45 | link_directories(${PCL_LIBRARY_DIRS}) 46 | link_directories(${G2O_LIBRARY_DIRS}) 47 | 48 | add_definitions(${PCL_DEFINITIONS}) 49 | add_definitions(-DCERES_GFLAGS_NAMESPACE=${GFLAGS_NAMESPACE}) 50 | 51 | include_directories( 52 | ${CERES_INCLUDE_DIRS} 53 | ${EIGEN_INCLUDE_DIRS} 54 | ${G2O_INCLUDE_DIRS} 55 | ${PCL_INCLUDE_DIRS} 56 | ${OPENCV_INCLUDE_DIRS} 57 | ${yaml_cpp_INCLUDE_DIRS} 58 | ) 59 | 60 | LIST(APPEND G2O_LIBS 61 | cxsparse 62 | cholmod 63 | g2o_cli g2o_ext_freeglut_minimal g2o_simulator 64 | g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d 65 | g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only 66 | g2o_types_sba g2o_types_slam3d g2o_csparse_extension 67 | g2o_opengl_helper g2o_solver_dense g2o_stuff 68 | g2o_types_sclam2d g2o_parser g2o_solver_pcg 69 | g2o_types_data g2o_types_sim3 70 | ) 71 | 72 | add_subdirectory(apps) 73 | add_subdirectory(graph_optimization) 74 | add_subdirectory(registration) 75 | add_subdirectory(submaps_tools) 76 | add_subdirectory(bathy_slam) 77 | add_subdirectory(meas_models) 78 | -------------------------------------------------------------------------------- /src/graph_optimization/src/types.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "graph_optimization/types.h" 13 | 14 | namespace ceres { 15 | namespace optimizer { 16 | 17 | std::istream& operator>>(std::istream& input, Pose3d& pose) { 18 | Eigen::Vector3d p; 19 | Eigen::Quaterniond q; 20 | 21 | input >> p.x() >> p.y() >> p.z() >> q.x() >> 22 | q.y() >> q.z() >> q.w(); 23 | // Normalize the quaternion to account for precision loss due to 24 | // serialization. 25 | pose.p = p; 26 | pose.q = q.normalized().toRotationMatrix().eulerAngles(0,1,2); 27 | 28 | return input; 29 | } 30 | 31 | typedef std::map, 32 | Eigen::aligned_allocator > > 33 | MapOfPoses; 34 | 35 | 36 | std::istream& operator>>(std::istream& input, Constraint3d& constraint) { 37 | Pose3d& t_be = constraint.t_be; 38 | input >> constraint.id_begin >> constraint.id_end >> t_be; 39 | // t_be.q = constraint.t_be.q.toRotationMatrix().eulerAngles(0,1,2); 40 | 41 | for (int i = 0; i < 6 && input.good(); ++i) { 42 | for (int j = i; j < 6 && input.good(); ++j) { 43 | input >> constraint.information(i, j); 44 | if (i != j) { 45 | constraint.information(j, i) = constraint.information(i, j); 46 | } 47 | } 48 | } 49 | return input; 50 | } 51 | 52 | } // namespace optimizer 53 | } // namespace ceres 54 | 55 | -------------------------------------------------------------------------------- /src/graph_optimization/include/graph_optimization/ceres_optimizer.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef CERES_OPTIMIZER_HPP 13 | #define CERES_OPTIMIZER_HPP 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "ceres/ceres.h" 20 | #include "graph_optimization/read_g2o.h" 21 | #include "graph_optimization/pose_graph_3d_error_term.h" 22 | #include "graph_optimization/types.h" 23 | #include "graph_optimization/graph_construction.hpp" 24 | 25 | #include "submaps_tools/submaps.hpp" 26 | 27 | #include "gflags/gflags.h" 28 | #include "glog/logging.h" 29 | 30 | 31 | namespace ceres{ 32 | namespace optimizer { 33 | 34 | // Constructs the nonlinear least squares optimization problem from the pose 35 | // graph constraints. 36 | void BuildOptimizationProblem(const VectorOfConstraints& constraints, 37 | MapOfPoses* poses, ceres::Problem* problem); 38 | 39 | // Returns true if the solve was successful. 40 | int SolveOptimizationProblem(ceres::Problem* problem); 41 | 42 | // Output the poses to the file with format: id x y z q_x q_y q_z q_w. 43 | bool OutputPoses(const std::string& filename, const MapOfPoses& poses); 44 | 45 | MapOfPoses ceresSolver(const std::string& outFilename, const int drConstraints); 46 | 47 | void updateSubmapsCeres(const MapOfPoses &poses, SubmapsVec& submaps_set); 48 | 49 | void saveOriginalTrajectory(SubmapsVec& submaps_set); 50 | 51 | } 52 | } 53 | 54 | #endif // CERES_OPTIMIZER_HPP 55 | -------------------------------------------------------------------------------- /src/graph_optimization/include/graph_optimization/graph_construction.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef GRAPH_CONSTRUCTION_HPP 13 | #define GRAPH_CONSTRUCTION_HPP 14 | 15 | #include "g2o/types/slam3d/vertex_se3.h" 16 | #include "g2o/types/slam3d/edge_se3.h" 17 | #include "g2o/core/factory.h" 18 | 19 | #include 20 | #include 21 | 22 | #include "submaps_tools/submaps.hpp" 23 | 24 | #include 25 | #include 26 | 27 | #include "graph_optimization/utils_g2o.hpp" 28 | 29 | using namespace std; 30 | using namespace Eigen; 31 | using namespace g2o; 32 | 33 | typedef std::vector > tf_vec; 34 | 35 | class GraphConstructor{ 36 | 37 | private: 38 | 39 | public: 40 | 41 | vector vertices_; 42 | vector drEdges_; 43 | vector lcEdges_; 44 | std::vector > covs_lc_; 45 | tf_vec drMeas_; 46 | tf_vec lcMeas_; 47 | tf_vec drChain_; 48 | int edge_covs_type_; 49 | 50 | GraphConstructor(std::vector > covs_lc); 51 | 52 | ~GraphConstructor(); 53 | 54 | void createNewVertex(SubmapObj& submap); 55 | 56 | void saveG2OFile(std::string outFilename); 57 | 58 | void findLoopClosures(SubmapObj &submap_i, 59 | const SubmapsVec& submaps_set, double info_thres); 60 | 61 | void createLCEdge(const SubmapObj& submap_from, const SubmapObj& submap_to); 62 | 63 | void createInitialEstimate(SubmapsVec &submaps_set); 64 | 65 | void createDREdge(const SubmapObj& submap); 66 | 67 | void addNoiseToGraph(GaussianGen& transSampler, GaussianGen& rotSampler); 68 | 69 | }; 70 | 71 | #endif // GRAPH_CONSTRUCTION_HPP 72 | -------------------------------------------------------------------------------- /src/registration/include/registration/utils_visualization.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef UTILS_VISUALIZATION_HPP 13 | #define UTILS_VISUALIZATION_HPP 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include "submaps_tools/submaps.hpp" 26 | #include "graph_optimization/graph_construction.hpp" 27 | #include "graph_optimization/ceres_optimizer.hpp" 28 | 29 | 30 | typedef pcl::PointCloud PointCloudT; 31 | typedef pcl::PointCloud PointCloudRGB; 32 | typedef pcl::PointXYZ PointT; 33 | using pcl::visualization::PCLVisualizer; 34 | typedef std::tuple grid_point; 35 | typedef std::vector > grid; 36 | 37 | class SubmapsVisualizer{ 38 | 39 | private: 40 | 41 | int vp1_; 42 | int vp2_; 43 | PCLVisualizer viewer_; 44 | int sample_cnt; 45 | int traj_cnt_; 46 | 47 | public: 48 | 49 | SubmapsVisualizer(PCLVisualizer& viewer); 50 | 51 | void setVisualizer(SubmapsVec &submaps_set, int num); 52 | 53 | void updateVisualizer(const SubmapsVec& submaps_set); 54 | 55 | // void plotPoseGraphG2O(const GraphConstructor& graph); 56 | 57 | void plotPoseGraphG2O(const GraphConstructor& graph, const SubmapsVec &submaps_set); 58 | 59 | void plotPoseGraphCeres(SubmapsVec &submaps_set); 60 | 61 | void addCoordinateSystem(const Eigen::Isometry3f& tf); 62 | 63 | void removeCoordinateSystem(); 64 | 65 | void visualizeGrid(const grid& grid); 66 | 67 | void addSubmap(const SubmapObj& submap_i, int vp); 68 | 69 | void plotMBESPing(const SubmapObj& submap_i, float spam, float n_beams, int vp); 70 | }; 71 | 72 | #endif // UTILS_VISUALIZATION_HPP 73 | -------------------------------------------------------------------------------- /src/registration/include/registration/gicp_reg.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef GICP_REG_HPP 13 | #define GICP_REG_HPP 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "submaps_tools/submaps.hpp" 28 | 29 | #include "data_tools/benchmark.h" 30 | 31 | #include "yaml-cpp/parser.h" 32 | #include "yaml-cpp/yaml.h" 33 | 34 | typedef std::vector > CovsVec; 35 | typedef std::shared_ptr CovsVecPtr; 36 | 37 | class SubmapRegistration { 38 | 39 | private: 40 | pcl::GeneralizedIterativeClosestPoint gicp_; 41 | Eigen::Matrix4f ret_tf_; 42 | benchmark::track_error_benchmark benchmark_; 43 | bool normal_use_knn_search; 44 | double normal_search_radius; 45 | int normal_search_k_neighbours; 46 | std::vector info_diag_values; 47 | 48 | public: 49 | 50 | SubmapRegistration(YAML::Node config); 51 | 52 | void loadConfig(YAML::Node config); 53 | 54 | bool gicpSubmapRegistration(SubmapObj &trg_submap, SubmapObj &src_submap); 55 | 56 | bool gicpSubmapRegistrationSimple(SubmapObj& trg_submap, SubmapObj& src_submap); 57 | 58 | void transformSubmap(SubmapObj& submap); 59 | 60 | SubmapObj constructTrgSubmap(const SubmapsVec& submaps_set, std::vector &overlaps, const DRNoise& dr_noise); 61 | 62 | double consistencyErrorOverlap(const SubmapObj& trg_submap, const SubmapObj& src_submap); 63 | 64 | ~SubmapRegistration(); 65 | 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | 68 | }; 69 | 70 | 71 | #endif // GICP_REG_HPP 72 | -------------------------------------------------------------------------------- /src/graph_optimization/include/graph_optimization/types.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef POSE_GRAPH_CERES_TYPES_H_ 13 | #define POSE_GRAPH_CERES_TYPES_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "Eigen/Core" 21 | #include "Eigen/Geometry" 22 | 23 | namespace ceres{ 24 | namespace optimizer{ 25 | 26 | struct Pose3d { 27 | Eigen::Vector3d p; // Position 28 | Eigen::Vector3d q; // Euler angles 29 | 30 | // The name of the data type in the g2o file format. 31 | static std::string name() { 32 | return "VERTEX_SE3:QUAT"; 33 | } 34 | 35 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 36 | }; 37 | 38 | std::istream& operator>>(std::istream& input, Pose3d& pose); 39 | 40 | typedef std::map, 41 | Eigen::aligned_allocator>> MapOfPoses; 42 | 43 | // The constraint between two vertices in the pose graph. The constraint is the 44 | // transformation from vertex id_begin to vertex id_end. 45 | struct Constraint3d { 46 | int id_begin; 47 | int id_end; 48 | 49 | // The transformation that represents the pose of the end frame E w.r.t. the 50 | // begin frame B. In other words, it transforms a vector in the E frame to 51 | // the B frame. 52 | Pose3d t_be; 53 | 54 | // The inverse of the covariance matrix for the measurement. The order of the 55 | // entries are x, y, z, delta orientation. 56 | Eigen::Matrix information; 57 | 58 | // The name of the data type in the g2o file format. 59 | static std::string name() { 60 | return "EDGE_SE3:QUAT"; 61 | } 62 | 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | }; 65 | 66 | std::istream& operator>>(std::istream& input, Constraint3d& constraint); 67 | 68 | typedef std::vector > 69 | VectorOfConstraints; 70 | 71 | } // namespace optimizer 72 | } // namespace ceres 73 | 74 | #endif // POSE_GRAPH_CERES_TYPES_H_ 75 | -------------------------------------------------------------------------------- /src/apps/src/read_auv_data.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "data_tools/transforms.h" 4 | #include "data_tools/benchmark.h" 5 | 6 | #include "data_tools/navi_data.h" 7 | #include "data_tools/std_data.h" 8 | #include "data_tools/gsf_data.h" 9 | #include 10 | #include 11 | #include 12 | 13 | #include "submaps_tools/submaps.hpp" 14 | #include "submaps_tools/cxxopts.hpp" 15 | 16 | #include "registration/utils_visualization.hpp" 17 | 18 | using namespace std; 19 | 20 | int main(int argc, char** argv) { 21 | string folder_str; 22 | string file_str; 23 | string type; 24 | 25 | cxxopts::Options options("example_reader", "Reads different mbes and sss file formats and saves them to a common format"); 26 | options.add_options() 27 | ("help", "Print help") 28 | ("folder", "Input folder containing mbes files", cxxopts::value(folder_str)) 29 | ("file", "Output file", cxxopts::value(file_str)) 30 | ("type", "Type of data to read, options: all, xtf, navi, gsf", cxxopts::value(type)); 31 | 32 | auto result = options.parse(argc, argv); 33 | if (result.count("help")) { 34 | cout << options.help({ "", "Group" }) << endl; 35 | exit(0); 36 | } 37 | 38 | if (result.count("folder") == 0) { 39 | cout << "Please provide folder containing mbes or sss files..." << endl; 40 | exit(0); 41 | } 42 | if (result.count("type") == 0) { 43 | cout << "Please provide input type, options: all, xtf, navi, gsf" << endl; 44 | exit(0); 45 | } 46 | 47 | 48 | boost::filesystem::path folder(folder_str); 49 | boost::filesystem::path pings_path = folder / "mbes_pings.cereal"; 50 | boost::filesystem::path submaps_path = folder / "submaps.cereal"; 51 | 52 | // if side scan, read and save, then return 53 | if (type == "xtf") { 54 | cout << "Not much to do by now" << endl; 55 | return 0; 56 | } 57 | 58 | // otherwise we have multibeam data, read and save 59 | std_data::mbes_ping::PingsT std_pings; 60 | if (type == "gsf") { 61 | gsf_data::gsf_mbes_ping::PingsT pings = std_data::parse_folder(folder); 62 | std::stable_sort(pings.begin(), pings.end(), [](const gsf_data::gsf_mbes_ping& ping1, const gsf_data::gsf_mbes_ping& ping2) { 63 | return ping1.time_stamp_ < ping2.time_stamp_; 64 | }); 65 | std_pings = gsf_data::convert_pings(pings); 66 | } 67 | else if (type == "all") { 68 | all_data::all_nav_entry::EntriesT entries = std_data::parse_folder(folder); 69 | all_data::all_mbes_ping::PingsT pings = std_data::parse_folder(folder); 70 | all_data::all_nav_attitude::EntriesT attitudes = std_data::parse_folder(folder); 71 | std_pings = all_data::convert_matched_entries(pings, entries, 0.); 72 | std_pings = all_data::match_attitude(std_pings, attitudes); 73 | } 74 | else if (type == "navi") { 75 | std_pings = std_data::parse_folder(folder / "Pings"); 76 | std_data::nav_entry::EntriesT entries = std_data::parse_folder(folder / "NavUTM"); 77 | navi_data::match_timestamps(std_pings, entries); 78 | } 79 | else { 80 | cout << "Type " << type << " is not supported!" << endl; 81 | return 0; 82 | } 83 | 84 | // write to disk 85 | std_data::write_data(std_pings, pings_path); 86 | 87 | 88 | return 0; 89 | } 90 | -------------------------------------------------------------------------------- /scripts/plot_results.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import matplotlib.pyplot as plot 4 | import numpy 5 | import sys 6 | from optparse import OptionParser 7 | 8 | parser = OptionParser() 9 | parser.add_option("--initial_poses", 10 | dest="initial_poses", 11 | default="", 12 | help="The filename that contains the original poses.") 13 | parser.add_option("--corrupted_poses", 14 | dest="corrupted_poses", 15 | default="", 16 | help="The filename that contains the optimized poses.") 17 | parser.add_option("--optimized_poses", 18 | dest="optimized_poses", 19 | default="", 20 | help="The filename that contains the optimized poses.") 21 | parser.add_option("-e", 22 | "--axes_equal", 23 | action="store_true", 24 | dest="axes_equal", 25 | default="", 26 | help="Make the plot axes equal.") 27 | parser.add_option("--output_file", 28 | dest="outputFile", 29 | default="", 30 | help="The output file.") 31 | 32 | (options, args) = parser.parse_args() 33 | 34 | # Read the original and optimized poses files. 35 | poses_original = None 36 | if options.initial_poses != '': 37 | poses_original = numpy.genfromtxt(options.initial_poses, usecols=(1, 2, 3)) 38 | 39 | poses_corrupted = None 40 | if options.corrupted_poses != '': 41 | poses_corrupted = numpy.genfromtxt(options.corrupted_poses, 42 | usecols=(1, 2, 3)) 43 | 44 | poses_optimized = None 45 | if options.optimized_poses != '': 46 | poses_optimized = numpy.genfromtxt(options.optimized_poses, 47 | usecols=(1, 2, 3)) 48 | 49 | sum_opt = 0.0 50 | sum_corr = 0.0 51 | for i in range(0, len(poses_optimized)): 52 | sum_opt += numpy.linalg.norm(poses_original[i, :] - poses_optimized[i, :]) 53 | sum_corr += numpy.linalg.norm(poses_original[i, :] - poses_corrupted[i, :]) 54 | 55 | sum_opt /= len(poses_optimized) 56 | sum_corr /= len(poses_optimized) 57 | 58 | print("Diff original and corrupted", sum_corr) 59 | print("Diff original and optimized", sum_opt) 60 | 61 | # with open(options.outputFile, "a") as text_file: 62 | # text_file.write("%s" % sum_corr) 63 | # text_file.write(" %s" % sum_opt) 64 | # text_file.close() 65 | # 66 | # Plots the results for the specified poses. 67 | figure = plot.figure() 68 | 69 | if poses_original is not None: 70 | axes = plot.subplot(1, 1, 1, projection='3d') 71 | 72 | plot.plot(poses_original[:, 0], 73 | poses_original[:, 1], 74 | poses_original[:, 2], 75 | '-', 76 | alpha=0.5, 77 | color="green", 78 | linewidth=2.0) 79 | 80 | if poses_corrupted is not None: 81 | plot.plot(poses_corrupted[:, 0], 82 | poses_corrupted[:, 1], 83 | poses_corrupted[:, 2], 84 | '-', 85 | alpha=0.5, 86 | color="red", 87 | linewidth=2.0) 88 | 89 | if poses_optimized is not None: 90 | plot.plot(poses_optimized[:, 0], 91 | poses_optimized[:, 1], 92 | poses_optimized[:, 2], 93 | '-', 94 | alpha=0.5, 95 | color="blue", 96 | linewidth=2.0) 97 | 98 | plot.title('Trajectories: GT (Green), Corrupted (Red), optimized (Blue)') 99 | 100 | plot.show() 101 | -------------------------------------------------------------------------------- /src/registration/include/registration/utils_registration.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef UTILS_HPP 13 | #define UTILS_HPP 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | 27 | 28 | using namespace std; 29 | typedef pcl::PointCloud PointCloudT; 30 | typedef pcl::PointXYZ PointT; 31 | 32 | 33 | struct submap { 34 | public: 35 | int submap_id; 36 | PointCloudT submap_pcl; 37 | Eigen::Matrix4f tf_submap_map; 38 | std::vector> auv_pose_; // AUV pose for each ping 39 | std::vector> pcl_covariances; 40 | Eigen::MatrixXf cov_submap_frame; // 6x6 cov of submap frame 41 | unsigned int pings_num_; 42 | unsigned int beams_per_ping_; 43 | 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | }; 46 | 47 | 48 | struct icp_match{ 49 | public: 50 | 51 | icp_match(PointT trg_point, PointT src_point, Eigen::Vector3f normal, Eigen::Vector3f error_mean, Eigen::Matrix3f error_sigma){ 52 | trg_point_ = trg_point; 53 | src_point_ = src_point; 54 | normal_ = normal; 55 | error_mean_ = error_mean; 56 | error_sigma_ = error_sigma; 57 | } 58 | 59 | PointT trg_point_; 60 | PointT src_point_; 61 | Eigen::Vector3f normal_; 62 | // Components of error pdf 63 | Eigen::Vector3f error_mean_; 64 | Eigen::Matrix3f error_sigma_; 65 | 66 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 67 | }; 68 | 69 | void subsampleMap(submap& submap_input); 70 | 71 | void outlierFilter(submap& submap_input); 72 | 73 | Eigen::Vector3f computePCAPcl(PointCloudT& set_Ai); 74 | 75 | Eigen::Affine3d create_rotation_matrix(double ax, double ay, double az); 76 | 77 | Eigen::Matrix4f inverseTfMatrix(Eigen::Matrix4f tf_mat); 78 | 79 | void plotSubmapsSet(const std::vector > &submaps_set); 80 | 81 | void print4x4Matrix (const Eigen::Matrix4f & matrix); 82 | 83 | #endif // UTILS_HPP 84 | -------------------------------------------------------------------------------- /src/bathy_slam/src/bathy_slam.cpp: -------------------------------------------------------------------------------- 1 | #include "bathy_slam/bathy_slam.hpp" 2 | 3 | 4 | BathySlam::BathySlam(GraphConstructor &graph_obj, SubmapRegistration &gicp_reg): 5 | graph_obj_(&graph_obj), gicp_reg_(&gicp_reg){ 6 | 7 | } 8 | 9 | BathySlam::~BathySlam(){ 10 | 11 | } 12 | 13 | SubmapsVec BathySlam::runOffline(SubmapsVec& submaps_gt, GaussianGen& transSampler, GaussianGen& rotSampler, YAML::Node config){ 14 | DRNoise dr_noise = loadDRNoiseFromFile(config); 15 | SubmapObj submap_trg(dr_noise); 16 | SubmapsVec submaps_prev, submaps_reg; 17 | ofstream fileOutputStream; 18 | fileOutputStream.open("loop_closures.txt", std::ofstream::out); 19 | 20 | double info_thres = 0.1; 21 | for(SubmapObj& submap_i: submaps_gt){ 22 | std::cout << " ----------- Submap " << submap_i.submap_id_ << ", swath " 23 | << submap_i.swath_id_ << " ------------"<< std::endl; 24 | 25 | // Look for loop closures 26 | for(SubmapObj& submap_k: submaps_reg){ 27 | // Don't look for overlaps between submaps of the same swath or the prev submap 28 | if(submap_k.submap_id_ != submap_i.submap_id_ - 1){ 29 | submaps_prev.push_back(submap_k); 30 | } 31 | } 32 | // Submaps in map_frame? 33 | bool submaps_in_map_tf = true; 34 | submap_i.findOverlaps(submaps_in_map_tf, submaps_prev, config["overlap_coverage"].as()); 35 | submaps_prev.clear(); 36 | 37 | #if INTERACTIVE == 1 38 | // Update visualizer 39 | submaps_reg.push_back(submap_i); // Add submap_i to registered set (just for visualization here) 40 | visualizer->updateVisualizer(submaps_reg); 41 | while(!viewer.wasStopped ()){ 42 | viewer.spinOnce (); 43 | } 44 | viewer.resetStoppedFlag(); 45 | submaps_reg.pop_back(); 46 | #endif 47 | // Create graph vertex i 48 | graph_obj_->createNewVertex(submap_i); 49 | 50 | // Create DR edge i and store (skip submap 0) 51 | if(submap_i.submap_id_ != 0 ){ 52 | std::cout << "DR edge from " << submap_i.submap_id_ -1 << " to " << submap_i.submap_id_<< std::endl; 53 | graph_obj_->createDREdge(submap_i); 54 | } 55 | 56 | // If potential loop closure detected 57 | SubmapObj submap_final = submap_i; 58 | if(!submap_i.overlaps_idx_.empty()){ 59 | // Save loop closure to txt 60 | if(fileOutputStream.is_open()){ 61 | fileOutputStream << submap_i.submap_id_; 62 | for(unsigned int j=0; jconstructTrgSubmap(submaps_reg, submap_i.overlaps_idx_, dr_noise); 70 | if (config["add_gaussian_noise"].as()) { 71 | addNoiseToSubmap(transSampler, rotSampler, submap_i); // Add disturbance to source submap 72 | } 73 | 74 | if(gicp_reg_->gicpSubmapRegistration(submap_trg, submap_i)){ 75 | submap_final = submap_i; 76 | } 77 | submap_trg.submap_pcl_.clear(); 78 | 79 | // Create loop closures 80 | graph_obj_->edge_covs_type_ = config["lc_edge_covs_type"].as(); 81 | graph_obj_->findLoopClosures(submap_final, submaps_reg, info_thres); 82 | } 83 | submaps_reg.push_back(submap_final); // Add registered submap_i 84 | 85 | #if INTERACTIVE == 1 86 | // Update visualizer 87 | visualizer->updateVisualizer(submaps_reg); 88 | while(!viewer.wasStopped ()){ 89 | viewer.spinOnce (); 90 | } 91 | viewer.resetStoppedFlag(); 92 | #endif 93 | } 94 | fileOutputStream.close(); 95 | 96 | 97 | return submaps_reg; 98 | } 99 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Bathymetric Graph SLAM 2 | 3 | Baseline SLAM framework for underwater vehicles. 4 | The algorithm gets a set of bathymetric submaps as input and corrects the global map constructed while refining the vehicle trajectory through a map-to-map registration followed by a pose graph optimization. 5 | 6 | 7 | ![real_data_example](https://github.com/ignaciotb/bathymetric_slam/blob/master/img/graph_borno.png) 8 | 9 | 10 | ## Paper introducing and applying the method 11 | The method implemented is described [in this paper](https://ieeexplore.ieee.org/abstract/document/8968241) and used [in this one](https://arxiv.org/abs/2003.10931) 12 | ``` 13 | @inproceedings{torroba2019towards, 14 | title={Towards Autonomous Industrial-Scale Bathymetric Surveying}, 15 | author={Torroba, Ignacio and Bore, Nils and Folkesson, John}, 16 | booktitle={2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 17 | pages={6377--6382}, 18 | year={2019}, 19 | organization={IEEE} 20 | } 21 | 22 | @article{torroba2020pointnetkl, 23 | title={PointNetKL: Deep Inference for GICP Covariance Estimation in Bathymetric SLAM}, 24 | author={Torroba, Ignacio and Sprague, Christopher Iliffe and Bore, Nils and Folkesson, John}, 25 | journal={IEEE Robotics and Automation Letters}, 26 | volume={5}, 27 | number={3}, 28 | pages={4078--4085}, 29 | year={2020}, 30 | publisher={IEEE} 31 | } 32 | ``` 33 | 34 | ## Dependencies (tested on Ubuntu 16.04 and 18.04) 35 | * AUVLIB [here](https://github.com/ignaciotb/auvlib) 36 | * PCL http://pointclouds.org/ 37 | * G2O https://github.com/RainerKuemmerle/g2o 38 | * Ceres http://ceres-solver.org/ 39 | 40 | Note that for G2O to be used by this repo you need to install it at a system level. 41 | From the G2O build folder, run 42 | ``` 43 | sudo make install 44 | ``` 45 | 46 | ## Building 47 | 48 | Clone this repository and create a `build` folder under the root, then execute 49 | ``` 50 | cd build 51 | cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=../install .. 52 | make -j4 53 | make install 54 | ``` 55 | 56 | Finally, add the following line to your ~/.bashrc file adapted to your own installation 57 | ``` 58 | export PATH=$PATH:/path/to/folder/bathymetric_slam/install/share 59 | ``` 60 | ### Available apps 61 | Under `bin` folder. 62 | The process outputs .png images with the maps of bathymetry and consistency error. 63 | The current script optimizes the graph with Ceres, but the app outputs a "graph.g2o" file which you can solve with G2O if preferred. 64 | 65 | ##### SLAM with simulated data 66 | In order to test the framework with data from the [SMARC simulator](https://github.com/smarc-project), use the toy dataset `map_small` under `sim_data`. 67 | You can visualize both the ground truth map and vehicle trajectory in the visualizer. To start the optimization process, hit "q". 68 | ``` 69 | ./bathy_slam_real --simulation yes --bathy_survey ../sim_data/map_small/ 70 | ``` 71 | The simulation outputs a measure of the error contained in the map, as well as the height maps and error plots as .png files. 72 | To increase the complexity of the sim dataset, increase the Gaussian noise to the vehicle's position estimate. 73 | In order to adapt the performance of the algorithm to the dataset, adjust the weights of the edges of the pose-graph accordingly and tune the GICP and the Ceres solver parameters. 74 | The algorithm is **not** by default tuned for the toy example `map_small`. 75 | 76 | ##### SLAM with real data 77 | To run the SLAM solution with real data from a bathymetric survey, currently the input is in the form of a cereal file containing all the necessary information from your data files. 78 | You can find a real survey carried out with an ROV [here](https://strands.pdc.kth.se/public/IROS-2019-Bathymetry/). Download it, adjust the framework values, and test it. 79 | ``` 80 | ./bathy_slam_real --simulation no --bathy_survey /path/to/datasets/mbes_pings.cereal 81 | ``` 82 | ### Generating your own cereal files from real surveys 83 | Take a look at the [AUVLIB](https://github.com/nilsbore/auvlib) toolbox in order to parse real MBES, SSS, navigation, etc data from the most common formats into .cereal files. 84 | -------------------------------------------------------------------------------- /src/graph_optimization/include/graph_optimization/pose_graph_3d_error_term.h: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef CERES_POSE_GRAPH_3D_ERROR_TERM_H_ 13 | #define CERES_POSE_GRAPH_3D_ERROR_TERM_H_ 14 | 15 | #include "Eigen/Core" 16 | #include "ceres/autodiff_cost_function.h" 17 | 18 | #include "graph_optimization/types.h" 19 | 20 | namespace ceres { 21 | namespace optimizer { 22 | 23 | template Eigen::Quaternion eulerToQuat(const Eigen::Matrix& vec){ 24 | 25 | Eigen::Quaternion q = Eigen::AngleAxis(vec[0], Eigen::Matrix::UnitX()) 26 | * Eigen::AngleAxis(vec[1], Eigen::Matrix::UnitY()) 27 | * Eigen::AngleAxis(vec[2], Eigen::Matrix::UnitZ()); 28 | return q; 29 | } 30 | 31 | 32 | class PoseGraph3dErrorTerm { 33 | public: 34 | PoseGraph3dErrorTerm(const Pose3d& t_ab_measured, const Eigen::Matrix& sqrt_information) 35 | : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {} 36 | 37 | template 38 | bool operator()(const T* const p_a_ptr, const T* const q_a_ptr, 39 | const T* const p_b_ptr, const T* const q_b_ptr, 40 | T* residuals_ptr) const 41 | { 42 | Eigen::Map > p_a(p_a_ptr); 43 | Eigen::Map > q_a(q_a_ptr); 44 | Eigen::Map > p_b(p_b_ptr); 45 | Eigen::Map > q_b(q_b_ptr); 46 | 47 | // Compute the relative transformation between the two frames. 48 | Eigen::Quaternion q_a_quat = eulerToQuat(q_a); 49 | Eigen::Quaternion q_b_quat = eulerToQuat(q_b); 50 | Eigen::Quaternion q_a_inverse = q_a_quat.conjugate(); 51 | Eigen::Quaternion q_ab_estimated = q_a_inverse * q_b_quat; 52 | 53 | // Represent the displacement between the two frames in the A frame. 54 | Eigen::Matrix p_ab_estimated = q_a_inverse * (p_b - p_a); 55 | 56 | // Compute the error between the two orientation estimates. 57 | Eigen::Matrix t_ab_meas_q = t_ab_measured_.q.template cast(); 58 | Eigen::Quaternion q_meas = eulerToQuat(t_ab_meas_q); 59 | Eigen::Quaternion delta_q = q_meas * q_ab_estimated.conjugate(); 60 | 61 | // Compute the residuals. 62 | Eigen::Map > residuals(residuals_ptr); 63 | residuals.template block<3, 1>(0, 0) = p_ab_estimated - t_ab_measured_.p.template cast(); 64 | residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec(); 65 | 66 | // Scale the residuals by the measurement uncertainty. 67 | residuals.applyOnTheLeft(sqrt_information_.template cast()); 68 | 69 | return true; 70 | } 71 | 72 | static ceres::CostFunction* Create(const Pose3d& t_ab_measured, const Eigen::Matrix& sqrt_information) 73 | { 74 | return new ceres::AutoDiffCostFunction( 75 | new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information)); 76 | } 77 | 78 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 79 | 80 | private: 81 | // The measurement for the position of B relative to A in the A frame. 82 | const Pose3d t_ab_measured_; 83 | // The square root of the measurement information matrix. 84 | const Eigen::Matrix sqrt_information_; 85 | }; 86 | 87 | } // namespace optimizer 88 | } // namespace ceres 89 | 90 | #endif // CERES_POSE_GRAPH_3D_ERROR_TERM_H_ 91 | -------------------------------------------------------------------------------- /src/graph_optimization/include/graph_optimization/read_g2o.h: -------------------------------------------------------------------------------- 1 | // Ceres Solver - A fast non-linear least squares minimizer 2 | // Copyright 2016 Google Inc. All rights reserved. 3 | // http://ceres-solver.org/ 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // 8 | // * Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright notice, 11 | // this list of conditions and the following disclaimer in the documentation 12 | // and/or other materials provided with the distribution. 13 | // * Neither the name of Google Inc. nor the names of its contributors may be 14 | // used to endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | // 29 | // Author: vitus@google.com (Michael Vitus) 30 | // 31 | // Reads a file in the g2o filename format that describes a pose graph problem. 32 | 33 | #ifndef READ_G2O_H_ 34 | #define READ_G2O_H_ 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include "glog/logging.h" 41 | 42 | namespace ceres{ 43 | namespace optimizer{ 44 | 45 | // Reads a single pose from the input and inserts it into the map. Returns false 46 | // if there is a duplicate entry. 47 | template 48 | bool ReadVertex(std::ifstream* infile, 49 | std::map, Allocator>* poses) { 50 | int id; 51 | Pose pose; 52 | *infile >> id >> pose; 53 | 54 | // Ensure we don't have duplicate poses. 55 | if (poses->find(id) != poses->end()) { 56 | LOG(ERROR) << "Duplicate vertex with ID: " << id; 57 | return false; 58 | } 59 | (*poses)[id] = pose; 60 | 61 | return true; 62 | } 63 | 64 | // Reads the contraints between two vertices in the pose graph 65 | template 66 | void ReadConstraint(std::ifstream* infile, 67 | std::vector* constraints) { 68 | Constraint constraint; 69 | *infile >> constraint; 70 | 71 | constraints->push_back(constraint); 72 | } 73 | 74 | // Reads a file in the g2o filename format that describes a pose graph 75 | // problem. The g2o format consists of two entries, vertices and constraints. 76 | // 77 | // In 2D, a vertex is defined as follows: 78 | // 79 | // VERTEX_SE2 ID x_meters y_meters yaw_radians 80 | // 81 | // A constraint is defined as follows: 82 | // 83 | // EDGE_SE2 ID_A ID_B A_x_B A_y_B A_yaw_B I_11 I_12 I_13 I_22 I_23 I_33 84 | // 85 | // where I_ij is the (i, j)-th entry of the information matrix for the 86 | // measurement. 87 | // 88 | // 89 | // In 3D, a vertex is defined as follows: 90 | // 91 | // VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w 92 | // 93 | // where the quaternion is in Hamilton form. 94 | // A constraint is defined as follows: 95 | // 96 | // EDGE_SE3:QUAT ID_a ID_b x_ab y_ab z_ab q_x_ab q_y_ab q_z_ab q_w_ab I_11 I_12 I_13 ... I_16 I_22 I_23 ... I_26 ... I_66 // NOLINT 97 | // 98 | // where I_ij is the (i, j)-th entry of the information matrix for the 99 | // measurement. Only the upper-triangular part is stored. The measurement order 100 | // is the delta position followed by the delta orientation. 101 | template 103 | bool ReadG2oFile(const std::string& filename, 104 | std::map, MapAllocator>* poses, 105 | std::vector* constraints) { 106 | CHECK(poses != NULL); 107 | CHECK(constraints != NULL); 108 | 109 | poses->clear(); 110 | constraints->clear(); 111 | 112 | std::ifstream infile(filename.c_str()); 113 | if (!infile) { 114 | return false; 115 | } 116 | 117 | std::string data_type; 118 | while (infile.good()) { 119 | // Read whether the type is a node or a constraint. 120 | infile >> data_type; 121 | if (data_type == Pose::name()) { 122 | if (!ReadVertex(&infile, poses)) { 123 | return false; 124 | } 125 | } 126 | else if (data_type == Constraint::name()) { 127 | ReadConstraint(&infile, constraints); 128 | } 129 | else { 130 | LOG(ERROR) << "Unknown data type: " << data_type; 131 | return false; 132 | } 133 | 134 | // Clear any trailing whitespace from the line. 135 | infile >> std::ws; 136 | } 137 | 138 | return true; 139 | } 140 | 141 | } // namespace optimizer 142 | } // namespace ceres 143 | 144 | #endif // READ_G2O_H_ 145 | -------------------------------------------------------------------------------- /src/submaps_tools/include/submaps_tools/submaps.hpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #ifndef SUBMAPS_HPP 13 | #define SUBMAPS_HPP 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include "data_tools/std_data.h" 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | #include "yaml-cpp/parser.h" 33 | #include "yaml-cpp/yaml.h" 34 | 35 | using namespace std; 36 | using namespace Eigen; 37 | typedef std::vector> PointsT; 38 | typedef pcl::PointCloud PointCloudT; 39 | typedef pcl::PointXYZ PointT; 40 | typedef std::vector> corners; 41 | typedef std::vector > covs; 42 | 43 | struct DRNoise{ 44 | double x, y, z; 45 | double roll, pitch, yaw; 46 | }; 47 | 48 | class SubmapObj{ 49 | 50 | private: 51 | 52 | public: 53 | int submap_id_; 54 | int swath_id_; 55 | PointCloudT submap_pcl_; 56 | std::vector overlaps_idx_; 57 | Eigen::Vector3d colors_; 58 | Eigen::Isometry3f submap_tf_; 59 | Eigen::Matrix submap_info_; 60 | Eigen::Matrix submap_lc_info_; 61 | Eigen::MatrixXd auv_tracks_; 62 | 63 | SubmapObj(const DRNoise& dr_noise); 64 | 65 | SubmapObj(const unsigned int& submap_id, const unsigned int& swath_id, PointCloudT& submap_pcl, const DRNoise& dr_noise); 66 | 67 | void findOverlaps(bool submaps_in_map_tf, std::vector > &submaps_set, double overlap_coverage); 68 | 69 | Eigen::Matrix createDRWeights(const DRNoise& dr_noise); 70 | 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | }; 73 | 74 | class MapObj: public SubmapObj{ 75 | 76 | public: 77 | 78 | MapObj(const DRNoise& dr_noise) : SubmapObj(dr_noise) {}; 79 | 80 | MapObj(const DRNoise& dr_noise, PointCloudT& map_pcl) : SubmapObj(dr_noise) {}; 81 | 82 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 83 | }; 84 | 85 | template 86 | void save(Archive & archive, 87 | SubmapObj const & m) 88 | { 89 | Eigen::MatrixXf points = m.submap_pcl_.getMatrixXfMap(3,4,0).transpose(); 90 | archive(CEREAL_NVP(m.submap_id_), CEREAL_NVP(m.swath_id_), CEREAL_NVP(points), 91 | CEREAL_NVP(m.overlaps_idx_), CEREAL_NVP(m.colors_), CEREAL_NVP(m.submap_tf_.matrix()), CEREAL_NVP(m.submap_info_), 92 | CEREAL_NVP(m.auv_tracks_)); 93 | } 94 | 95 | template 96 | void load(Archive & archive, 97 | SubmapObj & m) 98 | { 99 | Eigen::MatrixXf points; 100 | archive(CEREAL_NVP(m.submap_id_), CEREAL_NVP(m.swath_id_), CEREAL_NVP(points), 101 | CEREAL_NVP(m.overlaps_idx_), CEREAL_NVP(m.colors_), CEREAL_NVP(m.submap_tf_.matrix()), CEREAL_NVP(m.submap_info_), 102 | CEREAL_NVP(m.auv_tracks_)); 103 | for(unsigned int i=0; i> SubmapsVec; 109 | 110 | void readSubmapFile(const string submap_str, PointCloudT::Ptr submap_pcl); 111 | 112 | std::vector checkFilesInDir(DIR *dir); 113 | 114 | std::vector > readSubmapsInDir(const string& dir_path, const DRNoise& dr_noise); 115 | 116 | Array3f computeInfoInSubmap(const SubmapObj& submap); 117 | 118 | SubmapsVec parseSubmapsAUVlib(std_data::pt_submaps& ss); 119 | 120 | std::tuple parseMapAUVlib(std_data::pt_submaps& ss, const DRNoise& dr_noise); 121 | 122 | SubmapsVec parsePingsAUVlib(std_data::mbes_ping::PingsT& pings, const DRNoise& dr_noise); 123 | 124 | SubmapsVec createSubmaps(SubmapsVec& pings, int submap_size, const DRNoise& dr_noise); 125 | SubmapsVec createMap(SubmapsVec& pings, int submap_size, const DRNoise& dr_noise); 126 | 127 | void transformSubmapObj(SubmapObj& submap, Isometry3f& poseDRt); 128 | 129 | std::pair getSubmapCorners(bool submaps_in_map_tf, const SubmapObj& submap, double overlap_coverage); 130 | 131 | bool checkSubmapsOverlap(const corners submap_i_corners, const corners submap_k_corners); 132 | 133 | bool pointToLine(const Vector3d seg_a, const Vector3d seg_b, const Vector3d point_c); 134 | 135 | bool checkSubmapSize(const SubmapObj& submap_i, double overlap_coverage); 136 | 137 | template int sgn(T val) { 138 | return (T(0) < val) - (val < T(0)); 139 | } 140 | 141 | PointsT pclToMatrixSubmap(const SubmapsVec& submaps_set); 142 | 143 | PointsT trackToMatrixSubmap(const SubmapsVec& submaps_set); 144 | 145 | std::pair readCovMatrix(const std::string& file_name); 146 | 147 | covs readCovsFromFiles(boost::filesystem::path folder); 148 | 149 | std::vector > readGTLoopClosures(string& fileName, int submaps_nb); 150 | 151 | DRNoise loadDRNoiseFromFile(YAML::Node config); 152 | 153 | #endif // SUBMAPS_HPP 154 | -------------------------------------------------------------------------------- /src/graph_optimization/src/utils_g2o.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "graph_optimization/utils_g2o.hpp" 13 | 14 | using namespace std; 15 | using namespace g2o; 16 | using namespace Eigen; 17 | 18 | Matrix generateGaussianNoise(GaussianGen& transSampler, 19 | GaussianGen& rotSampler){ 20 | 21 | bool randomSeed = true; 22 | std::vector noiseTranslation; 23 | std::vector noiseRotation; 24 | noiseTranslation.push_back(3); 25 | noiseTranslation.push_back(3); 26 | noiseTranslation.push_back(0.001); 27 | noiseRotation.push_back(0.0001); 28 | noiseRotation.push_back(0.0001); 29 | noiseRotation.push_back(0.001); 30 | 31 | Eigen::Matrix3d transNoise = Eigen::Matrix3d::Zero(); 32 | for (int i = 0; i < 3; ++i) 33 | transNoise(i, i) = std::pow(noiseTranslation[i], 2); 34 | 35 | Eigen::Matrix3d rotNoise = Eigen::Matrix3d::Zero(); 36 | for (int i = 0; i < 3; ++i) 37 | rotNoise(i, i) = std::pow(noiseRotation[i], 2); 38 | 39 | // Information matrix of the distribution 40 | Eigen::Matrix information = Eigen::Matrix::Zero(); 41 | information.block<3,3>(0,0) = transNoise.inverse(); 42 | information.block<3,3>(3,3) = rotNoise.inverse(); 43 | 44 | // Gaussian noise generators 45 | transSampler.setDistribution(transNoise); 46 | rotSampler.setDistribution(rotNoise); 47 | 48 | if (randomSeed) { 49 | std::random_device r; 50 | std::seed_seq seedSeq{r(), r(), r(), r(), r()}; 51 | vector seeds(2); 52 | seedSeq.generate(seeds.begin(), seeds.end()); 53 | transSampler.seed(seeds[0]); 54 | rotSampler.seed(seeds[1]); 55 | } 56 | return information; 57 | } 58 | 59 | void addNoiseToSubmap(GaussianGen& transSampler, 60 | GaussianGen& rotSampler, 61 | SubmapObj& submap){ 62 | 63 | Eigen::Quaterniond gtQuat = (Eigen::Quaterniond)submap.submap_tf_.linear().cast(); 64 | Eigen::Vector3d gtTrans = submap.submap_tf_.translation().cast(); 65 | 66 | Eigen::Vector3d quatXYZ = rotSampler.generateSample(); 67 | double qw = 1.0 - quatXYZ.norm(); 68 | if (qw < 0) { 69 | qw = 0.; 70 | cerr << "x"; 71 | } 72 | std::random_device rd{}; 73 | std::mt19937 gen{rd()}; 74 | std::normal_distribution<> d{0,0.1}; 75 | 76 | // Eigen::Quaterniond rot(qw, quatXYZ.x(), quatXYZ.y(), quatXYZ.z()); 77 | // Bias in yaw 78 | double roll = 0.0, pitch = 0.0, yaw = /*0.001*/ d(gen); 79 | Matrix3d m; 80 | m = AngleAxisd(roll, Vector3d::UnitX()) 81 | * AngleAxisd(pitch, Vector3d::UnitY()) 82 | * AngleAxisd(yaw, Vector3d::UnitZ()); 83 | Eigen::Quaterniond rot(m); 84 | 85 | Eigen::Vector3d trans; 86 | // trans = transSampler.generateSample(); 87 | trans.setZero(); 88 | 89 | trans = gtTrans + trans; 90 | rot = gtQuat * rot; 91 | 92 | Eigen::Isometry3d noisyMeasurement = (Eigen::Isometry3d) rot; 93 | noisyMeasurement.translation() = trans; 94 | 95 | // Transform submap_i pcl and tf 96 | pcl::transformPointCloud(submap.submap_pcl_, submap.submap_pcl_, 97 | (noisyMeasurement.cast() * submap.submap_tf_.inverse()).matrix()); 98 | 99 | submap.submap_tf_ = noisyMeasurement.cast(); 100 | } 101 | 102 | void addNoiseToMap(GaussianGen& transSampler, 103 | GaussianGen& rotSampler, 104 | SubmapsVec& submap_set){ 105 | 106 | for (size_t i =1; i < submap_set.size(); i++){ 107 | 108 | std::cout << i << " -------" << std::endl; 109 | std::cout << submap_set.at(i-1).submap_tf_.matrix() << std::endl; 110 | 111 | } 112 | // Noise for all the submaps 113 | for (size_t i =1; i < submap_set.size(); i++){ 114 | Eigen::Isometry3f tf_prev = submap_set.at(i-1).submap_tf_; 115 | 116 | Eigen::Isometry3f meas_i = tf_prev.inverse() * submap_set.at(i).submap_tf_; 117 | Eigen::Quaternionf gtQuat = (Eigen::Quaternionf)meas_i.linear(); 118 | Eigen::Vector3f gtTrans = meas_i.translation(); 119 | 120 | std::random_device rd{}; 121 | std::mt19937 gen{rd()}; 122 | std::normal_distribution<> d{0,0.5}; 123 | 124 | // Bias in yaw 125 | float roll = 0.0, pitch = 0.0, yaw = /*0.5*/ d(gen); 126 | Matrix3f m; 127 | m = AngleAxisf(roll, Vector3f::UnitX()) 128 | * AngleAxisf(pitch, Vector3f::UnitY()) 129 | * AngleAxisf(yaw, Vector3f::UnitZ()); 130 | Eigen::Quaternionf rot(m); 131 | 132 | rot = gtQuat * rot; 133 | 134 | meas_i = (Eigen::Isometry3f) rot; 135 | meas_i.translation() = gtTrans; 136 | 137 | Eigen::Isometry3f estimate_i = tf_prev * meas_i; 138 | 139 | std::cout << i << " -------" << std::endl; 140 | std::cout << tf_prev.matrix() << std::endl; 141 | std::cout << meas_i.matrix() << std::endl; 142 | std::cout << estimate_i.matrix() << std::endl; 143 | 144 | // Transform submap_i pcl and tf 145 | pcl::transformPointCloud(submap_set.at(i).submap_pcl_, submap_set.at(i).submap_pcl_, 146 | (estimate_i * submap_set.at(i).submap_tf_.inverse()).matrix()); 147 | 148 | submap_set.at(i).submap_tf_ = estimate_i.cast(); 149 | } 150 | } 151 | -------------------------------------------------------------------------------- /scripts/naviedit_export_to_std_data.py: -------------------------------------------------------------------------------- 1 | """ 2 | Converts NaviEdit exported .xyz and .nav files into std_data 3 | """ 4 | import logging 5 | import os 6 | import sys 7 | 8 | import numpy as np 9 | import pandas as pd 10 | 11 | from auvlib.data_tools import all_data, std_data 12 | 13 | 14 | def process_xyz_pings_file(filepath): 15 | """Read a .xyz file containing info about each beam hits and return a processed dataframe 16 | where each row contains info for 1 MBES swath""" 17 | logging.warning(f'\tProcessing xyz file: {filepath}...') 18 | naviedit_pings = pd.read_csv(filepath, delimiter='\t', header=0) 19 | 20 | # Aggregate xyz hits of each beam into a hits array 21 | naviedit_pings['hit'] = naviedit_pings[['X', 'Y', 22 | 'Z']].apply(lambda row: list(row), 23 | axis=1) 24 | 25 | # Parse string datetime values and produce a datetime object 26 | naviedit_pings['time_string'] = naviedit_pings[[ 27 | 'yyyy', 'mmddhhmm', 'ss.ss' 28 | ]].apply(lambda row: ''.join(row.values.astype(str)), axis=1) 29 | naviedit_pings['time_string'] = pd.to_datetime( 30 | naviedit_pings['time_string'], format='%Y.%m%d%H%M.0%S.%f') 31 | naviedit_pings['time_stamp'] = naviedit_pings['time_string'].map( 32 | pd.Timestamp.timestamp) 33 | 34 | # Group the beams by Ping No -> aggregate hits from the same ping into a list 35 | beams = naviedit_pings.groupby('Ping No').agg({'hit': list}) 36 | 37 | # Get unique pings 38 | ping_info = naviedit_pings[[ 39 | 'time_string', 'time_stamp', 'Ping No', 'Tide', 'Heading', 'Heave', 40 | 'Pitch', 'Roll' 41 | ]].drop_duplicates().set_index('Ping No') 42 | 43 | # Final results with 1 row = 1 MBES swath 44 | mbes_ping_result = ping_info.join(beams) 45 | return mbes_ping_result 46 | 47 | 48 | def process_nav_file(filepath): 49 | """Read a .nav file and return a processed dataframe where each row = 1 positional info with timestamps.""" 50 | logging.warning(f'\tProcessing nav file: {filepath}...') 51 | naviedit_nav = pd.read_csv(filepath, header=0) 52 | 53 | # Parse string datetime values and produce a datetime object 54 | naviedit_nav['time_string'] = naviedit_nav[['DATE', 'TIME']].apply( 55 | lambda row: ''.join(row.values.astype(str)), axis=1) 56 | naviedit_nav['time_string'] = pd.to_datetime(naviedit_nav['time_string'], 57 | format='%Y%m%d%H:%M:%S.%f') 58 | naviedit_nav['time_stamp'] = naviedit_nav['time_string'].map( 59 | pd.Timestamp.timestamp) 60 | naviedit_nav.rename(columns={ 61 | 'EASTING': 'easting', 62 | 'NORTHING': 'northing', 63 | 'DEPTH': 'depth' 64 | }, 65 | inplace=True) 66 | 67 | return naviedit_nav 68 | 69 | 70 | def merge_pings_and_nav_df(pings_df, nav_df): 71 | """Merge the processed pings and navigation dataframes based on their timestamps. Return a merged dataframe 72 | where each row contains info about 1 MBES swath and the corresponding vehicle position in ENU coordinates.""" 73 | logging.warning('\tMerging pings and nav df...') 74 | merged = pings_df.copy() 75 | merged[['easting', 'northing']] = 0 76 | for row_idx, row in merged.iterrows(): 77 | ping_time = row['time_stamp'] 78 | nav_row_idx = nav_df['time_stamp'].le(ping_time).idxmin() 79 | nav_row = nav_df.iloc[nav_row_idx] 80 | nav_row_prev = nav_df.iloc[nav_row_idx - 1] 81 | 82 | ratio = (ping_time - nav_row_prev['time_stamp']) / ( 83 | nav_row['time_stamp'] - nav_row_prev['time_stamp']) 84 | east = nav_row_prev['easting'] + ratio * (nav_row['easting'] - 85 | nav_row_prev['easting']) 86 | north = nav_row_prev['northing'] + ratio * (nav_row['northing'] - 87 | nav_row_prev['northing']) 88 | depth = nav_row_prev['depth'] + ratio * (nav_row['depth'] - 89 | nav_row_prev['depth']) 90 | 91 | merged.loc[row_idx, ['easting', 'northing', 'depth']] = nav_row[[ 92 | 'easting', 'northing', 'depth' 93 | ]] 94 | return merged 95 | 96 | 97 | def construct_std_data_from_merged_df(merged_df): 98 | """Translate the merged_df dataframe into a list of std_data objects""" 99 | logging.warning('\tConstructing std_data from merged df...') 100 | merged_data = [] 101 | for row_idx, row in merged_df.iterrows(): 102 | row_data = std_data.mbes_ping() 103 | 104 | if row_idx == merged_df.index.min(): 105 | row_data.first_in_file_ = True 106 | 107 | row_data.id_ = row_idx 108 | row_data.beams = [np.array(x) for x in row['hit']] 109 | row_data.heading_ = row['Heading'] 110 | row_data.heave_ = row['Heave'] 111 | row_data.pitch_ = row['Pitch'] 112 | row_data.pos_ = np.array( 113 | [row['easting'], row['northing'], row['depth']]) 114 | row_data.roll_ = row['Roll'] 115 | row_data.time_stamp_ = int(row['time_stamp']) 116 | row_data.time_string_ = row['time_string'].strftime( 117 | '%Y-%m-%d %H:%M:%S.%f') 118 | 119 | merged_data.append(row_data) 120 | return merged_data 121 | 122 | 123 | def construct_std_data_from_naviedit_export(folder, store=True): 124 | """Given a folder with .xyz and .nav exports from NaviEdit, extract relevant fields 125 | and construct std_data from these files, optionally store the std_data in .cereal 126 | format to disk.""" 127 | filenames = sorted( 128 | set( 129 | x.split('.')[0] for x in os.listdir(folder) 130 | if x.split('.')[-1] in ['xyz', 'nav'])) 131 | for filename in filenames: 132 | if not os.path.exists(os.path.join( 133 | folder, f'{filename}.xyz')) or not os.path.exists( 134 | os.path.join(folder, f'{filename}.nav')): 135 | raise IOError( 136 | f'{filename}.xyz or {filename}.nav missing. Aborting...') 137 | 138 | std_data_dict = {} 139 | for i, filename in enumerate(filenames): 140 | logging.warning(f'Processing {i}/{len(filenames)} file {filename}...') 141 | pings_df = process_xyz_pings_file( 142 | os.path.join(folder, f'{filename}.xyz')) 143 | nav_df = process_nav_file(os.path.join(folder, f'{filename}.nav')) 144 | merged_df = merge_pings_and_nav_df(pings_df, nav_df) 145 | merged_std_data = construct_std_data_from_merged_df(merged_df) 146 | 147 | std_data_dict[filename] = merged_std_data 148 | outpath = os.path.join(folder, f'{filename}.cereal') 149 | if store: 150 | std_data.write_data(merged_std_data, outpath) 151 | return std_data_dict 152 | 153 | 154 | def main(): 155 | folder = sys.argv[1] 156 | 157 | # Set up logger 158 | logger = logging.getLogger(name='data processor logger') 159 | fhandler = logging.FileHandler(filename=os.path.join( 160 | folder, 'data_processing.log'), 161 | mode='a') 162 | fhandler.setLevel(logging.INFO) 163 | logger.addHandler(fhandler) 164 | 165 | # Construct std_data from the .xyz and .nav files from the given folder 166 | data_dict = construct_std_data_from_naviedit_export(folder=folder) 167 | 168 | melt_data_combined = [] 169 | for section in data_dict.values(): 170 | melt_data_combined.extend(section) 171 | std_data.write_data(melt_data_combined, 172 | os.path.join(folder, 'merged.cereal')) 173 | 174 | 175 | if __name__ == '__main__': 176 | main() 177 | -------------------------------------------------------------------------------- /src/registration/src/utils_registration.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "registration/utils_registration.hpp" 13 | 14 | Eigen::Affine3d create_rotation_matrix(double ax, double ay, double az) { 15 | Eigen::Affine3d rx = Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0))); 16 | Eigen::Affine3d ry = Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0))); 17 | Eigen::Affine3d rz = Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1))); 18 | return rz * ry * rx; 19 | } 20 | 21 | 22 | Eigen::Matrix4f inverseTfMatrix(Eigen::Matrix4f tf_mat){ 23 | 24 | Eigen::Matrix3f R_inv = tf_mat.topLeftCorner(3,3).transpose(); 25 | 26 | Eigen::Matrix4f tf_mat_inv = Eigen::Matrix4f::Identity(); 27 | tf_mat_inv.topLeftCorner(3,3) = R_inv; 28 | tf_mat_inv.topRightCorner(3,1) = R_inv * (-tf_mat.topRightCorner(3,1)); 29 | 30 | return tf_mat_inv; 31 | 32 | } 33 | 34 | 35 | void plotSubmapsSet(const std::vector>& submaps_set){ 36 | // Plot submaps 37 | pcl::visualization::PCLVisualizer viewer ("ICP demo"); 38 | viewer.setSize (1920, 1080); // Visualiser window size 39 | 40 | PointCloudT::Ptr submap_i_ptr; 41 | int cnt = 0; 42 | for(const submap& submap_i: submaps_set){ 43 | // if(submap_i.submap_id == 0 || submap_i.submap_id == 24){ 44 | submap_i_ptr.reset(new PointCloudT(submap_i.submap_pcl)); 45 | pcl::transformPointCloud(*submap_i_ptr, *submap_i_ptr, inverseTfMatrix(submap_i.tf_submap_map)); 46 | viewer.addPointCloud (submap_i_ptr, "cloud_" + std::to_string(cnt), 0); 47 | // } 48 | ++cnt; 49 | } 50 | 51 | while (!viewer.wasStopped()) { 52 | viewer.spinOnce (); 53 | } 54 | } 55 | 56 | 57 | void print4x4Matrix (const Eigen::Matrix4f & matrix) { 58 | printf ("Rotation matrix :\n"); 59 | printf (" | %6.3f %6.3f %6.3f | \n", matrix (0, 0), matrix (0, 1), matrix (0, 2)); 60 | printf ("R = | %6.3f %6.3f %6.3f | \n", matrix (1, 0), matrix (1, 1), matrix (1, 2)); 61 | printf (" | %6.3f %6.3f %6.3f | \n", matrix (2, 0), matrix (2, 1), matrix (2, 2)); 62 | printf ("Translation vector :\n"); 63 | printf ("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix (0, 3), matrix (1, 3), matrix (2, 3)); 64 | } 65 | 66 | Eigen::Vector3f computePCAPcl(PointCloudT& set_Ai){ 67 | 68 | // Compute the mean of the PCL 69 | Eigen::Vector4f com_Ai; 70 | pcl::compute3DCentroid(set_Ai, com_Ai); 71 | 72 | // Compute covariance matrix 73 | Eigen::Matrix3f cov_mat; 74 | pcl::computeCovarianceMatrixNormalized (set_Ai, com_Ai, cov_mat); 75 | // pcl::computeCovarianceMatrix(set_Ai_demean, cov_mat); 76 | 77 | // Extract eigenvalues and eigenvector from cov matrix 78 | Eigen::EigenSolver eigenSolver; 79 | eigenSolver.compute(cov_mat, true); 80 | 81 | Eigen::MatrixXf eigenVectors = eigenSolver.eigenvectors().real(); 82 | Eigen::VectorXf eigenvalues = eigenSolver.eigenvalues().real(); 83 | 84 | // Return eigenvector with smallest eigenvalue 85 | std::vector> pidx; 86 | for (unsigned int i = 0 ; i= average_dist*1.5){ 136 | submap_aux.submap_pcl.push_back(pointj); 137 | submap_aux.pcl_covariances.push_back(submap_input.pcl_covariances.at(cnt)); 138 | } 139 | ++cnt; 140 | } 141 | 142 | // Clear and store final output values 143 | submap_input.submap_pcl.clear(); 144 | submap_input.pcl_covariances.clear(); 145 | 146 | submap_input.submap_pcl = submap_aux.submap_pcl; 147 | submap_input.pcl_covariances = submap_aux.pcl_covariances; 148 | } 149 | 150 | 151 | void outlierFilter(submap &submap_input){ 152 | 153 | // Build Kdtree 154 | pcl::KdTreeFLANN kdtree; 155 | PointCloudT::Ptr pcl_ptr; 156 | pcl_ptr.reset(new PointCloudT(submap_input.submap_pcl)); 157 | kdtree.setInputCloud(pcl_ptr); 158 | 159 | // Average distance between NN points in pcl 160 | int average_nn = 0; 161 | int K = 4; 162 | std::vector pointIdxRadiusSearch(K); 163 | std::vector pointRadiusSquaredDistance(K); 164 | 165 | double radius = 1; 166 | for(PointT point_i: submap_input.submap_pcl.points){ 167 | average_nn += kdtree.radiusSearch (point_i, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance); 168 | } 169 | average_nn = average_nn / submap_input.submap_pcl.points.size(); 170 | std::cout << "Average number of nn: " << average_nn << std::endl; 171 | 172 | // Filter out points farther from any other than average dist 173 | submap submap_aux; 174 | for(PointT point_i: submap_input.submap_pcl.points){ 175 | if(kdtree.radiusSearch (point_i, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) >= average_nn*0.2){ 176 | submap_aux.submap_pcl.push_back(point_i); 177 | } 178 | } 179 | 180 | submap_input.submap_pcl.clear(); 181 | submap_input.submap_pcl = submap_aux.submap_pcl; 182 | } 183 | 184 | -------------------------------------------------------------------------------- /src/meas_models/include/meas_models/multibeam_simple.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MULTIBEAM_SIMPLE_HPP 2 | #define MULTIBEAM_SIMPLE_HPP 3 | 4 | #include 5 | #include 6 | #include "submaps_tools/submaps.hpp" 7 | 8 | template 9 | class MultibeamSensor: public pcl::VoxelGrid 10 | { 11 | protected: 12 | 13 | using pcl::VoxelGrid::min_b_; 14 | using pcl::VoxelGrid::max_b_; 15 | using pcl::VoxelGrid::div_b_; 16 | using pcl::VoxelGrid::leaf_size_; 17 | using pcl::VoxelGrid::inverse_leaf_size_; 18 | 19 | using PointCloudFilt = typename pcl::Filter::PointCloud; 20 | using PointCloudFiltPtr = typename PointCloudFilt::Ptr; 21 | using PointCloudFiltConstPtr = typename PointCloudFilt::ConstPtr; 22 | 23 | public: 24 | /** \brief Empty constructor. */ 25 | MultibeamSensor () 26 | { 27 | initialized_ = false; 28 | this->setSaveLeafLayout (true); 29 | } 30 | 31 | /** \brief Destructor. */ 32 | ~MultibeamSensor () 33 | { 34 | } 35 | 36 | inline PointCloudFilt 37 | getFilteredPointCloud () { return filtered_cloud_; } 38 | 39 | 40 | /** \brief Returns the minimum bounding of coordinates of the voxel grid (x,y,z). 41 | * \return the minimum coordinates (x,y,z) 42 | */ 43 | inline Eigen::Vector3f 44 | getMinBoundCoordinates () { return (b_min_.head<3> ()); } 45 | 46 | /** \brief Returns the maximum bounding of coordinates of the voxel grid (x,y,z). 47 | * \return the maximum coordinates (x,y,z) 48 | */ 49 | inline Eigen::Vector3f 50 | getMaxBoundCoordinates () { return (b_max_.head<3> ()); } 51 | 52 | /** \brief Returns the corresponding centroid (x,y,z) coordinates 53 | * in the grid of voxel (i,j,k). 54 | * \param[in] ijk the coordinate (i, j, k) of the voxel 55 | * \return the (x,y,z) coordinate of the voxel centroid 56 | */ 57 | inline Eigen::Vector4f 58 | getCentroidCoordinate (const Eigen::Vector3i& ijk) 59 | { 60 | int i,j,k; 61 | i = ((b_min_[0] < 0) ? (std::abs (min_b_[0]) + ijk[0]) : (ijk[0] - min_b_[0])); 62 | j = ((b_min_[1] < 0) ? (std::abs (min_b_[1]) + ijk[1]) : (ijk[1] - min_b_[1])); 63 | k = ((b_min_[2] < 0) ? (std::abs (min_b_[2]) + ijk[2]) : (ijk[2] - min_b_[2])); 64 | 65 | Eigen::Vector4f xyz; 66 | xyz[0] = b_min_[0] + (leaf_size_[0] * 0.5f) + (static_cast (i) * leaf_size_[0]); 67 | xyz[1] = b_min_[1] + (leaf_size_[1] * 0.5f) + (static_cast (j) * leaf_size_[1]); 68 | xyz[2] = b_min_[2] + (leaf_size_[2] * 0.5f) + (static_cast (k) * leaf_size_[2]); 69 | xyz[3] = 0; 70 | return xyz; 71 | } 72 | 73 | inline float 74 | round (float d) 75 | { 76 | return static_cast (std::floor (d + 0.5f)); 77 | } 78 | 79 | // We use round here instead of std::floor due to some numerical issues. 80 | /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). 81 | * \param[in] x the X point coordinate to get the (i, j, k) index at 82 | * \param[in] y the Y point coordinate to get the (i, j, k) index at 83 | * \param[in] z the Z point coordinate to get the (i, j, k) index at 84 | */ 85 | inline Eigen::Vector3i 86 | getGridCoordinatesRound (float x, float y, float z) 87 | { 88 | return Eigen::Vector3i (static_cast (round (x * inverse_leaf_size_[0])), 89 | static_cast (round (y * inverse_leaf_size_[1])), 90 | static_cast (round (z * inverse_leaf_size_[2]))); 91 | } 92 | 93 | // initialization flag 94 | bool initialized_; 95 | 96 | Eigen::Vector4f sensor_origin_; 97 | Eigen::Quaternionf sensor_orientation_; 98 | std::vector > beam_directions_; 99 | 100 | // minimum and maximum bounding box coordinates 101 | Eigen::Vector4f b_min_, b_max_; 102 | 103 | // voxel grid filtered cloud 104 | PointCloudFilt filtered_cloud_; 105 | 106 | void initializeVoxelGrid (SubmapObj submap_i){ 107 | std::cout << "Number of points " << submap_i.submap_pcl_.points.size() << std::endl; 108 | // initialization set to true 109 | initialized_ = true; 110 | // create the voxel grid and store the output cloud 111 | PointCloudT::Ptr cloud_ptr(new PointCloudT); 112 | *cloud_ptr = submap_i.submap_pcl_; 113 | this->setInputCloud(cloud_ptr); 114 | this->filter (filtered_cloud_); 115 | // PointCloudT filtered = filtered_cloud_; 116 | 117 | // Get the minimum and maximum bounding box dimensions 118 | b_min_[0] = (static_cast ( min_b_[0]) * leaf_size_[0]); 119 | b_min_[1] = (static_cast ( min_b_[1]) * leaf_size_[1]); 120 | b_min_[2] = (static_cast ( min_b_[2]) * leaf_size_[2]); 121 | b_max_[0] = (static_cast ( (max_b_[0]) + 1) * leaf_size_[0]); 122 | b_max_[1] = (static_cast ( (max_b_[1]) + 1) * leaf_size_[1]); 123 | b_max_[2] = (static_cast ( (max_b_[2]) + 1) * leaf_size_[2]); 124 | std::cout << "Minb " << min_b_.transpose() << std::endl; 125 | std::cout << "Maxb " << max_b_.transpose() << std::endl; 126 | } 127 | 128 | 129 | void createMBES(float spam, float n_beams, Eigen::Isometry3f& sensor_tf){ 130 | 131 | // set the sensor origin and sensor orientation 132 | sensor_origin_ << sensor_tf.translation(), 0.0; 133 | sensor_orientation_ = Eigen::Quaternionf(sensor_tf.linear()); 134 | 135 | 136 | float roll_step = spam/n_beams; 137 | float pitch = 0.0, yaw = 0.0; 138 | Eigen::Quaternionf q_180; 139 | q_180 = Eigen::AngleAxisf(3.1415, Eigen::Vector3f::UnitX()) 140 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 141 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 142 | 143 | sensor_orientation_ *= q_180; 144 | Eigen::Vector3f z_or = sensor_orientation_.toRotationMatrix().col(2); 145 | // std::cout << "Frame origin " << sensor_origin_.transpose() << std::endl; 146 | // std::cout << "Frame direction " << z_or.transpose() << std::endl; 147 | 148 | for(int i = -n_beams/2; i<=n_beams/2; i++){ 149 | Eigen::Quaternionf q = Eigen::AngleAxisf(roll_step*i, Eigen::Vector3f::UnitX()) 150 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 151 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 152 | beam_directions_.push_back(Eigen::Quaternionf(sensor_orientation_ * q)); 153 | } 154 | 155 | } 156 | 157 | int pingComputation (PointCloudT& ping_pcl){ 158 | if (!initialized_){ 159 | PCL_ERROR ("Voxel grid not initialized; call initializeVoxelGrid () first! \n"); 160 | return -1; 161 | } 162 | Eigen::Vector4f direction; 163 | 164 | // Check every beam 165 | for(Eigen::Quaternionf beam_n: beam_directions_){ 166 | Eigen::Matrix3f rot_beam = beam_n.toRotationMatrix(); 167 | direction << rot_beam.col(2), 0.0; 168 | direction.normalize (); 169 | // Estimate entry point into the voxel grid 170 | float tmin = rayBoxIntersection(sensor_origin_, direction); 171 | if (tmin != -1){ 172 | float z_max = 300; 173 | for(float z=0; zgetCentroidIndexAt (ijk); 182 | if (index != -1){ 183 | ping_pcl.points.push_back(PointT(voxel_max[0], voxel_max[1], voxel_max[2])); 184 | break; 185 | } 186 | } 187 | } 188 | } 189 | beam_directions_.clear(); 190 | return 0; 191 | 192 | } 193 | 194 | float rayBoxIntersection (const Eigen::Vector4f& origin, const Eigen::Vector4f& direction){ 195 | 196 | float tmin, tmax, tymin, tymax, tzmin, tzmax; 197 | if (direction[0] >= 0){ 198 | tmin = (b_min_[0] - origin[0]) / direction[0]; 199 | tmax = (b_max_[0] - origin[0]) / direction[0]; 200 | } 201 | else{ 202 | tmin = (b_max_[0] - origin[0]) / direction[0]; 203 | tmax = (b_min_[0] - origin[0]) / direction[0]; 204 | } 205 | 206 | 207 | if (direction[1] >= 0){ 208 | tymin = (b_min_[1] - origin[1]) / direction[1]; 209 | tymax = (b_max_[1] - origin[1]) / direction[1]; 210 | } 211 | else{ 212 | tymin = (b_max_[1] - origin[1]) / direction[1]; 213 | tymax = (b_min_[1] - origin[1]) / direction[1]; 214 | } 215 | 216 | if ((tmin > tymax) || (tymin > tmax)){ 217 | // PCL_ERROR ("no intersection with the bounding box \n"); 218 | tmin = -1.0f; 219 | return tmin; 220 | } 221 | 222 | if (tymin > tmin) 223 | tmin = tymin; 224 | if (tymax < tmax) 225 | tmax = tymax; 226 | 227 | if (direction[2] >= 0){ 228 | tzmin = (b_min_[2] - origin[2]) / direction[2]; 229 | tzmax = (b_max_[2] - origin[2]) / direction[2]; 230 | } 231 | else{ 232 | tzmin = (b_max_[2] - origin[2]) / direction[2]; 233 | tzmax = (b_min_[2] - origin[2]) / direction[2]; 234 | } 235 | 236 | if ((tmin > tzmax) || (tzmin > tmax)){ 237 | // PCL_ERROR ("no intersection with the bounding box \n"); 238 | tmin = -1.0f; 239 | return tmin; 240 | } 241 | 242 | if (tzmin > tmin) 243 | tmin = tzmin; 244 | if (tzmax < tmax) 245 | tmax = tzmax; 246 | 247 | return tmin; 248 | } 249 | }; 250 | 251 | #endif // MULTIBEAM_SIMPLE_HPP 252 | -------------------------------------------------------------------------------- /src/registration/src/gicp_reg.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "registration/gicp_reg.hpp" 13 | 14 | 15 | using namespace std; 16 | using namespace Eigen; 17 | using PointsT = std::vector>; 18 | 19 | 20 | 21 | SubmapRegistration::SubmapRegistration(YAML::Node config){ 22 | 23 | ret_tf_ = Eigen::Matrix4f::Identity(); 24 | benchmark_ = benchmark::track_error_benchmark(); 25 | 26 | // Constrain GICP to x,y, yaw 27 | pcl::registration::WarpPointRigid3D::Ptr warp_fcn 28 | (new pcl::registration::WarpPointRigid3D); 29 | 30 | pcl::registration::TransformationEstimationLM::Ptr te 31 | (new pcl::registration::TransformationEstimationLM); 32 | te->setWarpFunction (warp_fcn); 33 | gicp_.setTransformationEstimation(te); 34 | 35 | // GICP parameters 36 | loadConfig(config); 37 | } 38 | 39 | SubmapRegistration::~SubmapRegistration(){ 40 | 41 | } 42 | 43 | void SubmapRegistration::loadConfig(YAML::Node config){ 44 | gicp_.setMaxCorrespondenceDistance(config["gicp_max_correspondence_distance"].as()); 45 | gicp_.setMaximumIterations(config["gicp_maximum_iterations"].as()); 46 | gicp_.setTransformationEpsilon(config["gicp_transform_epsilon"].as()); 47 | gicp_.setEuclideanFitnessEpsilon(config["gicp_euclidean_fitness_epsilon"].as()); 48 | gicp_.setRANSACOutlierRejectionThreshold(config["gicp_ransac_outlier_rejection_threshold"].as()); 49 | normal_use_knn_search = config["gicp_normal_use_knn_search"].as(); 50 | normal_search_radius = config["gicp_normal_search_radius"].as(); 51 | normal_search_k_neighbours = config["gicp_normal_search_k_neighbours"].as(); 52 | info_diag_values = config["gicp_info_diag"].as>(); 53 | } 54 | 55 | SubmapObj SubmapRegistration::constructTrgSubmap(const SubmapsVec& submaps_set, std::vector& overlaps, const DRNoise& dr_noise){ 56 | 57 | // Merge submaps in overlaps into submap_trg 58 | SubmapObj submap_trg(dr_noise); 59 | std::cout << "Target submap consists of: "; 60 | for(SubmapObj submap_j: submaps_set){ 61 | if(std::find(overlaps.begin(), overlaps.end(), submap_j.submap_id_) != overlaps.end()){ 62 | std::cout << submap_j.submap_id_ << ", "; 63 | submap_trg.submap_pcl_ += submap_j.submap_pcl_; 64 | } 65 | } 66 | 67 | return submap_trg; 68 | } 69 | 70 | void SubmapRegistration::transformSubmap(SubmapObj& submap){ 71 | 72 | // Apply tranform to submap frame 73 | Isometry3f rel_tf = Isometry3f (Isometry3f(Translation3f(ret_tf_.block<3,1>(0,3)))* 74 | Isometry3f(Quaternionf(ret_tf_.block<3,3>(0,0)).normalized())); 75 | 76 | submap.submap_tf_ = rel_tf * submap.submap_tf_; 77 | } 78 | 79 | double SubmapRegistration::consistencyErrorOverlap(const SubmapObj& trg_submap, 80 | const SubmapObj& src_submap){ 81 | 82 | // Compute consistency error in overlapped area 83 | PointsT submaps; 84 | submaps.push_back(trg_submap.submap_pcl_.getMatrixXfMap(3,4,0).transpose().cast()); 85 | submaps.push_back(src_submap.submap_pcl_.getMatrixXfMap(3,4,0).transpose().cast()); 86 | 87 | Eigen::MatrixXd error_vals; 88 | double consistency_rms_error; 89 | std::vector>> grid_maps = benchmark_.create_grids_from_matrices(submaps); 90 | tie(consistency_rms_error, error_vals) = benchmark_.compute_consistency_error(grid_maps); 91 | 92 | return (consistency_rms_error); 93 | 94 | } 95 | 96 | 97 | bool SubmapRegistration::gicpSubmapRegistration(SubmapObj& trg_submap, SubmapObj& src_submap){ 98 | //N.B. this function modifies the src_submap when aligning it to 99 | 100 | // Copy the originals to work over them 101 | PointCloudT::Ptr src_pcl_ptr (new PointCloudT(src_submap.submap_pcl_)); 102 | PointCloudT::Ptr trg_pcl_ptr (new PointCloudT(trg_submap.submap_pcl_)); 103 | 104 | // Compute GICP covs externally 105 | pcl::NormalEstimation ne; 106 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree ()); 107 | ne.setSearchMethod(tree); 108 | if (normal_use_knn_search) { 109 | std::cout << "Using KNN search with K neighbours: " << normal_search_k_neighbours << std::endl; 110 | ne.setKSearch(normal_search_k_neighbours); 111 | } else { 112 | std::cout << "Use radius search with radius: " << normal_search_radius << std::endl; 113 | ne.setRadiusSearch(normal_search_radius); 114 | } 115 | 116 | pcl::PointCloud::Ptr normals_src(new pcl::PointCloud); 117 | ne.setInputCloud(src_pcl_ptr); 118 | ne.compute(*normals_src); 119 | 120 | pcl::PointCloud::Ptr normals_trg(new pcl::PointCloud); 121 | ne.setInputCloud(trg_pcl_ptr); 122 | ne.compute(*normals_trg); 123 | 124 | CovsVec covs_src; 125 | CovsVecPtr covs_src_ptr; 126 | pcl::features::computeApproximateCovariances(*src_pcl_ptr, *normals_src, covs_src); 127 | covs_src_ptr.reset(new CovsVec(covs_src)); 128 | 129 | CovsVec covs_trg; 130 | CovsVecPtr covs_trg_ptr; 131 | pcl::features::computeApproximateCovariances(*trg_pcl_ptr, *normals_trg, covs_trg); 132 | covs_trg_ptr.reset(new CovsVec(covs_trg)); 133 | 134 | // Compute GICP vanilla information matrix 135 | Eigen::Matrix3d avg_cov_src; 136 | avg_cov_src.setZero(); 137 | for(unsigned int n=0; nsize(); n++){ 138 | avg_cov_src += covs_src_ptr->at(n); 139 | } 140 | avg_cov_src /= (covs_src_ptr->size()); 141 | 142 | Eigen::Matrix3d avg_cov_trg; 143 | avg_cov_trg.setZero(); 144 | for(unsigned int n=0; nsize(); n++){ 145 | avg_cov_trg += covs_trg_ptr->at(n); 146 | } 147 | avg_cov_trg /= (covs_trg_ptr->size()); 148 | 149 | // The Iterative Closest Point algorithm 150 | gicp_.setInputSource(src_pcl_ptr); 151 | gicp_.setInputTarget(trg_pcl_ptr); 152 | gicp_.setSourceCovariances(covs_src_ptr); 153 | gicp_.setTargetCovariances(covs_trg_ptr); 154 | gicp_.align (src_submap.submap_pcl_); 155 | 156 | // Apply transform to submap 157 | ret_tf_ = gicp_.getFinalTransformation(); 158 | this->transformSubmap(src_submap); 159 | 160 | std::cout << "average cov src: " << avg_cov_src << std::endl; 161 | std::cout << "average cov target: " << avg_cov_trg << std::endl; 162 | 163 | std::cout << "ret_tf: " << ret_tf_ << std::endl; 164 | 165 | // Set GICP information matrix 166 | src_submap.submap_lc_info_.setZero(); 167 | Eigen::VectorXd info_diag = Eigen::Map(info_diag_values.data()); 168 | std::cout << "GICP info diag: " << info_diag << std::endl; 169 | src_submap.submap_lc_info_.bottomRightCorner(4,4) = info_diag.asDiagonal(); 170 | 171 | Eigen::Matrix3d rot = ret_tf_.topLeftCorner(3,3).cast(); 172 | std::cout << "rot: " << rot << std::endl; 173 | std::cout << "rot.T: " << rot.transpose() << std::endl; 174 | 175 | Eigen::Matrix3d gicp_cov = avg_cov_trg + rot*avg_cov_src*rot.transpose(); 176 | std::cout << "gicp_cov: " << gicp_cov << std::endl; 177 | src_submap.submap_lc_info_.topLeftCorner(2,2) = gicp_cov.topLeftCorner(2,2).inverse(); 178 | // std::cout << gicp_cov.topLeftCorner(3,3) << std::endl; 179 | 180 | for(int x=0; xtransformSubmap(src_submap); 207 | 208 | // Check for nan values 209 | for(int x=0; x > covs_lc): 19 | covs_lc_(covs_lc){ 20 | edge_covs_type_ = 2; 21 | } 22 | 23 | GraphConstructor::~GraphConstructor(){ 24 | 25 | } 26 | 27 | void GraphConstructor::createNewVertex(SubmapObj& submap){ 28 | 29 | VertexSE3* v = new VertexSE3; 30 | v->setId(submap.submap_id_); 31 | Eigen::Isometry3d t = submap.submap_tf_.cast(); 32 | v->setEstimate(t); 33 | vertices_.push_back(v); 34 | } 35 | 36 | 37 | void GraphConstructor::createDREdge(const SubmapObj& submap){ 38 | 39 | // Generate dead reckoning edges 40 | VertexSE3* prev = vertices_[submap.submap_id_-1]; 41 | VertexSE3* cur = vertices_[submap.submap_id_]; 42 | Eigen::Isometry3d t = prev->estimate().inverse() * cur->estimate(); 43 | EdgeSE3* e = new EdgeSE3; 44 | e->setVertex(0, prev); 45 | e->setVertex(1, cur); 46 | e->setMeasurement(t); 47 | e->setInformation(submap.submap_info_); 48 | 49 | drEdges_.push_back(e); 50 | drMeas_.push_back(t); 51 | } 52 | 53 | 54 | // Important: select method for computing edge's weight 55 | void GraphConstructor::createLCEdge(const SubmapObj& submap_from, const SubmapObj& submap_to){ 56 | 57 | std::cout << "LC edge from " << submap_from.submap_id_ << " to " << submap_to.submap_id_ << std::endl; 58 | 59 | // Generate loop closure edges 60 | VertexSE3* from = vertices_[submap_from.submap_id_]; 61 | VertexSE3* to = vertices_[submap_to.submap_id_]; 62 | //Eigen::Isometry3d t = from->estimate().inverse() * submap_to.submap_tf_.cast(); 63 | Eigen::Isometry3d t = submap_from.submap_tf_.cast().inverse() * submap_to.submap_tf_.cast(); 64 | EdgeSE3* e = new EdgeSE3; 65 | e->setVertex(0, from); 66 | e->setVertex(1, to); 67 | e->setMeasurement(t); 68 | 69 | // Information matrix for LC edges 70 | Eigen::Array3f info = computeInfoInSubmap(submap_from); 71 | 72 | // Submap covariance matrix 73 | Eigen::Matrix3f cov_matrix; 74 | Eigen::Vector4f xyz_centroid; 75 | pcl::compute3DCentroid(submap_from.submap_pcl_, xyz_centroid); 76 | pcl::computeCovarianceMatrix(submap_from.submap_pcl_, xyz_centroid, cov_matrix); 77 | 78 | // Info matrix proportional to variance in Z in the pointcloud 79 | Eigen::Matrix information = Eigen::Matrix::Zero(); 80 | Eigen::VectorXd info_diag(4), info_diag_trans(2); 81 | info_diag << 10000.0, 10000.0, 10000.0, 1000.0; 82 | information.bottomRightCorner(4,4) = info_diag.asDiagonal(); 83 | Eigen::Matrix2d cov_reg; 84 | 85 | switch (edge_covs_type_) { 86 | // Fixed external val 87 | case 0: 88 | // std::cout << "Fixed covs " << std::endl; 89 | info_diag_trans << covs_lc_.at(0)(0,0), covs_lc_.at(0)(0,0); 90 | information.topLeftCorner(2,2) = info_diag_trans.asDiagonal(); 91 | break; 92 | // Manual vals 93 | case 1: 94 | // std::cout << "Avg covs " << std::endl; 95 | info_diag << 10000.0, 10000.0, 10000.0, 1000.0; 96 | information.block<2,2>(0,0) << 5.382, -0.486, -0.486, 8.057; //Borno 97 | // information.block<2,2>(0,0) << 3.348, -3.127, -3.127, 5.445; // Antarctica 98 | break; 99 | // Covs from GICP 100 | case 2: 101 | // std::cout << "GICP covs " << std::endl; 102 | information = submap_from.submap_lc_info_; 103 | break; 104 | // Covs from external file 105 | case 3: 106 | // std::cout << "External covs " << std::endl; 107 | cov_reg = covs_lc_.at(submap_from.submap_id_); 108 | info_diag << 10000.0, 10000.0, 10000.0, 1000.0; 109 | information.topLeftCorner(2,2) = cov_reg.inverse(); 110 | break; 111 | default: 112 | info_diag_trans << 1.0, 1.0; 113 | information.topLeftCorner(2,2) = info_diag_trans.asDiagonal(); 114 | break; 115 | } 116 | 117 | // std::cout << information << std::endl; 118 | e->setInformation(information); 119 | 120 | // Check resulting COV is positive semi-definite 121 | Eigen::LLT lltOfA(information); 122 | if(lltOfA.info() != Eigen::NumericalIssue){ 123 | lcEdges_.push_back(e); 124 | lcMeas_.push_back(t); 125 | } 126 | } 127 | 128 | 129 | void GraphConstructor::findLoopClosures(SubmapObj& submap_i, const SubmapsVec& submaps_set, 130 | double info_thres){ 131 | 132 | // Check all submaps in overlaps vector 133 | for(SubmapObj submap_j: submaps_set){ 134 | if(find(submap_i.overlaps_idx_.begin(), submap_i.overlaps_idx_.end(), submap_j.submap_id_) 135 | != submap_i.overlaps_idx_.end()){ 136 | this->createLCEdge(submap_i, submap_j); 137 | } 138 | } 139 | } 140 | 141 | 142 | void GraphConstructor::createInitialEstimate(SubmapsVec& submaps_set){ 143 | 144 | // Concatenate all the odometry constraints to compute the initial kinematic chain 145 | for (size_t i =0; i < drEdges_.size(); i++) { 146 | Eigen::Isometry3d meas_i = drMeas_.at(i); 147 | EdgeSE3* e = drEdges_[i]; 148 | VertexSE3* from = static_cast(e->vertex(0)); 149 | VertexSE3* to = static_cast(e->vertex(1)); 150 | 151 | // Transform submap_i pcl and tf 152 | Eigen::Isometry3d estimate_i = from->estimate() * meas_i; 153 | pcl::transformPointCloud(submaps_set.at(i+1).submap_pcl_, submaps_set.at(i+1).submap_pcl_, 154 | (estimate_i.cast() * submaps_set.at(i+1).submap_tf_.cast().inverse()).matrix()); 155 | 156 | submaps_set.at(i+1).submap_tf_ = estimate_i.cast(); 157 | to->setEstimate(estimate_i); 158 | 159 | drChain_.push_back(estimate_i); 160 | } 161 | } 162 | 163 | /// Not tested yet! 164 | void GraphConstructor::addNoiseToGraph(GaussianGen& transSampler, GaussianGen& rotSampler){ 165 | 166 | std::random_device rd{}; 167 | std::mt19937 gen{rd()}; 168 | std::normal_distribution<> d{0,0.01}; 169 | 170 | // Noise for all the DR edges 171 | for (size_t i = 0; i < drEdges_.size(); ++i) { 172 | Eigen::Isometry3d meas_i = drMeas_.at(i); 173 | Eigen::Quaterniond gtQuat = (Eigen::Quaterniond)meas_i.linear(); 174 | Eigen::Vector3d gtTrans = meas_i.translation(); 175 | 176 | // Bias in yaw 177 | double roll = 0.0, pitch = 0.0, yaw = /*0.001*/ d(gen); 178 | Matrix3d m; 179 | m = AngleAxisd(roll, Vector3d::UnitX()) 180 | * AngleAxisd(pitch, Vector3d::UnitY()) 181 | * AngleAxisd(yaw, Vector3d::UnitZ()); 182 | 183 | Eigen::Vector3d quatXYZ = rotSampler.generateSample(); 184 | double qw = 1.0 - quatXYZ.norm(); 185 | if (qw < 0) { 186 | qw = 0.; 187 | cerr << "x"; 188 | } 189 | // Eigen::Quaterniond rot(qw, quatXYZ.x(), quatXYZ.y(), quatXYZ.z()); 190 | Eigen::Quaterniond rot(m); 191 | 192 | Eigen::Vector3d trans; 193 | // trans = transSampler.generateSample(); 194 | trans.setZero(); 195 | 196 | rot = gtQuat * rot; 197 | trans = gtTrans + trans; 198 | 199 | Eigen::Isometry3d noisyMeasurement = (Eigen::Isometry3d) rot; 200 | noisyMeasurement.translation() = trans; 201 | drMeas_.at(i) = noisyMeasurement; 202 | } 203 | } 204 | 205 | void GraphConstructor::saveG2OFile(std::string outFilename){ 206 | 207 | // Save graph to output file 208 | ofstream fileOutputStream; 209 | if (outFilename != "-") { 210 | cerr << "Writing into " << outFilename << endl; 211 | fileOutputStream.open(outFilename.c_str()); 212 | } else { 213 | cerr << "writing to stdout" << endl; 214 | } 215 | ostream& fout = outFilename != "-" ? fileOutputStream : std::cout; 216 | 217 | // Concatenate DR and LC edges (DR go first, according to g2o convention) 218 | vector edges; 219 | edges.insert(edges.begin(), lcEdges_.begin(), lcEdges_.end()); 220 | edges.insert(edges.begin(), drEdges_.begin(), drEdges_.end()); 221 | tf_vec meas; 222 | meas.insert(meas.begin(), lcMeas_.begin(), lcMeas_.end()); 223 | meas.insert(meas.begin(), drMeas_.begin(), drMeas_.end()); 224 | 225 | string vertexTag = Factory::instance()->tag(vertices_[0]); 226 | string edgeTag = Factory::instance()->tag(edges[0]); 227 | 228 | for (size_t i = 0; i < vertices_.size(); ++i) { 229 | VertexSE3* v = vertices_[i]; 230 | fout << vertexTag << " " << v->id() << " "; 231 | v->write(fout); 232 | fout << endl; 233 | } 234 | 235 | // Output 236 | for (size_t i = 0; i < edges.size(); ++i) { 237 | EdgeSE3* e = edges[i]; 238 | VertexSE3* from = static_cast(e->vertex(0)); 239 | VertexSE3* to = static_cast(e->vertex(1)); 240 | fout << edgeTag << " " << from->id() << " " << to->id() << " "; 241 | // Vector7 meas=g2o::internal::toVectorQT(e->measurement()); 242 | Vector7 meas_i=g2o::internal::toVectorQT(meas.at(i)); 243 | for (int i=0; i<7; i++) fout << meas_i[i] << " "; 244 | for (int i=0; iinformation().rows(); i++){ 245 | for (int j=i; jinformation().cols(); j++) { 246 | fout << e->information()(i,j) << " "; 247 | } 248 | } 249 | fout << endl; 250 | } 251 | } 252 | 253 | -------------------------------------------------------------------------------- /src/registration/src/utils_visualization.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "registration/utils_visualization.hpp" 13 | #include /* srand, rand */ 14 | #include /* time */ 15 | 16 | 17 | using namespace std; 18 | using pcl::visualization::PointCloudColorHandlerCustom; 19 | 20 | SubmapsVisualizer::SubmapsVisualizer(pcl::visualization::PCLVisualizer &viewer): 21 | viewer_(viewer){ 22 | 23 | } 24 | 25 | 26 | void SubmapsVisualizer::setVisualizer(SubmapsVec& submaps_set, int num){ 27 | 28 | // The color we will be using 29 | float black = 0.0; // Black 30 | float white = 1.0 - black; 31 | 32 | // Create one/two vertically separated viewports 33 | if (num == 2){ 34 | // Second viewport 35 | viewer_.createViewPort (0.0, 0.0, 0.5, 1.0, vp1_); 36 | viewer_.createViewPort (0.5, 0.0, 1.0, 1.0, vp2_); 37 | viewer_.addText ("Results", 10, 15, 16, white, white, white, "info_2", vp2_); 38 | viewer_.setBackgroundColor (white, white, white, vp2_); 39 | viewer_.addCoordinateSystem(5.0, "reference_frame", vp2_); 40 | } 41 | else{ 42 | viewer_.createViewPort (0.0, 0.0, 1.0, 1.0, vp1_); 43 | } 44 | 45 | 46 | // Adding text descriptions in each viewport 47 | viewer_.addText ("Ground truth", 10, 15, 16, white, white, white, "info_1", vp1_); 48 | 49 | // Set GT pointclouds 50 | unsigned int i = 0; 51 | PointCloudT::Ptr submap_ptr (new PointCloudT); 52 | for(SubmapObj& submap: submaps_set){ 53 | submap_ptr.reset(new PointCloudT(submap.submap_pcl_)); 54 | submap.colors_ = Vector3d(rand() % 256, rand() % 256, rand() % 256); 55 | PointCloudColorHandlerCustom cloud_color(submap_ptr, submap.colors_[0], submap.colors_[1], submap.colors_[2]); 56 | viewer_.addPointCloud(submap_ptr, cloud_color, "gt_cloud_" + std::to_string(i), vp1_); 57 | viewer_.addCoordinateSystem(3.0, submap.submap_tf_, "gt_cloud_" + std::to_string(i), vp1_); 58 | i++; 59 | } 60 | 61 | // Set GT trajectory (linearized) 62 | Vector3i dr_color = Vector3i(rand() % 256, rand() % 256, rand() % 256); 63 | for(unsigned int j=0; j(); 66 | SubmapObj submap_to = submaps_set.at(j); 67 | Eigen::Vector3d to_ps = submap_to.submap_tf_.translation().cast(); 68 | viewer_.addArrow(PointT(from_ps[0],from_ps[1],from_ps[2]), PointT(to_ps[0],to_ps[1],to_ps[2]), 69 | dr_color[0], dr_color[1], dr_color[2], false, "gt_dr_edge_" + std::to_string(j), vp1_); 70 | } 71 | 72 | // Set background color 73 | viewer_.setBackgroundColor (white, white, white, vp1_); 74 | 75 | // Set camera position and orientation 76 | viewer_.setSize (1920/2, 1080/2); 77 | viewer_.addCoordinateSystem(5.0, "gt_reference_frame", vp1_); 78 | } 79 | 80 | 81 | void SubmapsVisualizer::updateVisualizer(const SubmapsVec& submaps_set){ 82 | 83 | viewer_.removeAllPointClouds(vp1_); 84 | viewer_.removeAllCoordinateSystems(vp1_); 85 | viewer_.addCoordinateSystem(5.0, "reference_frame", vp1_); 86 | 87 | // Update pointclouds 88 | unsigned int i = 0; 89 | PointCloudT::Ptr submap_ptr (new PointCloudT); 90 | for(const SubmapObj& submap: submaps_set){ 91 | submap_ptr.reset(new PointCloudT(submap.submap_pcl_)); 92 | PointCloudColorHandlerCustom cloud_color(submap_ptr, submap.colors_[0], submap.colors_[1], submap.colors_[2]); 93 | viewer_.addPointCloud(submap_ptr, cloud_color, "cloud_" + std::to_string(i), vp1_); 94 | viewer_.addCoordinateSystem(3.0, submap.submap_tf_, "cloud_" + std::to_string(i), vp1_); 95 | i++; 96 | } 97 | viewer_.spinOnce(); 98 | } 99 | 100 | void SubmapsVisualizer::addSubmap(const SubmapObj& submap_i, int vp){ 101 | 102 | // Add pointclouds 103 | if(this->traj_cnt_ == NULL){ 104 | this->traj_cnt_ = 0; 105 | } 106 | PointCloudT::Ptr submap_ptr (new PointCloudT); 107 | submap_ptr.reset(new PointCloudT(submap_i.submap_pcl_)); 108 | PointCloudColorHandlerCustom cloud_color(submap_ptr, submap_i.colors_[0], submap_i.colors_[1], submap_i.colors_[2]); 109 | viewer_.addPointCloud(submap_ptr, cloud_color, "cloud_" + std::to_string(this->traj_cnt_), vp); 110 | viewer_.addCoordinateSystem(3.0, submap_i.submap_tf_, "cloud_" + std::to_string(this->traj_cnt_), vp); 111 | this->traj_cnt_++; 112 | } 113 | 114 | void SubmapsVisualizer::plotMBESPing(const SubmapObj& submap_i, float spam, float n_beams, int vp){ 115 | 116 | Eigen::Matrix3f rot = submap_i.submap_tf_.linear(); 117 | Eigen::Quaternionf rot_q = Quaternionf(rot); 118 | Eigen::Vector3f z_or = rot.col(2).transpose(); 119 | std::vector > beam_directions; 120 | 121 | float roll_step = spam/n_beams; 122 | float pitch = 0.0, yaw = 0.0; 123 | Eigen::Quaternionf q; 124 | q = Eigen::AngleAxisf(3.1415, Eigen::Vector3f::UnitX()) 125 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 126 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 127 | 128 | rot_q *= q; 129 | 130 | for(int i = -n_beams/2; i<=n_beams/2; i++){ 131 | // std::cout << "step " << i << " and angle " << roll_step*i << std::endl; 132 | q = Eigen::AngleAxisf(roll_step*i, Eigen::Vector3f::UnitX()) 133 | * Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) 134 | * Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()); 135 | beam_directions.push_back(Eigen::Quaternionf(rot_q * q)); 136 | Eigen::Matrix3f rot_mat = (rot_q * q).toRotationMatrix(); 137 | Eigen::Vector3f z_or = rot_mat.col(2).transpose(); 138 | // std::cout << "Beams directions " << z_or.transpose() << std::endl; 139 | 140 | pcl::ModelCoefficients line_coeff; 141 | line_coeff.values.resize (6); // We need 6 values 142 | line_coeff.values[0] = submap_i.submap_tf_.translation()[0]; 143 | line_coeff.values[1] = submap_i.submap_tf_.translation()[1]; 144 | line_coeff.values[2] = submap_i.submap_tf_.translation()[2]; 145 | line_coeff.values[3] = z_or.x (); 146 | line_coeff.values[4] = z_or.y (); 147 | line_coeff.values[5] = z_or.z (); 148 | viewer_.addLine(line_coeff, "line_" + std::to_string(i), vp); 149 | } 150 | } 151 | 152 | 153 | void SubmapsVisualizer::plotPoseGraphG2O(const GraphConstructor& graph, const SubmapsVec& submaps_set){ 154 | 155 | // Clean initial graph 156 | viewer_.removeAllPointClouds(vp1_); 157 | viewer_.removeAllCoordinateSystems(vp1_); 158 | viewer_.removeAllShapes(vp1_); 159 | viewer_.addCoordinateSystem(5.0, "reference_frame", vp1_); 160 | 161 | // Update pointclouds 162 | unsigned int i = 0; 163 | PointCloudT::Ptr submap_ptr (new PointCloudT); 164 | for(const SubmapObj& submap: submaps_set){ 165 | submap_ptr.reset(new PointCloudT(submap.submap_pcl_)); 166 | PointCloudColorHandlerCustom cloud_color(submap_ptr, submap.colors_[0], submap.colors_[1], submap.colors_[2]); 167 | viewer_.addPointCloud(submap_ptr, cloud_color, "cloud_" + std::to_string(i), vp1_); 168 | viewer_.addCoordinateSystem(3.0, submap.submap_tf_, "cloud_" + std::to_string(i), vp1_); 169 | i++; 170 | } 171 | 172 | // Plot initial trajectory estimate 173 | i = 0; 174 | Vector3i dr_color = Vector3i(rand() % 256, rand() % 256, rand() % 256); 175 | for(unsigned int j=0; j(); 178 | SubmapObj submap_to = submaps_set.at(j); 179 | Eigen::Vector3d to_ps = submap_to.submap_tf_.translation().cast(); 180 | viewer_.addArrow(PointT(from_ps[0],from_ps[1],from_ps[2]), PointT(to_ps[0],to_ps[1],to_ps[2]), 181 | dr_color[0], dr_color[1], dr_color[2], false, "final_dr_edge_" + std::to_string(j), vp1_); 182 | } 183 | 184 | // Plot LC edges 185 | i = 0; 186 | Vector3i lc_color = Vector3i(rand() % 256, rand() % 256, rand() % 256); 187 | for(EdgeSE3* edgeLC: graph.lcEdges_){ 188 | VertexSE3* from = static_cast(edgeLC->vertex(0)); 189 | VertexSE3* to = static_cast(edgeLC->vertex(1)); 190 | Eigen::Vector3d from_ps = from->estimate().translation(); 191 | Eigen::Vector3d to_ps = to->estimate().translation(); 192 | viewer_.addArrow(PointT(from_ps[0],from_ps[1],from_ps[2]), PointT(to_ps[0],to_ps[1],to_ps[2]), 193 | lc_color[0], lc_color[1], lc_color[2], false, "lc_edge_" + std::to_string(i), vp1_); 194 | i++; 195 | } 196 | 197 | viewer_.spinOnce(); 198 | } 199 | 200 | void SubmapsVisualizer::plotPoseGraphCeres(SubmapsVec& submaps_set){ 201 | 202 | // Clean initial graph 203 | viewer_.removeAllPointClouds(vp1_); 204 | viewer_.removeAllCoordinateSystems(vp1_); 205 | viewer_.removeAllShapes(vp1_); 206 | viewer_.addCoordinateSystem(5.0, "reference_frame", vp1_); 207 | 208 | // Update pointclouds 209 | unsigned int i = 0; 210 | PointCloudT::Ptr submap_ptr (new PointCloudT); 211 | for(const SubmapObj& submap: submaps_set){ 212 | submap_ptr.reset(new PointCloudT(submap.submap_pcl_)); 213 | PointCloudColorHandlerCustom cloud_color(submap_ptr, submap.colors_[0], submap.colors_[1], submap.colors_[2]); 214 | viewer_.addPointCloud(submap_ptr, cloud_color, "cloud_" + std::to_string(i), vp1_); 215 | viewer_.addCoordinateSystem(3.0, submap.submap_tf_, "cloud_" + std::to_string(i), vp1_); 216 | i++; 217 | } 218 | 219 | // Plot final trajectory estimate 220 | Vector3i dr_color = Vector3i(rand() % 256, rand() % 256, rand() % 256); 221 | for(unsigned int j=0; j(); 224 | SubmapObj submap_to = submaps_set.at(j); 225 | Eigen::Vector3d to_ps = submap_to.submap_tf_.translation().cast(); 226 | viewer_.addArrow(PointT(from_ps[0],from_ps[1],from_ps[2]), PointT(to_ps[0],to_ps[1],to_ps[2]), 227 | dr_color[0], dr_color[1], dr_color[2], false, "final_dr_edge_" + std::to_string(j), vp1_); 228 | } 229 | 230 | viewer_.spinOnce(); 231 | } 232 | 233 | void SubmapsVisualizer::addCoordinateSystem(const Eigen::Isometry3f& tf){ 234 | 235 | viewer_.addCoordinateSystem(2.0, tf, "sample_" + std::to_string(this->sample_cnt++), vp2_); 236 | } 237 | 238 | void SubmapsVisualizer::removeCoordinateSystem(){ 239 | 240 | for(int i=0; i(tf_grid), "grid_" + std::to_string(cnt++), vp1_); 253 | } 254 | viewer_.spinOnce(); 255 | } 256 | -------------------------------------------------------------------------------- /src/apps/src/test_slam_real.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "data_tools/std_data.h" 21 | #include "data_tools/benchmark.h" 22 | 23 | #include "submaps_tools/cxxopts.hpp" 24 | #include "submaps_tools/submaps.hpp" 25 | 26 | #include "registration/utils_visualization.hpp" 27 | #include "registration/gicp_reg.hpp" 28 | 29 | #include "graph_optimization/utils_g2o.hpp" 30 | #include "graph_optimization/graph_construction.hpp" 31 | #include "graph_optimization/ceres_optimizer.hpp" 32 | #include "graph_optimization/read_g2o.h" 33 | 34 | #include "bathy_slam/bathy_slam.hpp" 35 | 36 | #include 37 | 38 | #define INTERACTIVE 0 39 | #define VISUAL 0 40 | 41 | using namespace Eigen; 42 | using namespace std; 43 | using namespace g2o; 44 | 45 | bool next_step = false; 46 | int current_step = 0; 47 | 48 | 49 | void add_benchmark(SubmapsVec& submaps, benchmark::track_error_benchmark& benchmark, const string& name, bool is_groundtruth=false) { 50 | PointsT map = pclToMatrixSubmap(submaps); 51 | PointsT track = trackToMatrixSubmap(submaps); 52 | if (is_groundtruth) { 53 | benchmark.add_ground_truth(map, track); 54 | } else { 55 | benchmark.add_benchmark(map, track, name); 56 | } 57 | } 58 | 59 | void benchmark_gt(SubmapsVec& submaps_gt, benchmark::track_error_benchmark& benchmark) { 60 | // Benchmark GT 61 | add_benchmark(submaps_gt, benchmark, "0_original", true); 62 | ceres::optimizer::saveOriginalTrajectory(submaps_gt); // Save original trajectory to txt 63 | std::cout << "Visualizing original survey, press space to continue" << std::endl; 64 | } 65 | 66 | SubmapsVec build_bathymetric_graph(GraphConstructor& graph_obj, SubmapsVec& submaps_gt, 67 | GaussianGen& transSampler, GaussianGen& rotSampler, YAML::Node config) { 68 | 69 | // GICP reg for submaps 70 | SubmapRegistration gicp_reg(config); 71 | 72 | // Create SLAM solver and run offline 73 | std::cout << "Building bathymetric graph with GICP submap registration" << std::endl; 74 | BathySlam slam_solver(graph_obj, gicp_reg); 75 | SubmapsVec submaps_reg = slam_solver.runOffline(submaps_gt, transSampler, rotSampler, config); 76 | std::cout << "Done building graph, press space to continue" << std::endl; 77 | return submaps_reg; 78 | } 79 | 80 | // Create initial graph estimates, optionally add gaussian noise if add_gaussian_noise = true 81 | void create_initial_graph_estimate(GraphConstructor& graph_obj, SubmapsVec& submaps_reg, GaussianGen& transSampler, GaussianGen& rotSampler, bool add_gaussian_noise) { 82 | std::cout << "Add gaussian noise = " << add_gaussian_noise << std::endl; 83 | if (add_gaussian_noise) { 84 | // Add noise to edges on the graph 85 | graph_obj.addNoiseToGraph(transSampler, rotSampler); 86 | std::cout << "Gaussian noise added to graph" << std::endl; 87 | } 88 | // Create initial DR chain and visualize 89 | graph_obj.createInitialEstimate(submaps_reg); 90 | std::cout << "Initial graph estimate constructed, press space to continue" << std::endl; 91 | 92 | } 93 | 94 | void optimize_graph(GraphConstructor& graph_obj, SubmapsVec& submaps_reg, std::string outFilename, char* argv0, boost::filesystem::path output_path) { 95 | // Save graph to output g2o file (optimization can be run with G2O) 96 | graph_obj.saveG2OFile(outFilename); 97 | 98 | // Optimize graph and save to cereal 99 | google::InitGoogleLogging(argv0); 100 | ceres::optimizer::MapOfPoses poses = ceres::optimizer::ceresSolver(outFilename, graph_obj.drEdges_.size()); 101 | ceres::optimizer::updateSubmapsCeres(poses, submaps_reg); 102 | std::cout << "Output cereal: " << boost::filesystem::basename(output_path) << std::endl; 103 | std::ofstream os(boost::filesystem::basename(output_path) + ".cereal", std::ofstream::binary); 104 | { 105 | cereal::BinaryOutputArchive oarchive(os); 106 | oarchive(submaps_reg); 107 | os.close(); 108 | } 109 | std::cout << "Graph optimized, press space to continue" << std::endl; 110 | } 111 | 112 | void print_benchmark_results(SubmapsVec& submaps_reg, benchmark::track_error_benchmark& benchmark) { 113 | benchmark.print_summary(); 114 | 115 | std::string command_str = "python ../scripts/plot_results.py --initial_poses poses_original.txt --corrupted_poses poses_corrupted.txt --optimized_poses poses_optimized.txt"; 116 | const char *command = command_str.c_str(); 117 | system(command); 118 | } 119 | 120 | void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing) { 121 | if (event.getKeySym() == "space" && event.keyDown()) { 122 | next_step = true; 123 | current_step++; 124 | } 125 | } 126 | 127 | int main(int argc, char** argv){ 128 | // Inputs 129 | std::string folder_str, path_str, output_str, simulation, config_path; 130 | cxxopts::Options options("MyProgram", "One line description of MyProgram"); 131 | options.add_options() 132 | ("help", "Print help") 133 | ("simulation", "Simulation data from Gazebo", cxxopts::value(simulation)) 134 | ("bathy_survey", "Input MBES pings in cereal file if simulation = no. If in simulation" 135 | "input path to map_small folder", cxxopts::value(path_str)) 136 | ("config", "YAML config file", cxxopts::value(config_path)); 137 | 138 | auto result = options.parse(argc, argv); 139 | if (result.count("help")) { 140 | cout << options.help({ "", "Group" }) << endl; 141 | exit(0); 142 | } 143 | if(output_str.empty()){ 144 | output_str = "output_cereal.cereal"; 145 | } 146 | boost::filesystem::path output_path(output_str); 147 | string outFilename = "graph_corrupted.g2o"; // G2O output file 148 | 149 | YAML::Node config = YAML::LoadFile(config_path); 150 | std::cout << "Config file: " << config_path << std::endl; 151 | DRNoise dr_noise = loadDRNoiseFromFile(config); 152 | 153 | // Parse submaps from cereal file 154 | boost::filesystem::path submaps_path(path_str); 155 | std::cout << "Input data " << submaps_path << std::endl; 156 | 157 | SubmapsVec submaps_gt, submaps_reg; 158 | if(simulation == "yes"){ 159 | submaps_gt = readSubmapsInDir(submaps_path.string(), dr_noise); 160 | } 161 | else{ 162 | std_data::mbes_ping::PingsT std_pings = std_data::read_data(submaps_path); 163 | std::cout << "Number of pings in survey " << std_pings.size() << std::endl; 164 | { 165 | SubmapsVec traj_pings = parsePingsAUVlib(std_pings, dr_noise); 166 | int submap_size = config["submap_size"].as(); 167 | submaps_gt = createSubmaps(traj_pings, submap_size, dr_noise); 168 | 169 | // Filtering of submaps 170 | PointCloudT::Ptr cloud_ptr (new PointCloudT); 171 | pcl::VoxelGrid voxel_grid_filter; 172 | voxel_grid_filter.setInputCloud (cloud_ptr); 173 | voxel_grid_filter.setLeafSize(config["downsampling_leaf_x"].as(), 174 | config["downsampling_leaf_y"].as(), 175 | config["downsampling_leaf_z"].as()); 176 | for(SubmapObj& submap_i: submaps_gt){ 177 | *cloud_ptr = submap_i.submap_pcl_; 178 | voxel_grid_filter.setInputCloud(cloud_ptr); 179 | voxel_grid_filter.filter(*cloud_ptr); 180 | submap_i.submap_pcl_ = *cloud_ptr; 181 | } 182 | } 183 | } 184 | std::cout << "Number of submaps " << submaps_gt.size() << std::endl; 185 | 186 | // Graph constructor 187 | // Read training covs from folder 188 | covs covs_lc; 189 | boost::filesystem::path folder(folder_str); 190 | if(boost::filesystem::is_directory(folder)) { 191 | covs_lc = readCovsFromFiles(folder); 192 | } 193 | GraphConstructor graph_obj(covs_lc); 194 | 195 | // Noise generators 196 | GaussianGen transSampler, rotSampler; 197 | Matrix information = generateGaussianNoise(transSampler, rotSampler); 198 | 199 | // flag for adding gaussian noise to submaps and graph 200 | bool add_gaussian_noise = config["add_gaussian_noise"].as(); 201 | 202 | benchmark::track_error_benchmark benchmark("real_data", config["benchmark_nbr_rows"].as(), config["benchmark_nbr_cols"].as()); 203 | std::cout << "Benchmark nbr rows and cols: " << benchmark.benchmark_nbr_rows << ", " << benchmark.benchmark_nbr_cols << std::endl; 204 | 205 | #if VISUAL != 1 206 | benchmark_gt(submaps_gt, benchmark); 207 | submaps_reg = build_bathymetric_graph(graph_obj, submaps_gt, transSampler, rotSampler, config); 208 | add_benchmark(submaps_gt, benchmark, "1_After_GICP_GT"); 209 | add_benchmark(submaps_reg, benchmark, "2_After_GICP_reg"); 210 | 211 | add_benchmark(submaps_reg, benchmark, "3_Before_init_graph_estimates_reg"); 212 | create_initial_graph_estimate(graph_obj, submaps_reg, transSampler, rotSampler, add_gaussian_noise); 213 | add_benchmark(submaps_reg, benchmark, "4_After_init_graph_estimates_reg"); 214 | 215 | add_benchmark(submaps_reg, benchmark, "5_before_optimize_graph"); 216 | optimize_graph(graph_obj, submaps_reg, outFilename, argv[0], output_path); 217 | add_benchmark(submaps_reg, benchmark, "6_optimized"); 218 | #endif 219 | 220 | // Visualization 221 | #if VISUAL == 1 222 | PCLVisualizer viewer ("Submaps viewer"); 223 | viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*) NULL); 224 | viewer.loadCameraParameters("Antarctica7"); 225 | SubmapsVisualizer* visualizer = new SubmapsVisualizer(viewer); 226 | visualizer->setVisualizer(submaps_gt, 1); 227 | 228 | while (!viewer.wasStopped()) { 229 | viewer.spinOnce(); 230 | if (next_step) { 231 | next_step = false; 232 | switch (current_step) 233 | { 234 | case 1: 235 | // Benchmark GT 236 | benchmark_gt(submaps_gt, benchmark); 237 | submaps_reg = build_bathymetric_graph(graph_obj, submaps_gt, transSampler, rotSampler, config); 238 | visualizer->updateVisualizer(submaps_reg); 239 | // Benchmark GT after GICP, the GT submaps have now been moved due to GICP registration 240 | add_benchmark(submaps_gt, benchmark, "1_After_GICP_GT"); 241 | add_benchmark(submaps_reg, benchmark, "2_After_GICP_reg"); 242 | break; 243 | case 2: 244 | add_benchmark(submaps_reg, benchmark, "3_Before_init_graph_estimates_reg"); 245 | create_initial_graph_estimate(graph_obj, submaps_reg, transSampler, rotSampler, add_gaussian_noise); 246 | visualizer->plotPoseGraphG2O(graph_obj, submaps_reg); 247 | // Benchmark corrupted (or not corrupted if add_gaussian_noise = false) 248 | add_benchmark(submaps_reg, benchmark, "4_After_init_graph_estimates_reg"); 249 | break; 250 | case 3: 251 | add_benchmark(submaps_reg, benchmark, "5_before_optimize_graph"); 252 | optimize_graph(graph_obj, submaps_reg, outFilename, argv[0], output_path); 253 | // Visualize Ceres output 254 | visualizer->plotPoseGraphCeres(submaps_reg); 255 | // Benchmark Optimized 256 | add_benchmark(submaps_reg, benchmark, "6_optimized"); 257 | break; 258 | default: 259 | break; 260 | } 261 | } 262 | } 263 | delete(visualizer); 264 | print_benchmark_results(submaps_reg, benchmark); 265 | #endif 266 | 267 | return 0; 268 | } 269 | -------------------------------------------------------------------------------- /src/graph_optimization/src/ceres_optimizer.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "graph_optimization/ceres_optimizer.hpp" 13 | 14 | namespace ceres{ 15 | namespace optimizer { 16 | 17 | // Constructs the nonlinear least squares optimization problem from the pose 18 | // graph constraints. 19 | void BuildOptimizationProblem(const VectorOfConstraints& constraints, 20 | MapOfPoses* poses, ceres::Problem* problem, 21 | int drConstraints) { 22 | CHECK(poses != NULL); 23 | CHECK(problem != NULL); 24 | if (constraints.empty()) { 25 | LOG(INFO) << "No constraints, no problem to optimize."; 26 | return; 27 | } 28 | 29 | // LossFunction* loss_function = new ceres::HuberLoss(1); 30 | LossFunction* loss_function = nullptr; 31 | SubsetParameterization* z_local_param = new SubsetParameterization(3, std::vector{2}); 32 | SubsetParameterization* roll_pitch_local_param = new SubsetParameterization(3, std::vector{0,1}); 33 | 34 | // DR constraints 35 | std::cout << "Adding DR " << drConstraints << " constraints" << std::endl; 36 | for (VectorOfConstraints::const_iterator constraints_iter = constraints.begin(); 37 | constraints_iter != constraints.begin() + drConstraints; ++constraints_iter){ 38 | 39 | const Constraint3d& constraint = *constraints_iter; 40 | 41 | MapOfPoses::iterator pose_begin = poses->find(constraint.id_begin); 42 | CHECK(pose_begin != poses->end()) 43 | << "Pose with ID: " << constraint.id_begin << " not found."; 44 | MapOfPoses::iterator pose_end = poses->find(constraint.id_end); 45 | CHECK(pose_end != poses->end()) 46 | << "Pose with ID: " << constraint.id_end << " not found."; 47 | 48 | // Cost function 49 | const Eigen::Matrix sqrt_information = 50 | constraint.information.llt().matrixL(); 51 | ceres::CostFunction* cost_function = 52 | PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information); 53 | 54 | // Residual block 55 | problem->AddResidualBlock(cost_function, loss_function, 56 | pose_begin->second.p.data(), 57 | pose_begin->second.q.data(), 58 | pose_end->second.p.data(), 59 | pose_end->second.q.data()); 60 | 61 | // Constraints in roll and pitch 62 | problem->SetParameterization(pose_begin->second.q.data(), roll_pitch_local_param); 63 | problem->SetParameterization(pose_end->second.q.data(), roll_pitch_local_param); 64 | 65 | // Constraints in z 66 | problem->SetParameterization(pose_begin->second.p.data(), z_local_param); 67 | problem->SetParameterization(pose_end->second.p.data(), z_local_param); 68 | 69 | // Set boundaries for x, y and yaw 70 | // double upp_constraint = 10; 71 | // double low_constraint = 10; 72 | // double yaw_upp_const = M_PI/10.0; 73 | // double yaw_low_const = M_PI/10.0; 74 | // problem->SetParameterLowerBound(pose_begin->second.p.data(), 0, pose_begin->second.p[0] - low_constraint); 75 | // problem->SetParameterLowerBound(pose_end->second.p.data(), 0, pose_end->second.p[0] - low_constraint ); 76 | // problem->SetParameterUpperBound(pose_begin->second.p.data(), 0, pose_begin->second.p[0] + upp_constraint); 77 | // problem->SetParameterUpperBound(pose_end->second.p.data(), 0, pose_end->second.p[0] + upp_constraint); 78 | 79 | // problem->SetParameterLowerBound(pose_begin->second.p.data(), 1, pose_begin->second.p[1] - low_constraint); 80 | // problem->SetParameterLowerBound(pose_end->second.p.data(), 1, pose_end->second.p[1] - low_constraint); 81 | // problem->SetParameterUpperBound(pose_begin->second.p.data(), 1, pose_begin->second.p[1] + upp_constraint); 82 | // problem->SetParameterUpperBound(pose_end->second.p.data(), 1, pose_end->second.p[1] + upp_constraint); 83 | 84 | // problem->SetParameterLowerBound(pose_begin->second.q.data(), 2, pose_begin->second.q[2] - yaw_low_const); 85 | // problem->SetParameterLowerBound(pose_end->second.q.data(), 2, pose_end->second.q[2] - yaw_low_const); 86 | // problem->SetParameterUpperBound(pose_begin->second.q.data(), 2, pose_begin->second.q[2] + yaw_upp_const); 87 | // problem->SetParameterUpperBound(pose_end->second.q.data(), 2, pose_end->second.q[2] + yaw_upp_const); 88 | } 89 | 90 | // LC constraints 91 | if(constraints.size() - drConstraints != 0){ 92 | std::cout << "Adding LC " << constraints.size() - drConstraints << " constraints" << std::endl; 93 | for (VectorOfConstraints::const_iterator constraints_iter = constraints.begin() + drConstraints+1; 94 | constraints_iter != constraints.end(); ++constraints_iter){ 95 | 96 | const Constraint3d& constraint = *constraints_iter; 97 | 98 | MapOfPoses::iterator pose_begin = poses->find(constraint.id_begin); 99 | CHECK(pose_begin != poses->end()) 100 | << "Pose with ID: " << constraint.id_begin << " not found."; 101 | MapOfPoses::iterator pose_end = poses->find(constraint.id_end); 102 | CHECK(pose_end != poses->end()) 103 | << "Pose with ID: " << constraint.id_end << " not found."; 104 | 105 | const Eigen::Matrix sqrt_information = 106 | constraint.information.llt().matrixL(); 107 | 108 | // Cost function 109 | ceres::CostFunction* cost_function = 110 | PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information); 111 | 112 | // Residual block 113 | problem->AddResidualBlock(cost_function, loss_function, 114 | pose_begin->second.p.data(), 115 | pose_begin->second.q.data(), 116 | pose_end->second.p.data(), 117 | pose_end->second.q.data()); 118 | 119 | // Constraints in roll and pitch 120 | problem->SetParameterization(pose_begin->second.q.data(), roll_pitch_local_param); 121 | problem->SetParameterization(pose_end->second.q.data(), roll_pitch_local_param); 122 | 123 | // Constraints in z 124 | problem->SetParameterization(pose_begin->second.p.data(), z_local_param); 125 | problem->SetParameterization(pose_end->second.p.data(), z_local_param); 126 | 127 | // Set boundaries for x, y and yaw 128 | // double upp_constraint = 10; 129 | // double low_constraint = 10; 130 | // double yaw_upp_const = M_PI/100.0; 131 | // double yaw_low_const = M_PI/100.0; 132 | // problem->SetParameterLowerBound(pose_begin->second.p.data(), 0, pose_begin->second.p[0] - low_constraint); 133 | // problem->SetParameterLowerBound(pose_end->second.p.data(), 0, pose_end->second.p[0] - low_constraint ); 134 | // problem->SetParameterUpperBound(pose_begin->second.p.data(), 0, pose_begin->second.p[0] + upp_constraint); 135 | // problem->SetParameterUpperBound(pose_end->second.p.data(), 0, pose_end->second.p[0] + upp_constraint); 136 | 137 | // problem->SetParameterLowerBound(pose_begin->second.p.data(), 1, pose_begin->second.p[1] - low_constraint); 138 | // problem->SetParameterLowerBound(pose_end->second.p.data(), 1, pose_end->second.p[1] - low_constraint); 139 | // problem->SetParameterUpperBound(pose_begin->second.p.data(), 1, pose_begin->second.p[1] + upp_constraint); 140 | // problem->SetParameterUpperBound(pose_end->second.p.data(), 1, pose_end->second.p[1] + upp_constraint); 141 | 142 | // problem->SetParameterLowerBound(pose_begin->second.q.data(), 2, pose_begin->second.q[2] - yaw_low_const); 143 | // problem->SetParameterLowerBound(pose_end->second.q.data(), 2, pose_end->second.q[2] - yaw_low_const); 144 | // problem->SetParameterUpperBound(pose_begin->second.q.data(), 2, pose_begin->second.q[2] + yaw_upp_const); 145 | // problem->SetParameterUpperBound(pose_end->second.q.data(), 2, pose_end->second.q[2] + yaw_upp_const); 146 | } 147 | } 148 | 149 | // Constrain the gauge freedom: set first AUV pose as anchor constant 150 | MapOfPoses::iterator pose_start_iter = poses->begin(); 151 | CHECK(pose_start_iter != poses->end()) << "There are no poses."; 152 | problem->SetParameterBlockConstant(pose_start_iter->second.p.data()); 153 | problem->SetParameterBlockConstant(pose_start_iter->second.q.data()); 154 | } 155 | 156 | // Returns true if the solve was successful. 157 | int SolveOptimizationProblem(ceres::Problem* problem) { 158 | CHECK(problem != NULL); 159 | 160 | ceres::Solver::Options options; 161 | options.max_num_iterations = 300; 162 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 163 | // options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; 164 | // options.linear_solver_type = ceres::SPARSE_SCHUR; 165 | // options.linear_solver_type = ceres::SPARSE_QR; 166 | //options.linear_solver_type = ceres::DENSE_QR; 167 | 168 | // options.minimizer_type = ceres::LINE_SEARCH; 169 | options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; 170 | options.use_inner_iterations = false; 171 | options.minimizer_progress_to_stdout = true; 172 | options.num_threads =4; 173 | options.eta = 1e-9; 174 | 175 | ceres::Solver::Summary summary; 176 | ceres::Solve(options, problem, &summary); 177 | // std::cout << summary.FullReport() << '\n'; 178 | 179 | 180 | return summary.iterations.size(); 181 | } 182 | 183 | // Output the poses to the file with format: id x y z q_x q_y q_z q_w. 184 | bool OutputPoses(const std::string& filename, const MapOfPoses& poses) { 185 | std::fstream outfile; 186 | outfile.open(filename.c_str(), std::istream::out); 187 | if (!outfile) { 188 | LOG(ERROR) << "Error opening the file: " << filename; 189 | return false; 190 | } 191 | for (std::map, 192 | Eigen::aligned_allocator > >:: 193 | const_iterator poses_iter = poses.begin(); 194 | poses_iter != poses.end(); ++poses_iter) { 195 | const std::map, 196 | Eigen::aligned_allocator > >:: 197 | value_type& pair = *poses_iter; 198 | 199 | Eigen::Quaterniond q = eulerToQuat(pair.second.q); 200 | outfile << pair.first << " " << pair.second.p.transpose() << " " 201 | << q.x() << " " << q.y() << " " 202 | << q.z() << " " << q.w() << '\n'; 203 | } 204 | return true; 205 | } 206 | 207 | MapOfPoses ceresSolver(const std::string& outFilename, const int drConstraints){ 208 | // Ceres solver 209 | ceres::optimizer::MapOfPoses poses; 210 | ceres::optimizer::VectorOfConstraints constraints; 211 | 212 | CHECK(ceres::optimizer::ReadG2oFile(outFilename, &poses, &constraints)) 213 | << "Error reading the file: " << outFilename; 214 | 215 | CHECK(ceres::optimizer::OutputPoses("poses_corrupted.txt", poses)) 216 | << "Error outputting to poses_corrupted.txt"; 217 | 218 | std::cout << "Original poses output" << std::endl; 219 | 220 | ceres::Problem problem; 221 | ceres::optimizer::BuildOptimizationProblem(constraints, &poses, &problem, drConstraints); 222 | 223 | std::cout << "Ceres problem built" << std::endl; 224 | 225 | int iterations = ceres::optimizer::SolveOptimizationProblem(&problem); 226 | // << "The solve was not successful, exiting."; 227 | 228 | CHECK(ceres::optimizer::OutputPoses("poses_optimized.txt", poses)) 229 | << "Error outputting to poses_optimized.txt"; 230 | 231 | return poses; 232 | } 233 | 234 | void updateSubmapsCeres(const ceres::optimizer::MapOfPoses& poses, SubmapsVec& submaps_set){ 235 | 236 | // Update pointclouds 237 | unsigned int i = 0; 238 | for(SubmapObj& submap: submaps_set){ 239 | // Final pose of submap_i 240 | ceres::optimizer::Pose3d pose_i = poses.at(i); 241 | Eigen::Quaterniond q = Eigen::AngleAxisd(pose_i.q.x(), Eigen::Vector3d::UnitX()) 242 | * Eigen::AngleAxisd(pose_i.q.y(), Eigen::Vector3d::UnitY()) 243 | * Eigen::AngleAxisd(pose_i.q.z(), Eigen::Vector3d::UnitZ()); 244 | 245 | Isometry3f final_tf = (Isometry3f) q.cast(); 246 | final_tf.translation() = pose_i.p.cast(); 247 | 248 | // Transform submap_i pcl and tf 249 | pcl::transformPointCloud(submap.submap_pcl_, submap.submap_pcl_, 250 | (final_tf * submap.submap_tf_.inverse()).matrix()); 251 | submap.submap_tf_ = final_tf; 252 | i++; 253 | } 254 | } 255 | 256 | 257 | void saveOriginalTrajectory(SubmapsVec& submaps_set){ 258 | 259 | covs covs_lc; 260 | GraphConstructor* graph = new GraphConstructor(covs_lc); 261 | 262 | for(SubmapObj& submap_i: submaps_set){ 263 | graph->createNewVertex(submap_i); 264 | 265 | // Create DR edge i and store (skip submap 0) 266 | if(submap_i.submap_id_ != 0 ){ 267 | graph->createDREdge(submap_i); 268 | } 269 | } 270 | 271 | string outFilename = "graph_original.g2o"; 272 | graph->saveG2OFile(outFilename); 273 | ceres::optimizer::MapOfPoses poses; 274 | ceres::optimizer::VectorOfConstraints constraints; 275 | CHECK(ceres::optimizer::ReadG2oFile(outFilename, &poses, &constraints)) 276 | << "Error reading the file: " << outFilename; 277 | 278 | CHECK(ceres::optimizer::OutputPoses("poses_original.txt", poses)) 279 | << "Error outputting to poses_original.txt"; 280 | } 281 | 282 | 283 | } // namespace optimizer 284 | } // namespace ceres 285 | -------------------------------------------------------------------------------- /src/submaps_tools/src/submaps.cpp: -------------------------------------------------------------------------------- 1 | /* Copyright 2019 Ignacio Torroba (torroba@kth.se) 2 | * 3 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | * 5 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 7 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 8 | * 9 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 10 | */ 11 | 12 | #include "submaps_tools/submaps.hpp" 13 | #include 14 | 15 | using namespace Eigen; 16 | 17 | DRNoise loadDRNoiseFromFile(YAML::Node config) { 18 | return DRNoise{ 19 | .x=config["dr_noise_x"].as(), 20 | .y=config["dr_noise_y"].as(), 21 | .z=config["dr_noise_z"].as(), 22 | .roll=config["dr_noise_roll"].as(), 23 | .pitch=config["dr_noise_pitch"].as(), 24 | .yaw=config["dr_noise_yaw"].as(), 25 | }; 26 | } 27 | 28 | SubmapObj::SubmapObj(const DRNoise& dr_noise){ 29 | 30 | submap_info_ = createDRWeights(dr_noise); 31 | } 32 | 33 | SubmapObj::SubmapObj(const unsigned int& submap_id, const unsigned int& swath_id, PointCloudT& submap_pcl, const DRNoise& dr_noise): 34 | submap_id_(submap_id), swath_id_(swath_id), submap_pcl_(submap_pcl){ 35 | 36 | // AUV pose estimate while acquiring submap 37 | submap_tf_ = (Eigen::Isometry3f) submap_pcl.sensor_orientation_; 38 | submap_tf_.translation() = Eigen::Vector3f(submap_pcl.sensor_origin_.head(3)); 39 | 40 | // Delete sensor pose in pcl and transform the points accordingly 41 | submap_pcl_.sensor_origin_ << 0.0,0.0,0.0,0.0; 42 | submap_pcl_.sensor_orientation_ = Eigen::Quaternionf(0,0,0,0); 43 | pcl::transformPointCloud(submap_pcl_, submap_pcl_, submap_tf_.matrix()); 44 | 45 | submap_info_ = createDRWeights(dr_noise); 46 | } 47 | 48 | Eigen::Matrix SubmapObj::createDRWeights(const DRNoise& dr_noise){ 49 | 50 | // Uncertainty on vehicle nav across submaps (assuming here that each has a similar length) 51 | std::vector noiseTranslation; 52 | std::vector noiseRotation; 53 | noiseTranslation.push_back(dr_noise.x); 54 | noiseTranslation.push_back(dr_noise.y); 55 | noiseTranslation.push_back(dr_noise.z); 56 | noiseRotation.push_back(dr_noise.roll); 57 | noiseRotation.push_back(dr_noise.pitch); 58 | noiseRotation.push_back(dr_noise.yaw); 59 | 60 | Eigen::Matrix3d transNoise = Eigen::Matrix3d::Zero(); 61 | for (int i = 0; i < 3; ++i) 62 | transNoise(i, i) = std::pow(noiseTranslation[i], 2); 63 | 64 | Eigen::Matrix3d rotNoise = Eigen::Matrix3d::Zero(); 65 | for (int i = 0; i < 3; ++i) 66 | rotNoise(i, i) = std::pow(noiseRotation[i], 2); 67 | 68 | // Information matrix of the distribution 69 | Eigen::Matrix information = Eigen::Matrix::Zero(); 70 | information.block<3,3>(0,0) = transNoise.inverse(); 71 | information.block<3,3>(3,3) = rotNoise.inverse(); 72 | 73 | // std::cout << information << std::endl; 74 | return information; 75 | } 76 | 77 | void SubmapObj::findOverlaps(bool submaps_in_map_tf, 78 | std::vector > &submaps_set, double overlap_coverage){ 79 | 80 | overlaps_idx_.clear(); 81 | 82 | std::vector> corners_set; 83 | corners submap_i_corners = std::get<1>(getSubmapCorners(submaps_in_map_tf, *this, overlap_coverage)); 84 | // Extract corners of all submaps 85 | for(SubmapObj& submap_j: submaps_set){ 86 | corners_set.push_back(getSubmapCorners(submaps_in_map_tf, submap_j, overlap_coverage)); 87 | } 88 | 89 | bool overlap_flag; 90 | for(unsigned int k=0; k(corners_set.at(k))); 94 | if(overlap_flag == true){ 95 | overlaps_idx_.push_back(std::get<0>(corners_set.at(k))); 96 | } 97 | } 98 | } 99 | 100 | 101 | std::pair getSubmapCorners(bool submaps_in_map_tf, const SubmapObj& submap, double overlap_coverage){ 102 | 103 | // Transform point cloud back to map frame 104 | Eigen::MatrixXf points; 105 | if(submaps_in_map_tf){ 106 | PointCloudT submap_pcl_aux; 107 | pcl::transformPointCloud(submap.submap_pcl_, submap_pcl_aux, submap.submap_tf_.inverse().matrix()); 108 | points = submap_pcl_aux.getMatrixXfMap(3,4,0).transpose(); 109 | } 110 | else{ 111 | points = submap.submap_pcl_.getMatrixXfMap(3,4,0).transpose(); 112 | } 113 | 114 | // Extract corners 115 | double min_x, min_y, max_x, max_y; 116 | min_x = points.col(0).minCoeff() * overlap_coverage; // min x 117 | min_y = points.col(1).minCoeff() * overlap_coverage; // min y 118 | max_x = points.col(0).maxCoeff() * overlap_coverage; // max x 119 | max_y = points.col(1).maxCoeff() * overlap_coverage; // max y 120 | 121 | // 2D transformation of the corners back to original place 122 | // Eigen::Isometry2d submap_tf2d = (Eigen::Isometry2d) submap.submap_tf_.linear().cast(); 123 | // submap_tf2d.translation() = submap.submap_tf_.matrix().block<2,1>(0,3).cast(); 124 | 125 | corners submap_i_corners; 126 | submap_i_corners.push_back(submap.submap_tf_.cast() * Vector3d(min_x, min_y, 0)); 127 | submap_i_corners.push_back(submap.submap_tf_.cast() * Vector3d(min_x, max_y, 0)); 128 | submap_i_corners.push_back(submap.submap_tf_.cast() * Vector3d(max_x, max_y, 0)); 129 | submap_i_corners.push_back(submap.submap_tf_.cast() * Vector3d(max_x, min_y, 0)); 130 | 131 | return std::make_pair(submap.submap_id_, submap_i_corners); 132 | } 133 | 134 | 135 | bool checkSubmapsOverlap(const corners submap_i_corners, const corners submap_k_corners){ 136 | 137 | // Check every corner of i against every edge of k 138 | int inside; 139 | bool overlap = false; 140 | unsigned int k_next; 141 | for(Vector3d corner_i: submap_i_corners){ 142 | if(overlap == true){ 143 | break; 144 | } 145 | inside = 0; 146 | for(unsigned int k = 0; k b 164 | bool pointToLine(const Vector3d seg_a, const Vector3d seg_b, const Vector3d point_c){ 165 | 166 | int s = (seg_b[1] - seg_a[1]) * point_c[0] + (seg_a[0] - seg_b[0]) * point_c[1] + (seg_b[0] * seg_a[1] - seg_a[0] * seg_b[1]); 167 | 168 | return (s > 0)? true: false; // Point on right side 169 | } 170 | 171 | 172 | void readSubmapFile(const string submap_str, PointCloudT::Ptr submap_pcl){ 173 | 174 | if (pcl::io::loadPCDFile (submap_str, *submap_pcl) == -1){ 175 | PCL_ERROR ("Couldn't read .pcd file \n"); 176 | } 177 | } 178 | 179 | 180 | std::vector checkFilesInDir(DIR *dir){ 181 | // Check files and directories within directory 182 | struct dirent *ent; 183 | std::vector files; 184 | while ((ent = readdir (dir)) != NULL) { 185 | if( ent->d_type != DT_DIR ){ 186 | // If directory, move on 187 | files.push_back(std::string(ent->d_name)); 188 | } 189 | } 190 | closedir(dir); 191 | return files; 192 | } 193 | 194 | 195 | std::vector> readSubmapsInDir(const string& dir_path, const DRNoise& dr_noise){ 196 | 197 | std::vector> submaps_set; 198 | DIR *dir; 199 | if ((dir = opendir(dir_path.c_str())) != NULL) { 200 | // Open directory and check all files inside 201 | std::vector files = checkFilesInDir(dir); 202 | std::sort(files.begin(), files.end()); 203 | 204 | PointCloudT::Ptr submap_ptr (new PointCloudT); 205 | // For every file in the dir 206 | int submap_cnt = 0; 207 | int swath_cnt = 0; 208 | double prev_direction = 0; 209 | Eigen::Vector3f euler; 210 | for(const std::string file: files){ 211 | string file_name = std::string(dir_path) + file; 212 | std::cout << "Reading file: " << file_name << std::endl; 213 | readSubmapFile(file_name, submap_ptr); 214 | // Update swath counter 215 | euler = submap_ptr->sensor_orientation_.toRotationMatrix().eulerAngles(2, 1, 0); 216 | if(abs(euler[2] - prev_direction) > M_PI/2 /*&& euler[0]>0.0001*/){ 217 | swath_cnt = swath_cnt + 1; 218 | prev_direction = euler[2]; 219 | } 220 | SubmapObj submap_i(submap_cnt, swath_cnt, *submap_ptr, dr_noise); 221 | 222 | // Add AUV track to submap object 223 | submap_i.auv_tracks_.conservativeResize(submap_i.auv_tracks_.rows()+1, 3); 224 | submap_i.auv_tracks_.row(0) = submap_i.submap_tf_.translation().transpose().cast(); 225 | submaps_set.push_back(submap_i); 226 | submap_cnt ++; 227 | } 228 | } 229 | return submaps_set; 230 | } 231 | 232 | 233 | PointsT pclToMatrixSubmap(const SubmapsVec& submaps_set){ 234 | 235 | PointsT submaps; 236 | for(const SubmapObj& submap: submaps_set){ 237 | Eigen::MatrixXf points_submap_i = submap.submap_pcl_.getMatrixXfMap(3,4,0).transpose(); 238 | submaps.push_back(points_submap_i.cast()); 239 | } 240 | 241 | return submaps; 242 | } 243 | 244 | PointsT trackToMatrixSubmap(const SubmapsVec& submaps_set){ 245 | 246 | PointsT tracks; 247 | for(const SubmapObj& submap: submaps_set){ 248 | tracks.push_back(submap.auv_tracks_); 249 | } 250 | 251 | return tracks; 252 | } 253 | 254 | 255 | Eigen::Array3f computeInfoInSubmap(const SubmapObj& submap){ 256 | 257 | // Beams z centroid 258 | Eigen::Array3f mean_beam; 259 | float beam_cnt = 0.0; 260 | mean_beam.setZero(); 261 | for (const PointT& beam: submap.submap_pcl_){ 262 | mean_beam += beam.getArray3fMap(); 263 | beam_cnt ++; 264 | } 265 | mean_beam = mean_beam / beam_cnt; 266 | 267 | // Condition number of z 268 | Eigen::Array3f cond_num; 269 | cond_num.setZero(); 270 | for (const PointT& beam: submap.submap_pcl_){ 271 | cond_num += (beam.getArray3fMap() - mean_beam).abs(); 272 | } 273 | 274 | return cond_num; 275 | } 276 | 277 | SubmapsVec parsePingsAUVlib(std_data::mbes_ping::PingsT& pings, const DRNoise& dr_noise){ 278 | 279 | SubmapsVec pings_subs; 280 | std::cout << std::fixed; 281 | std::cout << std::setprecision(10); 282 | 283 | // For every .all file 284 | Isometry3d submap_tf; 285 | Isometry3d world_map_tf; 286 | int ping_cnt = 0; 287 | for (auto pos = pings.begin(); pos != pings.end(); ) { 288 | auto next = std::find_if(pos, pings.end(), [&](const std_data::mbes_ping& ping) { 289 | return ping.first_in_file_ && (&ping != &(*pos)); 290 | }); 291 | std_data::mbes_ping::PingsT track_pings; 292 | 293 | track_pings.insert(track_pings.end(), pos, next); 294 | if (pos == next) { 295 | break; 296 | } 297 | 298 | Vector3d ang; 299 | Vector3f pitch; 300 | Eigen::Matrix3d RM; 301 | 302 | /// For every ping in the .all file: transform coord from world to map frame 303 | /// where map frame is located where the AUV starts the mission 304 | for (std_data::mbes_ping& ping : track_pings) { 305 | if(ping_cnt == 0){ 306 | ang << 0.0, 0.0, 0; 307 | Vector3d dir = ping.beams.back() - ping.beams.front(); 308 | ang[2] = std::atan2(dir(1), dir(0)) + M_PI/2.0; 309 | RM = data_transforms::euler_to_matrix(ang(0), ang(1), ang(2)); 310 | world_map_tf.translation() = ping.pos_; 311 | world_map_tf.linear() = RM; 312 | std::cout << "Map reference frame " << world_map_tf.translation().transpose() << std::endl; 313 | } 314 | 315 | SubmapObj ping_sub = SubmapObj(dr_noise); 316 | for (Vector3d& p : ping.beams) { 317 | p = world_map_tf.linear().transpose() * p 318 | - world_map_tf.linear().transpose() * world_map_tf.translation(); 319 | ping_sub.submap_pcl_.points.push_back(PointT(p.x(), p.y(), p.z())); 320 | } 321 | ping_sub.submap_tf_.translation() = (world_map_tf.linear().transpose() 322 | * ping.pos_- world_map_tf.linear().transpose() 323 | * world_map_tf.translation()).cast(); 324 | 325 | ang << 0.0, 0.0, 0; 326 | Vector3d dir = ping.beams.back() - ping.beams.front(); 327 | ang[2] = std::atan2(dir(1), dir(0)) + M_PI/2.0; 328 | RM = data_transforms::euler_to_matrix(ang(0), ang(1), ang(2)); 329 | ping_sub.submap_tf_.linear() = (RM).cast(); 330 | pings_subs.push_back(ping_sub); 331 | ping_cnt++; 332 | } 333 | pos = next; 334 | } 335 | return pings_subs; 336 | } 337 | 338 | SubmapsVec createSubmaps(SubmapsVec& pings, int submap_size, const DRNoise& dr_noise){ 339 | 340 | SubmapsVec submaps_vec; 341 | std::vector> pings_tfs; 342 | Eigen::MatrixXd* auv_track = new Eigen::MatrixXd; 343 | int cnt = 0; 344 | int submap_cnt = 0; 345 | int swath_cnt = 0; 346 | SubmapObj* submap_k = new SubmapObj(dr_noise); 347 | for(SubmapObj& ping_i: pings){ 348 | submap_k->submap_pcl_ += ping_i.submap_pcl_; 349 | pings_tfs.push_back(ping_i.submap_tf_); 350 | if(auv_track->rows() < cnt + 1){ 351 | auv_track->conservativeResize(auv_track->rows() + 1, 3); 352 | } 353 | auv_track->row(cnt) = submap_k->submap_tf_.translation().array().transpose().cast(); 354 | cnt++; 355 | if(cnt > submap_size){ 356 | cnt = 0; 357 | submap_k->submap_id_ = submap_cnt; 358 | submap_k->submap_tf_ = pings_tfs.at(submap_size/2); 359 | pings_tfs.clear(); 360 | submap_k->auv_tracks_ = *auv_track; 361 | delete auv_track; 362 | auv_track = new Eigen::MatrixXd(); 363 | 364 | if(submap_cnt>0){ 365 | Eigen::Quaternionf rot_k = Eigen::Quaternionf(submap_k->submap_tf_.linear()); 366 | Eigen::Quaternionf rot_prev = Eigen::Quaternionf(submaps_vec.at(submap_cnt-1).submap_tf_.linear()); 367 | auto euler = (rot_k * rot_prev.inverse()).toRotationMatrix().eulerAngles(0,1,2); 368 | if(std::abs(euler(2)) > M_PI*0.9){ 369 | swath_cnt++; 370 | } 371 | } 372 | submap_cnt++; 373 | submap_k->swath_id_ = swath_cnt; 374 | 375 | submaps_vec.push_back(*submap_k); 376 | delete submap_k; 377 | submap_k = new SubmapObj(dr_noise); 378 | } 379 | } 380 | delete auv_track; 381 | 382 | return submaps_vec; 383 | } 384 | 385 | SubmapsVec createMap(SubmapsVec& pings, int submap_size, const DRNoise& dr_noise){ 386 | 387 | SubmapsVec submaps_vec; 388 | std::vector> pings_tfs; 389 | Eigen::MatrixXd* auv_track = new Eigen::MatrixXd; 390 | int cnt = 0; 391 | int submap_cnt = 0; 392 | int swath_cnt = 0; 393 | SubmapObj* submap_k = new SubmapObj(dr_noise); 394 | for(SubmapObj& ping_i: pings){ 395 | submap_k->submap_pcl_ += ping_i.submap_pcl_; 396 | pings_tfs.push_back(ping_i.submap_tf_); 397 | if(auv_track->rows() < cnt + 1){ 398 | auv_track->conservativeResize(auv_track->rows() + 1, 3); 399 | } 400 | auv_track->row(cnt) = submap_k->submap_tf_.translation().array().transpose().cast(); 401 | cnt++; 402 | if(cnt > submap_size){ 403 | cnt = 0; 404 | submap_k->submap_id_ = submap_cnt; 405 | submap_k->submap_tf_ = pings_tfs.at(submap_size/2); 406 | pings_tfs.clear(); 407 | submap_k->auv_tracks_ = *auv_track; 408 | delete auv_track; 409 | auv_track = new Eigen::MatrixXd(); 410 | 411 | if(submap_cnt>0){ 412 | Eigen::Quaternionf rot_k = Eigen::Quaternionf(submap_k->submap_tf_.linear()); 413 | Eigen::Quaternionf rot_prev = Eigen::Quaternionf(submaps_vec.at(submap_cnt-1).submap_tf_.linear()); 414 | auto euler = (rot_k * rot_prev.inverse()).toRotationMatrix().eulerAngles(0,1,2); 415 | if(std::abs(euler(2)) > M_PI*0.9){ 416 | swath_cnt++; 417 | } 418 | } 419 | submap_cnt++; 420 | submap_k->swath_id_ = swath_cnt; 421 | 422 | submaps_vec.push_back(*submap_k); 423 | delete submap_k; 424 | submap_k = new SubmapObj(dr_noise); 425 | } 426 | } 427 | delete auv_track; 428 | 429 | return submaps_vec; 430 | } 431 | 432 | SubmapsVec parseSubmapsAUVlib(std_data::pt_submaps& ss, const DRNoise& dr_noise){ 433 | 434 | SubmapsVec submaps_set; 435 | std::cout << std::fixed; 436 | std::cout << std::setprecision(10); 437 | int swath_cnt =0; 438 | int swath_cnt_prev = 0; 439 | std::cout << "Number of submaps " << ss.points.size() << std::endl; 440 | 441 | int submap_id = 0; 442 | Isometry3d map_tf; 443 | Isometry3d submap_tf; 444 | for(unsigned int k=0; k0){ 450 | if(std::norm(ss.angles.at(k)[2] - ss.angles.at(k-1)[2]) > M_PI){ 451 | ++swath_cnt; 452 | } 453 | } 454 | submap_k.swath_id_ = swath_cnt; 455 | 456 | // Apply original transform to points and vehicle track 457 | MatrixXd submap = ss.points.at(k); 458 | submap = submap * ss.rots.at(k).transpose(); 459 | submap.array().rowwise() += ss.trans.at(k).transpose().array(); 460 | 461 | MatrixXd tracks = ss.tracks.at(k); 462 | tracks = tracks * ss.rots.at(k).transpose(); 463 | tracks.array().rowwise() += ss.trans.at(k).transpose().array(); 464 | submap_k.auv_tracks_ = tracks; 465 | 466 | // Construct submap tf 467 | int mid = (tracks.size() % 2 == 0)? tracks.size()/2: (tracks.size()+1)/2; 468 | Eigen::Quaterniond rot(ss.rots.at(k)); 469 | Eigen::Vector3d trans = ss.trans.at(k); 470 | trans(2) = tracks.row(mid)(1); 471 | submap_tf.translation() = trans; 472 | submap_tf.linear() = rot.toRotationMatrix(); 473 | 474 | // Create map frame on top of first submap frame: avoid losing accuracy on floats 475 | if(k==0){ 476 | map_tf = submap_tf; 477 | std::cout << "Map reference frame " << map_tf.translation().transpose() << std::endl; 478 | } 479 | submap.array().rowwise() -= map_tf.translation().transpose().array(); 480 | submap_k.auv_tracks_.array().rowwise() -= map_tf.translation().transpose().array(); 481 | submap_tf.translation().array() -= map_tf.translation().transpose().array(); 482 | submap_k.submap_tf_ = submap_tf.cast(); 483 | 484 | // Create PCL from PointsT 485 | for(unsigned int i=0; iparseMapAUVlib(std_data::pt_submaps& ss, const DRNoise& dr_noise){ 495 | 496 | MapObj map_j(dr_noise); 497 | Isometry3d map_tf; 498 | Isometry3d submap_tf; 499 | for(unsigned int k=0; k submap_corners = getSubmapCorners(true, submap_i, overlap_coverage); 548 | double grid_x, grid_y; 549 | unsigned int k_next; 550 | bool reject = false; 551 | for(unsigned int k=0; k<2; k++){ 552 | k_next = k + 1; 553 | // Check against four edges 554 | Eigen::Vector3d corner_i = std::get<1>(submap_corners).at(k); 555 | Eigen::Vector3d corner_i2 = std::get<1>(submap_corners).at(k_next); 556 | if(k == 0){ 557 | grid_y = (corner_i - corner_i2).norm(); 558 | } 559 | else if (k == 1){ 560 | grid_x = (corner_i - corner_i2).norm(); 561 | } 562 | } 563 | 564 | if(abs(grid_x/grid_y) < 0.4 || abs(grid_y/grid_x) < 0.4){ 565 | reject = true; 566 | } 567 | return reject; 568 | } 569 | 570 | std::pair readCovMatrix(const std::string& file_name){ 571 | 572 | std::string line; 573 | Eigen::Matrix2d prior, posterior; 574 | prior.setZero(); posterior.setZero(); 575 | 576 | std::ifstream input; 577 | input.open(file_name); 578 | if(input.fail()) { 579 | cout << "ERROR: Cannot open the file..." << endl; 580 | exit(0); 581 | } 582 | 583 | int i = 0; 584 | while (std::getline(input, line)){ 585 | std::istringstream iss(line); 586 | double a, b; 587 | if (!(iss >> a >> b)) { break; } // error or end of file 588 | if(i<2){prior.row(i) = Eigen::Vector2d(a, b).transpose();} 589 | else{posterior.row(i-2) = Eigen::Vector2d(a, b).transpose();} 590 | i++; 591 | } 592 | return std::make_pair(prior, posterior); 593 | } 594 | 595 | covs readCovsFromFiles(boost::filesystem::path folder){ 596 | 597 | typedef vector v_path; 598 | v_path v; 599 | copy(boost::filesystem::directory_iterator(folder), 600 | boost::filesystem::directory_iterator(), back_inserter(v)); 601 | sort(v.begin(), v.end()); 602 | 603 | // Read covs generated from NN 604 | std::vector files; 605 | for (v_path::const_iterator it(v.begin()), it_end(v.end()); it != it_end; ++it) { 606 | if (boost::filesystem::is_directory(*it)) { 607 | continue; 608 | } 609 | 610 | if (boost::filesystem::extension(*it) != ".txt") { 611 | continue; 612 | } 613 | files.push_back(it->string()); 614 | } 615 | 616 | covs covs_lc(files.size()); 617 | for (unsigned int file = 0; file < files.size(); file++) { 618 | std::regex regex("\\_"); 619 | std::vector out( 620 | std::sregex_token_iterator(files.at(file).begin(), files.at(file).end(), regex, -1), 621 | std::sregex_token_iterator()); 622 | 623 | std::regex regex2("\\."); 624 | std::vector out2( 625 | std::sregex_token_iterator(out.back().begin(), out.back().end(), regex2, -1), 626 | std::sregex_token_iterator()); 627 | 628 | Eigen::Matrix2d prior, posterior; 629 | std::tie(prior, posterior) = readCovMatrix(files.at(file)); 630 | covs_lc.at(std::stoi(out2.front())) = posterior; 631 | 632 | out.clear(); 633 | out2.clear(); 634 | } 635 | 636 | return covs_lc; 637 | } 638 | 639 | 640 | /// Read loop closures from external file 641 | std::vector > readGTLoopClosures(string& fileName, int submaps_nb){ 642 | 643 | std::ifstream infile(fileName); 644 | std::vector > overlaps(static_cast(submaps_nb)); 645 | 646 | std::string line; 647 | while(std::getline(infile, line)){ 648 | vector result; 649 | boost::split(result, line, boost::is_any_of(" ")); 650 | vector result_d(result.size()); 651 | std::transform(result.begin(), result.end(), result_d.begin(), [](const std::string& val){ 652 | return std::stoi(val); 653 | }); 654 | overlaps.at(result_d[0]).insert(overlaps.at(result_d[0]).end(), result_d.begin()+1, result_d.end()); 655 | } 656 | 657 | return overlaps; 658 | } 659 | --------------------------------------------------------------------------------