Repository: gaoxiang12/lightning-lm Branch: master Commit: 1325fed8fa97 Files: 235 Total size: 1.1 MB Directory structure: gitextract_lz8vrmh6/ ├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── README.md ├── README_CN.md ├── cmake/ │ └── packages.cmake ├── config/ │ ├── default.yaml │ ├── default_livox.yaml │ ├── default_nclt.yaml │ ├── default_robosense.yaml │ ├── default_utbm.yaml │ └── default_vbr.yaml ├── doc/ │ └── g2p5_vbr.pgm ├── docker/ │ ├── Dockerfile │ └── README.md ├── package.xml ├── pcd/ │ └── .gitkeep ├── scripts/ │ ├── install_dep.sh │ ├── merge_bags.py │ └── save_default_map.sh ├── src/ │ ├── CMakeLists.txt │ ├── app/ │ │ ├── CMakeLists.txt │ │ ├── run_frontend_offline.cc │ │ ├── run_loc_offline.cc │ │ ├── run_loc_online.cc │ │ ├── run_loop_offline.cc │ │ ├── run_slam_offline.cc │ │ ├── run_slam_online.cc │ │ └── test_ui.cc │ ├── common/ │ │ ├── constant.h │ │ ├── eigen_types.h │ │ ├── functional_points.h │ │ ├── imu.h │ │ ├── keyframe.h │ │ ├── loop_candidate.h │ │ ├── measure_group.h │ │ ├── nav_state.cc │ │ ├── nav_state.h │ │ ├── odom.h │ │ ├── options.cc │ │ ├── options.h │ │ ├── params.cc │ │ ├── params.h │ │ ├── point_def.h │ │ ├── pose_rpy.h │ │ ├── s2.hpp │ │ ├── std_types.h │ │ └── timed_pose.h │ ├── core/ │ │ ├── g2p5/ │ │ │ ├── g2p5.cc │ │ │ ├── g2p5.h │ │ │ ├── g2p5_grid_data.h │ │ │ ├── g2p5_map.cc │ │ │ ├── g2p5_map.h │ │ │ ├── g2p5_subgrid.cc │ │ │ └── g2p5_subgrid.h │ │ ├── ivox3d/ │ │ │ ├── hilbert.hpp │ │ │ ├── ivox3d.h │ │ │ └── ivox3d_node.hpp │ │ ├── lightning_math.hpp │ │ ├── lio/ │ │ │ ├── anderson_acceleration.h │ │ │ ├── eskf.cc │ │ │ ├── eskf.hpp │ │ │ ├── imu_filter.h │ │ │ ├── imu_processing.hpp │ │ │ ├── laser_mapping.cc │ │ │ ├── laser_mapping.h │ │ │ ├── pointcloud_preprocess.cc │ │ │ ├── pointcloud_preprocess.h │ │ │ └── pose6d.h │ │ ├── localization/ │ │ │ ├── lidar_loc/ │ │ │ │ ├── lidar_loc.cc │ │ │ │ ├── lidar_loc.h │ │ │ │ └── pclomp/ │ │ │ │ ├── ndt_omp.cpp │ │ │ │ ├── ndt_omp.h │ │ │ │ ├── ndt_omp_impl.hpp │ │ │ │ ├── voxel_grid_covariance_omp.cpp │ │ │ │ ├── voxel_grid_covariance_omp.h │ │ │ │ └── voxel_grid_covariance_omp_impl.hpp │ │ │ ├── localization.cpp │ │ │ ├── localization.h │ │ │ ├── localization_result.cc │ │ │ ├── localization_result.h │ │ │ └── pose_graph/ │ │ │ ├── pgo.cc │ │ │ ├── pgo.h │ │ │ ├── pgo_impl.cc │ │ │ ├── pgo_impl.h │ │ │ ├── pose_extrapolator.cc │ │ │ ├── pose_extrapolator.h │ │ │ └── smoother.h │ │ ├── loop_closing/ │ │ │ ├── loop_closing.cc │ │ │ └── loop_closing.h │ │ ├── maps/ │ │ │ ├── tiled_map.cc │ │ │ ├── tiled_map.h │ │ │ ├── tiled_map_chunk.cc │ │ │ └── tiled_map_chunk.h │ │ ├── miao/ │ │ │ ├── CMakeLists.txt │ │ │ ├── core/ │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── common/ │ │ │ │ │ ├── config.h │ │ │ │ │ ├── macros.h │ │ │ │ │ ├── math.h │ │ │ │ │ ├── string_tools.cc │ │ │ │ │ └── string_tools.h │ │ │ │ ├── graph/ │ │ │ │ │ ├── base_binary_edge.h │ │ │ │ │ ├── base_edge.h │ │ │ │ │ ├── base_fixed_sized_edge.h │ │ │ │ │ ├── base_fixed_sized_edge.hpp │ │ │ │ │ ├── base_unary_edge.h │ │ │ │ │ ├── base_vec_vertex.h │ │ │ │ │ ├── base_vertex.h │ │ │ │ │ ├── edge.h │ │ │ │ │ ├── graph.cc │ │ │ │ │ ├── graph.h │ │ │ │ │ ├── optimizer.cc │ │ │ │ │ ├── optimizer.h │ │ │ │ │ └── vertex.h │ │ │ │ ├── math/ │ │ │ │ │ ├── marginal_covariance_cholesky.cc │ │ │ │ │ ├── marginal_covariance_cholesky.h │ │ │ │ │ ├── matrix_operations.h │ │ │ │ │ └── misc.h │ │ │ │ ├── opti_algo/ │ │ │ │ │ ├── algo_select.h │ │ │ │ │ ├── opti_algo_dogleg.cc │ │ │ │ │ ├── opti_algo_dogleg.h │ │ │ │ │ ├── opti_algo_gauss_newton.cc │ │ │ │ │ ├── opti_algo_gauss_newton.h │ │ │ │ │ ├── opti_algo_lm.cc │ │ │ │ │ ├── opti_algo_lm.h │ │ │ │ │ ├── optimization_algorithm.cc │ │ │ │ │ └── optimization_algorithm.h │ │ │ │ ├── robust_kernel/ │ │ │ │ │ ├── cauchy.h │ │ │ │ │ ├── dcs.h │ │ │ │ │ ├── fair.h │ │ │ │ │ ├── huber.h │ │ │ │ │ ├── mc_clure.h │ │ │ │ │ ├── pseudo_huber.h │ │ │ │ │ ├── robust_kernel.h │ │ │ │ │ ├── robust_kernel_all.h │ │ │ │ │ ├── saturated.h │ │ │ │ │ ├── tukey.h │ │ │ │ │ └── welsch.h │ │ │ │ ├── solver/ │ │ │ │ │ ├── block_solver.h │ │ │ │ │ ├── block_solver.hpp │ │ │ │ │ ├── linear_solver.h │ │ │ │ │ ├── linear_solver_ccs.h │ │ │ │ │ ├── linear_solver_dense.h │ │ │ │ │ ├── linear_solver_eigen.h │ │ │ │ │ ├── linear_solver_pcg.h │ │ │ │ │ ├── linear_solver_pcg.hpp │ │ │ │ │ ├── solver.cc │ │ │ │ │ └── solver.h │ │ │ │ ├── sparse/ │ │ │ │ │ ├── sparse_block_matrix.h │ │ │ │ │ ├── sparse_block_matrix.hpp │ │ │ │ │ ├── sparse_block_matrix_ccs.h │ │ │ │ │ ├── sparse_block_matrix_diagonal.h │ │ │ │ │ └── sparse_block_matrix_hashmap.h │ │ │ │ └── types/ │ │ │ │ ├── edge_se2.h │ │ │ │ ├── edge_se2_prior.h │ │ │ │ ├── edge_se3.h │ │ │ │ ├── edge_se3_height_prior.h │ │ │ │ ├── edge_se3_prior.h │ │ │ │ ├── vertex_pointxyz.h │ │ │ │ ├── vertex_se2.h │ │ │ │ └── vertex_se3.h │ │ │ ├── examples/ │ │ │ │ ├── CMakeLists.txt │ │ │ │ ├── bal/ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ └── bal_example.cc │ │ │ │ ├── data_fitting/ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ ├── circle_fit.cc │ │ │ │ │ └── curve_fit.cc │ │ │ │ ├── icp/ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ └── gicp_demo.cc │ │ │ │ ├── incremental/ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ └── incremental.cc │ │ │ │ ├── pose_graph/ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ └── pose_graph.cc │ │ │ │ ├── sba/ │ │ │ │ │ ├── CMakeLists.txt │ │ │ │ │ └── sba_demo.cc │ │ │ │ └── sphere/ │ │ │ │ ├── CMakeLists.txt │ │ │ │ └── sphere.cc │ │ │ └── utils/ │ │ │ ├── CMakeLists.txt │ │ │ ├── sampler.cc │ │ │ ├── sampler.h │ │ │ └── tuple_tools.h │ │ └── system/ │ │ ├── async_message_process.h │ │ ├── loc_system.cc │ │ ├── loc_system.h │ │ ├── slam.cc │ │ └── slam.h │ ├── io/ │ │ ├── dataset_type.h │ │ ├── file_io.cc │ │ ├── file_io.h │ │ ├── yaml_io.cc │ │ └── yaml_io.h │ ├── ui/ │ │ ├── pangolin_window.cc │ │ ├── pangolin_window.h │ │ ├── pangolin_window_impl.cc │ │ ├── pangolin_window_impl.h │ │ ├── ui_car.cc │ │ ├── ui_car.h │ │ ├── ui_cloud.cc │ │ ├── ui_cloud.h │ │ ├── ui_trajectory.cc │ │ └── ui_trajectory.h │ ├── utils/ │ │ ├── async_message_process.h │ │ ├── pointcloud_utils.cc │ │ ├── pointcloud_utils.h │ │ ├── sync.h │ │ ├── timer.cc │ │ └── timer.h │ └── wrapper/ │ ├── bag_io.cc │ ├── bag_io.h │ └── ros_utils.h ├── srv/ │ ├── LocCmd.srv │ └── SaveMap.srv └── thirdparty/ ├── Sophus/ │ ├── average.hpp │ ├── common.hpp │ ├── formatstring.hpp │ ├── geometry.hpp │ ├── interpolate.hpp │ ├── interpolate_details.hpp │ ├── num_diff.hpp │ ├── rotation_matrix.hpp │ ├── rxso2.hpp │ ├── rxso3.hpp │ ├── se2.hpp │ ├── se3.hpp │ ├── sim2.hpp │ ├── sim3.hpp │ ├── sim_details.hpp │ ├── so2.hpp │ ├── so3.hpp │ ├── types.hpp │ └── velocities.hpp └── livox_ros_driver/ ├── CMakeLists.txt ├── msg/ │ ├── CustomMsg.msg │ └── CustomPoint.msg └── package.xml ================================================ FILE CONTENTS ================================================ ================================================ FILE: .clang-format ================================================ --- Language: Cpp # BasedOnStyle: Google AccessModifierOffset: -1 AlignAfterOpenBracket: Align AlignConsecutiveAssignments: false AlignConsecutiveDeclarations: false AlignEscapedNewlines: Left AlignOperands: true AlignTrailingComments: true AllowAllParametersOfDeclarationOnNextLine: true AllowShortBlocksOnASingleLine: false AllowShortCaseLabelsOnASingleLine: false AllowShortFunctionsOnASingleLine: All AllowShortIfStatementsOnASingleLine: Always AllowShortLoopsOnASingleLine: true AlwaysBreakAfterDefinitionReturnType: None AlwaysBreakAfterReturnType: None AlwaysBreakBeforeMultilineStrings: true AlwaysBreakTemplateDeclarations: true BinPackArguments: true BinPackParameters: true BraceWrapping: AfterClass: false AfterControlStatement: false AfterEnum: false AfterFunction: false AfterNamespace: false AfterObjCDeclaration: false AfterStruct: false AfterUnion: false AfterExternBlock: false BeforeCatch: false BeforeElse: false IndentBraces: false SplitEmptyFunction: true SplitEmptyRecord: true SplitEmptyNamespace: true BreakBeforeBinaryOperators: None BreakBeforeBraces: Attach BreakBeforeInheritanceComma: false BreakBeforeTernaryOperators: true BreakConstructorInitializersBeforeComma: false BreakConstructorInitializers: BeforeColon BreakAfterJavaFieldAnnotations: false BreakStringLiterals: true ColumnLimit: 120 CommentPragmas: '^ IWYU pragma:' CompactNamespaces: false ConstructorInitializerAllOnOneLineOrOnePerLine: true ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 4 Cpp11BracedListStyle: true DerivePointerAlignment: true DisableFormat: false ExperimentalAutoDetectBinPacking: false FixNamespaceComments: true ForEachMacros: - foreach - Q_FOREACH - BOOST_FOREACH IncludeBlocks: Preserve IncludeCategories: - Regex: '^' Priority: 2 - Regex: '^<.*\.h>' Priority: 1 - Regex: '^<.*' Priority: 2 - Regex: '.*' Priority: 3 IncludeIsMainRegex: '([-_](test|unittest))?$' IndentCaseLabels: true IndentPPDirectives: None IndentWidth: 4 IndentWrappedFunctionNames: false JavaScriptQuotes: Leave JavaScriptWrapImports: true KeepEmptyLinesAtTheStartOfBlocks: false MacroBlockBegin: '' MacroBlockEnd: '' MaxEmptyLinesToKeep: 1 NamespaceIndentation: None ObjCBlockIndentWidth: 2 ObjCSpaceAfterProperty: false ObjCSpaceBeforeProtocolList: false PenaltyBreakAssignment: 2 PenaltyBreakBeforeFirstCallParameter: 1 PenaltyBreakComment: 300 PenaltyBreakFirstLessLess: 120 PenaltyBreakString: 1000 PenaltyExcessCharacter: 1000000 PenaltyReturnTypeOnItsOwnLine: 200 PointerAlignment: Left RawStringFormats: - Delimiters: [pb] Language: TextProto BasedOnStyle: google ReflowComments: true SortIncludes: true SortUsingDeclarations: true SpaceAfterCStyleCast: false SpaceAfterTemplateKeyword: true SpaceBeforeAssignmentOperators: true SpaceBeforeParens: ControlStatements SpaceInEmptyParentheses: false SpacesBeforeTrailingComments: 2 SpacesInAngles: false SpacesInContainerLiterals: true SpacesInCStyleCastParentheses: false SpacesInParentheses: false SpacesInSquareBrackets: false Standard: Auto TabWidth: 8 UseTab: Never ... ================================================ FILE: .gitignore ================================================ *build *install *log *.vscode *libs **data **/.vscode/ bin **cmake-build-debug** **cmake-build-release** ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.16) project(lightning) # Default to C++17 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif () # set to release by default set(CMAKE_BUILD_TYPE Release) # ------------------------------------------------------------- # 导入第三方库，同时做一些全局编译选项配置 add_definitions("-DPCL_NO_PRECOMPILE") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -fopenmp -fPIC -g") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O2 -fopenmp -fPIC -g -ggdb") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS} -ggdb -g -fopenmp -fPIC -g") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -g") if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif () set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) # set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/libs) # set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/libs) list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) include(cmake/packages.cmake) # msg 和 srv set(MY_SRV_FILES "") set(SRC_DIR ${PROJECT_SOURCE_DIR}/srv) file(GLOB_RECURSE SRC_FILES ${SRC_DIR}/*.srv) foreach (SRC_FILE ${SRC_FILES}) file(RELATIVE_PATH REL_PATH ${SRC_DIR} ${SRC_FILE}) list(APPEND MY_SRV_FILES ${PROJECT_SOURCE_DIR}:srv/${REL_PATH}) endforeach () rosidl_generate_interfaces(${PROJECT_NAME} ${MY_SRV_FILES} ${MY_MSG_FILES} ${MY_ACTION_FILES} DEPENDENCIES std_msgs geometry_msgs sensor_msgs nav_msgs ) include_directories( ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp ) install(DIRECTORY config DESTINATION lib/${PROJECT_NAME}/ ) add_subdirectory(thirdparty/livox_ros_driver) add_subdirectory(src) ament_package() ================================================ FILE: README.md ================================================ # Lightning-LM English | [中文](./README_CN.md) Lightning-Speed Lidar Localization and Mapping Lightning-LM is a complete laser mapping and localization module. Features of Lightning-LM: 1. [done] Complete 3D Lidar SLAM, fast LIO front-end (AA-FasterLIO), standard 2. [done] 3D to 2D map conversion (g2p5), optional, if selected outputs real-time 2D grid map, can be saved 3. [done] Real-time loop closure detection, standard, performs back-end loop closure detection and correction if selected 4. [done] Smooth high-precision 3D Lidar localization, standard 5. [done] Dynamic loading scheme for map partitions, suitable for large-scale scenes 6. [done] Localization with separate dynamic and static layers, adaptable to dynamic scenes, selectable strategies for dynamic layer, optional, if selected saves dynamic layer map content, three strategies available (short-term, medium-term, permanent), default is permanent 7. [done] High-frequency IMU smooth output, standard, 100Hz 8. GPS geoinformation association, optional (TODO) 9. Vehicle odometry input, optional (TODO) 10. [done] Lightweight optimization library miao and incremental optimization (derived from g2o, but lighter and faster, supports incremental optimization, no need to rebuild optimization model), standard, used in both loop closure and localization 11. [done] Two verification schemes: offline and online. Offline allows breakpoint debugging with strong consistency. Online allows multi-threaded concurrency, fast processing speed, dynamic frame skipping, and low resource usage. 12. [done] High-frequency output based on extrapolator and smoother, adjustable smoothing factor 13. [done] High-performance computing: All the above features can run using less than one CPU core on the pure CPU side (online localization 0.8 cores, mapping 1.2 cores, 32-line LiDAR, without UI). ## Updates ### 2026.4.2 - Significantly improved the stability of mapping and localization, now adapted to multi-floor data mentioned in issues and data provided by Deep Robotics. Related data is being uploaded to BaiduYun — feel free to try it! - Adjusted the structure and dimensions of state variables. `ba`, `grav`, `offset_R`, `offset_t` no longer need to be estimated online, reducing the state vector to 12 dimensions (previously 23). - Added a correction scale for laser localization. Localization is now based on LIO prediction to prevent large laser jumps. - Added point-to-point ICP error in the LaserMapping module; point-to-point computations are also accelerated with multi-threading. - Added some practical tricks in the ESKF module. - Localization now uses LIO keyframes for map-to-map registration. - Tuned parameters for several Deep Robotics datasets and GitHub issue datasets. - Adjusted the ESKF interface to accommodate point-to-point ICP (which has different dimensions from point-to-plane ICP). - Added Kalman filter tricks: symmetrization of the P matrix, protection of minimum values, etc. ### 2026.3.20 - MapIncremental is now called at the keyframe level to improve LIO robustness (no drift on the VBR dataset). - Fixed a Jacobian issue with the height constraint (issue #100, #110). - Fixed an out-of-bounds issue in the loop closure detection module (issue #88). - Adapted to Deep Robotics quadruped robot data (RoboSense lidar type=4). - Added timestamp data checks (VBR has abnormal timestamp issues). - Loop closure detection now uses the optimized pose as the initial estimate (useful for large loops). - If the point cloud after voxelization has too few points, use the pre-voxelization point cloud for LIO (prevents too few points after downsampling). - Fixed an issue with `std::vector` in parallelization. - Fixed several issues that could cause segmentation faults. ### 2025.11.27 - Added Cauchy's kernel in the LIO module. - Added the `try_self_extrap` configuration in the localization module (disabled by default). When disabled, the localization module does not use its own extrapolated pose for localization (since the localization interval is large and can be inaccurate when the vehicle moves significantly). - Added a configuration file for Livox, as it is widely used. - If a fixed height is set during mapping, localization will also use this map height (disabled by default). ### 2025.11.13 - Fix two logic typos in FasterLIO. - Add global height constraint that can be configured in loop_closing.with_height. If set true, lightning-lm will keep the output map into the same height to avoid the Z-axis drifting in large scenes. It should not be set if you are using lightning-lm in scenes that have multi-floor or stairs structures. ## Examples - Mapping on the VBR campus dataset: ![](./doc/slam_vbr.gif) - Localization on VBR ![](./doc/lm_loc_vbr_campus.gif) - Map on VBR - Point Cloud ![](./doc/campus_vbr.png) - Grid Map ![](./doc/campus.png) - Localization on the NCLT dataset ![](./doc/lm_loc1_nclt.gif) - Data on the Deep Robotics quadruped robot ![](./doc/demo_ysc1.png) ![](./doc/demo_ysc2.png) ![](./doc/demo_ysc3.png) - Tilted mounting demo ![](./doc/demo_github.png) ## Build ### Environment Ubuntu 22.04 or higher. Ubuntu 20.04 should also work, but not tested. ### Dependencies - ros2 humble or above - Pangolin (for visualization, see thirdparty) - OpenCV - PCL - yaml-cpp - glog - gflags - pcl_conversions On Ubuntu 22.04, run: ```bash ./scripts/install_dep.sh```. ### Build Build this package with ```colcon build```. Then ```source install/setup.bash``` to use it. ### Build Results After building, you will get the corresponding online/offline mapping and localization programs for this package. The offline programs are suitable for scenarios with offline data packets to quickly obtain mapping/localization results, while the online programs are suitable for scenarios with actual sensors to obtain real-time results. For example, calling the offline mapping program on the NCLT dataset: ```ros2 run lightning run_slam_offline --input_bag ~/data/NCLT/20130110/20130110.db3 --config ./config/default_nclt.yaml``` If you want to call the online version, just change the offline part to online. ## Testing on Datasets You can directly use our converted datasets. If you need the original datasets, you need to convert them to the ros2 db3 format. Converted dataset addresses: - OneDrive: https://1drv.ms/f/c/1a7361d22c554503/EpDSys0bWbxDhNGDYL_O0hUBa2OnhNRvNo2Gey2id7QMQA?e=7Ui0f5 - BaiduYun: https://pan.baidu.com/s/1XmFitUtnkKa2d0YtWquQXw?pwd=xehn 提取码: xehn Original dataset addresses: - NCLT dataset: http://robots.engin.umich.edu/nclt/ - UrbanLoco dataset: https://github.com/weisongwen/UrbanLoco - VBR dataset: https://www.rvp-group.net/slam-dataset.html ### Mapping Test 1. Real-time mapping (real-time bag playback) - Start the mapping program: ```ros2 run lightning run_slam_online --config ./config/default_nclt.yaml``` - Play the data bag - Save the map ```ros2 service call /lightning/save_map lightning/srv/SaveMap "{map_id: new_map}"``` 2. Offline mapping (traverse data, faster) - ```ros2 run lightning run_slam_offline --config ./config/default_nclt.yaml --input_bag [bag_file]``` - It will automatically save to the data/new_map directory after finishing. 3. Viewing the map - View the full map: ```pcl_viewer ./data/new_map/global.pcd``` - The actual map is stored in blocks, global.pcd is only for displaying the result. - map.pgm stores the 2D grid map information. - Note that during the localization program run or upon exit, results for dynamic layers might also be stored in the same directory, so there might be more files. ### Localization Test 1. Real-time localization - Write the map path to `system.map_path` in the yaml file, default is `new_map` (consistent with the mapping default). - Place the vehicle at the mapping starting point. - Start the localization program: ```ros2 run lightning run_loc_online --config ./config/default_nclt.yaml``` - Play the bag or input sensor data. 2. Offline localization - ```ros2 run lightning run_loc_offline --config ./config/default_nclt.yaml --input_bag [bag_file]``` 3. Receiving localization results - The localization program outputs TF topics at the same frequency as the IMU (50-100Hz). ### Debugging on Your Own Device First, you need to know your LiDAR type and set the corresponding `fasterlio.lidar_type`. Set it to 1 for Livox series, 2 for Velodyne, 3 for Ouster. If it's not one of the above types, you can refer to the Velodyne setup method. A simpler way is to first record a ros2 bag, get offline mapping and localization working, and then debug the online situation. You usually need to modify `common.lidar_topic` and `common.imu_topic` to set the LiDAR and IMU topics. The IMU and LiDAR extrinsic parameters can default to zero; we are not sensitive to them. The `fasterlio.time_scale` related to timestamps is sensitive. You should pay attention to whether the LiDAR point cloud has timestamps for each point and if they are calculated correctly. This code is in `core/lio/pointcloud_preprocess`. Refer to the next section for other parameter adjustments. ### Deep Robotics Quadruped Robot Repo: https://github.com/DeepRoboticsLab/lightning-lm-deep-robotics Video: [Embodied Intelligence Episode 3 | [Lynx M20] [SLAM] M20 RoboSense LiDAR Usage and Secondary Development, Using lightning-lm as an Example] https://www.bilibili.com/video/BV12YQZBqE1b?vd_source=57f46145c37bfb96f7583c9e02081590 ### Fine-tuning Lightning-LM You can fine-tune Lightning by modifying the configuration file, turning some features on or off. Common configuration items include: - `system.with_loop_closing` Whether loop closure detection is needed - `system.with_ui` Whether 3D UI is needed - `system.with_2dui` Whether 2D UI is needed - `system.with_g2p5` Whether grid map is needed - `system.map_path` Storage path for the map - `fasterlio.point_filter_num` Point sampling number. Increasing this results in fewer points, faster computation, but not recommended to set above 10. - `g2p5.esti_floor` Whether g2p5 needs to dynamically estimate ground parameters. If the LiDAR rotates horizontally and the height is constant, you can turn this option off. - `g2p5.grid_map_resolution` Resolution of the grid map ### TODO - [done] UI displays trajectory after loop closure - [done] Grid map saved in ROS-compatible format - [done] Check if grid map resolution values are normal - Force 2D output - Additional convenience features (turn localization on/off, reinitialize, specify location, etc.) ### Test Results 1. Mapping - NCLT: pass - VBR: pass - Livox Multi Floor: pass - GitHub issues: - Tilted 30 degrees https://github.com/gaoxiang12/lightning-lm/issues/75#issuecomment-4131131883 pass (need to disable IMU filter) - multi_floor multi-floor map: pass (can map but cannot loop close) - Outdoor only, elevated bridge - geely: pass - Deep Robotics (yunshenchu): - building1 multi-floor indoor/outdoor mixed: pass - building2: pass - building3: pass - grass: need to increase minimum height, e.g., above 0.5 - road1: same as above, pass 2. Localization ## Miscellaneous 1. Converting ros1 data to ros2 Install ``` pip install -i https://pypi.tuna.tsinghua.edu.cn/simple rosbags``` Convert: ```rosbags-convert --src [your_ROS1_bag_file.bag] --dst [output_ROS2_bag_directory]``` --- ## Star History [![Star History Chart](https://api.star-history.com/svg?repos=gaoxiang12/lightning-lm&type=date&legend=top-left)](https://www.star-history.com/#gaoxiang12/lightning-lm&type=date&legend=top-left) ================================================ FILE: README_CN.md ================================================ # Lightning-LM [English](./README.md) | 中文 Lightning-Speed Lidar Localization and Mapping Lightning-LM 是一个完整的激光建图+定位模块。 Lightning-LM特性： 1. [done] 完整的3D Lidar SLAM，快速的LIO前端（AA-FasterLIO），标配 2. [done] 3D至2D地图转换（g2p5），选配，选上的话会输出实时的2D栅格，可以保存 3. [done] 实时回环检测，标配，选上的话会进行后端回环检测并闭环 4. [done] 流畅的高精3D Lidar 定位，标配 5. [done] 地图分区动态加载方案，适用大场景 6. [done] 动静态图层分离定位，适配动态场景，可选择动态图层的策略，选配，选上的话会保存动态图层的地图内容，有三种策略可以选（短期、中期、永久），默认永久 7. [done] 高频率IMU平滑输出，标配，100Hz 8. GPS地理信息关联，选配 (TODO) 9. 车辆里程计输入，选配 (TODO) 10. [done] 轻量优化库miao以及增量式优化（来自g2o，但更轻更快，支持增量优化，不需要重新构建优化模型），标配，在回环、定位中均有用到 11. [done] 离线与在线两种验证方案。离线可以断点调试，一致性强。在线可以多线程并发，处理速度快，可以设置动态跳帧，占用低。 12. [done] 基于外推器和平滑器的高频率输出，平滑因子可调 13. [done] 高性能计算：以上这些特性在纯CPU端不到一个核心就可以运行（在线定位0.8个核，建图1.2个核，32线雷达，无UI情况） ## 更新 ### 2026.4.2 - 对建图和定位的稳定性进行了大量提升，已适配issue里提到的多层数据，云深处提供的多层数据，相关数据正在上传百度云，欢迎大家尝试！ - 调整了状态变量的结构和维度，现在ba, grav，offset_R, offset_t不用在线估计了，状态变量减到了12维（原本是23维） - 添加定位部分激光定位的修正量比例，现在会基于LIO预测来进行定位，防止激光跳变太大 - LaserMapping部分加上了点到点ICP误差,点到点的部分也使用多线程加速 - ESKF部分加了一些实用的trick - 定位现在会取LIO的关键帧来进行map to map配准 - 针对云深处的几个数据集和github issues里的数据集做了调参 - 调整了ESKF的接口以适应点到点ICP的情况（因为点到点ICP与点面ICP的维度不同） - 添加了一些Kalman filter里的tricks: 对P阵做对称化，保护最小值等 ### 2026.3.20 - MapIncremental在关键帧层面调用，增强LIO的鲁棒性（VBR数据集上不飘移） - 修正了高度约束的雅可比问题 issue#100 #110 - 修正回环检测模块的越界问题 issue#88 - 适配云深处四足机器人数据 (RoboSense的lidar type=4) - 增加了一些关于时间戳的数据检查(VBR存在异常时间戳问题） - 回环检测现在使用优化后的位姿作为检测的初值(在大回环时有用) - 如果voxel之后的点云数量太少，使用voxel之前的点云进行LIO（防止降采样之后点太少） - 修正了std::vector在并行化时的问题 - 修正了几处可能导致seg fault的问题 ### 2025.11.27 - 在LIO模块中添加了Cauchy's kernel - 在定位模块中增加了配置： try_self_extrap，默认关闭。也就是定位模块不会根据自身外推的位姿做定位（因为定位间隔较大，车辆运动较大时不准）。 - 添加了一个livox的配置文件，因为用livox的人比较多 - 如果建图时设置了固定高度，那么定位也会使用这个地图高度（默认关闭） ### 2025.11.13 - 修复了FasterLIO中的两个逻辑问题 - 增加了高度约束，在loop_closing.with_height中配置。配置高度约束以后，lightning会保障输出地图的水平度（限制Z轴飘移），但这样就不适用于多层室内之类的带有立体结构的场景。 ## 案例 - VBR campus数据集上的建图： ![](./doc/slam_vbr.gif) - VBR上的定位 ![](./doc/lm_loc_vbr_campus.gif) - VBR上的地图 - 点云 ![](./doc/campus_vbr.png) - 栅格 ![](./doc/campus.png) - NCLT 数据集上的定位 ![](./doc/lm_loc1_nclt.gif) - 云深处四足机械狗上的数据 ![](./doc/demo_ysc1.png) ![](./doc/demo_ysc2.png) ![](./doc/demo_ysc3.png) - 斜装的DEMO ![](./doc/demo_github.png) ## 编译 ### 环境 Ubuntu 22.04 或更高版本。 Ubuntu 20.04 应该也可行，未测试。 ### 依赖 - ros2 humble 及以上 - Pangolin（用于可视化，见thirdparty） - OpenCV - PCL - yaml-cpp - glog - gflags - pcl_conversions 在Ubuntu 22.04上，执行：```bash ./scripts/install_dep.sh```即可。 ### 编译 ```colcon build```本包即可。 然后```source install/setup.bash```即可使用。 ### 编译结果 编译后，会得到本包对应的在线/离线建图程序与定位程序。离线程序适用于存在离线数据包，快速得到建图/定位结果的方案，在线程序则适用于有实际传感器，得到实时结果的方案。 例如：在NCLT数据集上调用离线建图程序: ```ros2 run lightning run_slam_offline --input_bag ~/data/NCLT/20130110/20130110.db3 --config ./config/default_nclt.yaml``` 如果希望调用在线的版本，则将offline部分改成online即可。 ## 在数据集上测试 您可以直接使用我们转换完的数据集。如果需要原始的数据集，您需要将它们转换到ros2的db3格式。 转换后的数据集地址： - OneDrive: https://1drv.ms/f/c/1a7361d22c554503/EpDSys0bWbxDhNGDYL_O0hUBa2OnhNRvNo2Gey2id7QMQA?e=7Ui0f5 - BaiduYun: https://pan.baidu.com/s/1XmFitUtnkKa2d0YtWquQXw?pwd=xehn 提取码: xehn 原始数据集地址： - NCLT 数据集：http://robots.engin.umich.edu/nclt/ - UrbanLoco 数据集： https://github.com/weisongwen/UrbanLoco - VBR 数据集：https://www.rvp-group.net/slam-dataset.html ### 建图测试 1. 实时建图（实时播包） - 启动建图程序: ```ros2 run lightning run_slam_online --config ./config/default_nclt.yaml``` - 播放数据包 - 保存地图 ```ros2 service call /lightning/save_map lightning/srv/SaveMap "{map_id: new_map}"``` 2. 离线建图（遍历跑数据，更快一些） - ```ros2 run lightning run_slam_offline --config ./config/default_nclt.yaml --input_bag 数据包``` - 结束后会自动保存至data/new_map目录下 3. 查看地图 - 查看完整地图：```pcl_viewer ./data/new_map/global.pcd``` - 实际地图是分块存储的，global.pcd仅用于显示结果 - map.pgm存储了2D栅格地图信息 - 请注意，在定位程序运行过程中或退出时，也可能在同目录存储动态图层的结果，所以文件可能会有更多。 ### 定位测试 1. 实时定位 - 将地图路径写到yaml中的 system-map_path 下，默认是new_map（和建图默认一致) - 将车放在建图起点处 - 启动定位程序： ```ros2 run lightning run_loc_online --config ./config/default_nclt.yaml``` - 播包或者输入传感器数据即可 2. 离线定位 - ```ros2 run lightning run_loc_offline --config ./config/default_nclt.yaml --input_bag 数据包``` 3. 接收定位结果 - 定位程序输出与IMU同频的TF话题（50-100Hz） ### 在您自己的设备上调试 首先您需要知道自己的雷达类型，设置对应的fasterlio.lidar_type类型。livox系列的配置成1，Velodyne的设置成2,ouster设置成3. 如果不在以上种类，可以参考velodyne的设置方式。 比较简单的方式是先录一个ros2的数据包，将离线的建图、定位调通后，再去调试在线的情况。 您通常需要修改common.lidar_topic和common.imu_topic来设置雷达与imu的话题。 imu和雷达外参默认为零就好，我们对这个不敏感。 时间戳相关的fasterlio.time_scale是敏感的。您最好关注一下雷达点云是否带有每个点的时间戳，以及它们是否计算正确。这些代码在core/lio/pointcloud_preprocess里. 其他参数调整参考下一节。 ### 云深处四足机器人 repo在这里：https://github.com/DeepRoboticsLab/lightning-lm-deep-robotics 视频：【具身智能第三期 | [Lynx M20] [SLAM] M20速腾雷达使用与二次开发，以复现lightning-lm为例】https://www.bilibili.com/video/BV12YQZBqE1b?vd_source=57f46145c37bfb96f7583c9e02081590 ### 对lightning-lm进行微调 您可以在配置文件中对lightning进行微调，打开或者关闭一些功能。常见的配置项有： - system.with_loop_closing 是否需要回环检测 - system.with_ui 是否需要3DUI - system.with_2dui 是否需要2DUI - system.with_g2p5 是否需要栅格地图 - system.map_path 地图的存储路径 - fasterlio.point_filter_num 点的采样数。调大后点数会少一些，计算更快，但不建议调到10以上。 - g2p5.esti_floor g2p5是否需要动态估计地面参数。如果雷达水平旋转且高度不变，可以关闭此选项. - g2p5.grid_map_resolution 栅格地图的分辨率 ### TODO - [done] UI显示闭环后轨迹 - [done] 栅格地图保存为兼容ROS的形式 - [done] 检查栅格地图的分辨率取值是否正常 - 强制2D输出 - 额外便利性功能（打开关闭定位，重新初始化，指定位置等） ### 测试情况 1. 建图 - NCLT: pass - VBR: pass - Livox Multi Floor: pass - github: - 斜装30度 https://github.com/gaoxiang12/lightning-lm/issues/75#issuecomment-4131131883 pass 需要关掉IMU filter - multi_floor 多层地图 pass 可以建图但是没法闭环 - 纯室外 高架桥 - geely: pass - yunshenchu - building1 多层室内外混合 pass - building2 pass - building3 pass - grass 需要把最小高度设高一些 例如0.5以上 - road1 同上 pass 2. 定位 ## 其他 1. 将ros1数据转换至ros2 安装 ``` pip install -i https://pypi.tuna.tsinghua.edu.cn/simple rosbags``` 转换: ```rosbags-convert --src [你的ROS1_bag文件.bag] --dst [输出ROS2bag目录]``` ## Star History [![Star History Chart](https://api.star-history.com/svg?repos=gaoxiang12/lightning-lm&type=date&legend=top-left)](https://www.star-history.com/#gaoxiang12/lightning-lm&type=date&legend=top-left) ================================================ FILE: cmake/packages.cmake ================================================ find_package(glog REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) find_package(yaml-cpp REQUIRED) find_package(Pangolin REQUIRED) find_package(OpenGL REQUIRED) find_package(pcl_conversions REQUIRED) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(OpenCV REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(rosbag2_cpp REQUIRED) find_package(rosidl_default_generators REQUIRED) # OMP find_package(OpenMP) if (OPENMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif () if (BUILD_WITH_MARCH_NATIVE) add_compile_options(-march=native) else () add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") endif () include_directories( ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS} ${GLEW_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} ${pcl_conversions_INCLUDR_DIRS} ${rclcpp_INCLUDE_DIRS} ${rosbag2_cpp_INCLUDE_DIRS} ${nav_msgs_INCLUDE_DIRS} ) include_directories( ${CMAKE_CURRENT_BINARY_DIR}/thirdparty/livox_ros_driver/rosidl_generator_cpp ) include_directories( ${PROJECT_SOURCE_DIR}/src ${PROJECT_SOURCE_DIR}/thirdparty ) set(third_party_libs ${PCL_LIBRARIES} ${OpenCV_LIBS} ${Pangolin_LIBRARIES} glog gflags ${yaml-cpp_LIBRARIES} ${pcl_conversions_LIBRARIES} tbb ${rosbag2_cpp_LIBRARIES} ) ================================================ FILE: config/default.yaml ================================================ # 针对NCLT的配置 common: dataset: "nclt" lidar_topic: "points_raw" livox_lidar_topic: "/livox/lidar" imu_topic: "/livox/imu" # fasterlio 参数 fasterlio: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 blind: 0.1 # 盲区，小于盲区距离的不会处理 time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效 skip_lidar_num: 0 # 点云的跳帧情况 point_filter_num: 4 # 点云采样参数 max_iteration: 4 # 最大迭代次数 filter_size_scan: 0.5 # 对scan进行的降采样 filter_size_map: 0.5 ivox_grid_resolution: 0.5 # default=0.2 ivox_nearby_type: 18 # 6, 18, 26 esti_plane_threshold: 0.1 # default=0.1 use_aa: false # use anderson-acceleration acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ 0, 0, 0.28 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] keep_first_imu_estimation: false kf_dis_th: 1.0 kf_angle_th: 10.0 enable_icp_part: false plane_icp_weight: 300.0 min_pts: 500 imu_filter: false proj_kfs: false g2p5: esti_floor: false # 是否动态估计地面 lidar_height: 0.0 # 雷达安装高度 floor_height: -1.2 # 默认地面高度 min_th_floor: 0.50 # 距离地面这个高度的障碍物被扫入栅格地图中 max_th_floor: 1.0 # 距离地面这个高度的障碍物被扫入栅格地图中 grid_map_resolution: 0.2 # 栅格地图分辨率 system: with_loop_closing: true # 是否需要回环检测 with_ui: true # 是否需要3D UI with_2dui: false # 是否需要2D UI with_g2p5: true # 是否需要栅格地图 step_on_kf: false # 是否在关键帧处暂停 enable_lidar_loc_skip: false enable_lidar_loc_rviz: false enable_lidar_odom_skip: false evaluation: false lidar_loc_skip_num: 4 lidar_odom_skip_num: 2 map_path: ./data/new_map/ pub_tf: true loop_closing: loop_kf_gap: 20 # 每隔多少个关键帧检查一次 min_id_interval: 20 # 被检查的关键帧ID间隔 closest_id_th: 50 # 历史关键帧与当前帧的ID间隔 max_range: 20.0 # 候选帧的最大距离 ndt_score_th: 1.3 # ndt位姿分值 with_height: false ### localization ================================================================================================ lidar_loc: enable_dynamic_polygon: false enable_parking_static: true enable_icp_adjust: false filter_intensity_max: 100.0 filter_intensity_min: 10.0 filter_z_max: 30.0 filter_z_min: -1.0 force_2d: false grid_search_angle_range: 180.0 grid_search_angle_step: 60 lidar_loc_odom_th: 0.5 init_with_fp: true min_init_confidence: 1.8 update_dynamic_cloud: false update_err_h: 0.2 update_err_v: 0.2 update_kf_dis: 2.0 update_lidar_loc_score: 2.5 loc_on_kf: true try_self_extrap: false maps: delete_when_unload: false dyn_cloud_policy: persistent load_dyn_cloud: true load_map_size: 2 max_pts_dyn_chunk: 100000 save_dyn_when_quit: true save_dyn_when_unload: false unload_map_size: 4 with_dyn_area: false pgo: dr_ang_noise: 0.5 dr_pos_noise: 1.0 dr_pos_noise_ratio: 1.0 lidar_loc_ang_noise: 1.0 lidar_loc_outlier_th: 5.0 lidar_loc_pos_noise: 0.3 lidar_odom_ang_noise: 1.0 lidar_odom_pos_noise: 0.3 pgo_frame_converge_ang_th: 1.0 pgo_frame_converge_pos_th: 0.05 rtk_fix_ang_noise: 2.0 rtk_fix_pos_noise: 1.0 rtk_height_noise_ratio: 10.0 rtk_other_ang_noise: 10.0 rtk_other_pos_noise: 5.0 rtk_outlier_th: 1.0 smooth_factor: 0.01 roi: height_max: 10.0 height_min: 0.5 range_max: 70 range_min: 3.0 ui: car_length: 2.0 car_width: 1.6 car_height: 1.7 scans: 1 ================================================ FILE: config/default_livox.yaml ================================================ # 针对NCLT的配置 common: dataset: "nclt" lidar_topic: "points_raw" livox_lidar_topic: "/livox/lidar" imu_topic: "/livox/imu" # fasterlio 参数 fasterlio: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 blind: 0.1 # 盲区，小于盲区距离的不会处理 time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效 skip_lidar_num: 0 # 点云的跳帧情况 point_filter_num: 4 # 点云采样参数 max_iteration: 4 # 最大迭代次数 filter_size_scan: 0.5 # 对scan进行的降采样 filter_size_map: 0.5 ivox_grid_resolution: 0.5 # default=0.2 ivox_nearby_type: 18 # 6, 18, 26 esti_plane_threshold: 0.1 # default=0.1 use_aa: false # use anderson-acceleration acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ 0, 0, 0.28 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] keep_first_imu_estimation: false kf_dis_th: 1.0 kf_angle_th: 10.0 enable_icp_part: false plane_icp_weight: 300.0 min_pts: 500 imu_filter: false proj_kfs: false g2p5: esti_floor: false # 是否动态估计地面 lidar_height: 0.0 # 雷达安装高度 floor_height: -1.2 # 默认地面高度 min_th_floor: 0.50 # 距离地面这个高度的障碍物被扫入栅格地图中 max_th_floor: 1.0 # 距离地面这个高度的障碍物被扫入栅格地图中 grid_map_resolution: 0.2 # 栅格地图分辨率 system: with_loop_closing: true # 是否需要回环检测 with_ui: true # 是否需要3D UI with_2dui: false # 是否需要2D UI with_g2p5: true # 是否需要栅格地图 step_on_kf: false # 是否在关键帧处暂停 enable_lidar_loc_skip: false enable_lidar_loc_rviz: false enable_lidar_odom_skip: false evaluation: false lidar_loc_skip_num: 4 lidar_odom_skip_num: 2 map_path: ./data/new_map/ pub_tf: true loop_closing: loop_kf_gap: 20 # 每隔多少个关键帧检查一次 min_id_interval: 20 # 被检查的关键帧ID间隔 closest_id_th: 50 # 历史关键帧与当前帧的ID间隔 max_range: 20.0 # 候选帧的最大距离 ndt_score_th: 1.3 # ndt位姿分值 with_height: false ### localization ================================================================================================ lidar_loc: enable_dynamic_polygon: false enable_parking_static: true enable_icp_adjust: false filter_intensity_max: 100.0 filter_intensity_min: 10.0 filter_z_max: 30.0 filter_z_min: -1.0 force_2d: false grid_search_angle_range: 180.0 grid_search_angle_step: 60 lidar_loc_odom_th: 0.5 init_with_fp: true min_init_confidence: 1.8 update_dynamic_cloud: false update_err_h: 0.2 update_err_v: 0.2 update_kf_dis: 2.0 update_lidar_loc_score: 2.5 loc_on_kf: true try_self_extrap: false maps: delete_when_unload: false dyn_cloud_policy: persistent load_dyn_cloud: true load_map_size: 2 max_pts_dyn_chunk: 100000 save_dyn_when_quit: true save_dyn_when_unload: false unload_map_size: 4 with_dyn_area: false pgo: dr_ang_noise: 0.5 dr_pos_noise: 1.0 dr_pos_noise_ratio: 1.0 lidar_loc_ang_noise: 1.0 lidar_loc_outlier_th: 5.0 lidar_loc_pos_noise: 0.3 lidar_odom_ang_noise: 1.0 lidar_odom_pos_noise: 0.3 pgo_frame_converge_ang_th: 1.0 pgo_frame_converge_pos_th: 0.05 rtk_fix_ang_noise: 2.0 rtk_fix_pos_noise: 1.0 rtk_height_noise_ratio: 10.0 rtk_other_ang_noise: 10.0 rtk_other_pos_noise: 5.0 rtk_outlier_th: 1.0 smooth_factor: 0.01 roi: height_max: 10.0 height_min: 0.5 range_max: 70 range_min: 3.0 ui: car_length: 2.0 car_width: 1.6 car_height: 1.7 scans: 1 ================================================ FILE: config/default_nclt.yaml ================================================ # 针对NCLT的配置 common: dataset: "nclt" lidar_topic: "points_raw" livox_lidar_topic: "/livox/lidar" imu_topic: "imu_raw" # fasterlio 参数 fasterlio: lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 blind: 0.5 # 盲区，小于盲区距离的不会处理 time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效 skip_lidar_num: 0 # 点云的跳帧情况 point_filter_num: 6 # 点云采样参数 max_iteration: 4 # 最大迭代次数 filter_size_scan: 0.5 # 对scan进行的降采样 filter_size_map: 0.5 ivox_grid_resolution: 0.5 # default=0.2 ivox_nearby_type: 18 # 6, 18, 26 esti_plane_threshold: 0.1 # default=0.1 use_aa: false # use anderson-acceleration acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ 0, 0, 0.0 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] keep_first_imu_estimation: false kf_dis_th: 2.0 kf_angle_th: 5.0 enable_icp_part: false plane_icp_weight: 500.0 min_pts: 300 imu_filter: true proj_kfs: false g2p5: esti_floor: false # 是否动态估计地面 lidar_height: 0.0 # 雷达安装高度 floor_height: -1.2 # 默认地面高度 min_th_floor: 0.50 # 距离地面这个高度的障碍物被扫入栅格地图中 max_th_floor: 1.0 # 距离地面这个高度的障碍物被扫入栅格地图中 grid_map_resolution: 0.2 # 栅格地图分辨率 system: with_loop_closing: true # 是否需要回环检测 with_ui: true # 是否需要3D UI with_2dui: false # 是否需要2D UI with_g2p5: false # 是否需要栅格地图 step_on_kf: false # 是否在关键帧处暂停 enable_lidar_loc_skip: false enable_lidar_loc_rviz: false enable_lidar_odom_skip: false lidar_loc_skip_num: 4 lidar_odom_skip_num: 2 map_path: ./data/new_map/ pub_tf: true loop_closing: loop_kf_gap: 50 # 每隔多少个关键帧检查一次 min_id_interval: 20 # 被检查的关键帧ID间隔 closest_id_th: 50 # 历史关键帧与当前帧的ID间隔 max_range: 30.0 # 候选帧的最大距离 ndt_score_th: 1.0 # ndt位姿分值 with_height: true ### localization ================================================================================================ lidar_loc: enable_dynamic_polygon: false enable_parking_static: true enable_icp_adjust: false filter_intensity_max: 100.0 filter_intensity_min: 10.0 filter_z_max: 30.0 filter_z_min: -1.0 force_2d: false grid_search_angle_range: 180.0 grid_search_angle_step: 60 lidar_loc_odom_th: 0.5 init_with_fp: true min_init_confidence: 1.8 update_dynamic_cloud: true update_err_h: 0.2 update_err_v: 0.2 update_kf_dis: 2.0 update_lidar_loc_score: 2.5 loc_on_kf: true try_self_extrap: false maps: delete_when_unload: false dyn_cloud_policy: persistent load_dyn_cloud: true load_map_size: 2 max_pts_dyn_chunk: 100000 save_dyn_when_quit: true save_dyn_when_unload: false unload_map_size: 4 with_dyn_area: false pgo: dr_ang_noise: 0.5 dr_pos_noise: 1.0 dr_pos_noise_ratio: 1.0 lidar_loc_ang_noise: 1.0 lidar_loc_outlier_th: 5.0 lidar_loc_pos_noise: 0.3 lidar_odom_ang_noise: 1.0 lidar_odom_pos_noise: 0.3 pgo_frame_converge_ang_th: 1.0 pgo_frame_converge_pos_th: 0.05 rtk_fix_ang_noise: 2.0 rtk_fix_pos_noise: 1.0 rtk_height_noise_ratio: 10.0 rtk_other_ang_noise: 10.0 rtk_other_pos_noise: 5.0 rtk_outlier_th: 1.0 smooth_factor: 0.01 roi: height_max: 5.0 height_min: -2.0 range_max: 70 range_min: 3.0 ui: car_length: 2.0 car_width: 1.6 car_height: 1.7 scans: 1 ================================================ FILE: config/default_robosense.yaml ================================================ # 针对NCLT的配置 common: dataset: "robosense" lidar_topic: "/LIDAR/POINTS" livox_lidar_topic: "/livox/lidar" imu_topic: "/IMU" # fasterlio 参数 fasterlio: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for robosense scan_line: 32 blind: 0.5 # 盲区，小于盲区距离的不会处理 time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效 skip_lidar_num: 0 # 点云的跳帧情况 point_filter_num: 6 # 点云采样参数 max_iteration: 4 # 最大迭代次数 filter_size_scan: 0.5 # 对scan进行的降采样 filter_size_map: 0.5 ivox_grid_resolution: 0.5 # default=0.2 ivox_nearby_type: 18 # 6, 18, 26 esti_plane_threshold: 0.1 # default=0.1 use_aa: false # use anderson-acceleration acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ 0, 0, 0 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] keep_first_imu_estimation: false kf_dis_th: 1.0 kf_angle_th: 2.0 enable_icp_part: true kalman_R: 0.1 min_pts: 300 imu_filter: true g2p5: esti_floor: false # 是否动态估计地面 lidar_height: 0.0 # 雷达安装高度 floor_height: -1.2 # 默认地面高度 min_th_floor: 0.50 # 距离地面这个高度的障碍物被扫入栅格地图中 max_th_floor: 1.0 # 距离地面这个高度的障碍物被扫入栅格地图中 grid_map_resolution: 0.2 # 栅格地图分辨率 system: with_loop_closing: true # 是否需要回环检测 with_ui: true # 是否需要3D UI with_2dui: true # 是否需要2D UI with_g2p5: true # 是否需要栅格地图 step_on_kf: true # 是否在关键帧处暂停 enable_lidar_loc_skip: false enable_lidar_loc_rviz: false enable_lidar_odom_skip: false lidar_loc_skip_num: 4 lidar_odom_skip_num: 0 map_path: ./data/new_map/ pub_tf: true loop_closing: loop_kf_gap: 20 # 每隔多少个关键帧检查一次 min_id_interval: 20 # 被检查的关键帧ID间隔 closest_id_th: 50 # 历史关键帧与当前帧的ID间隔 max_range: 30.0 # 候选帧的最大距离 ndt_score_th: 1.0 # ndt位姿分值 with_height: true ### localization ================================================================================================ lidar_loc: enable_dynamic_polygon: false enable_parking_static: true enable_icp_adjust: false filter_intensity_max: 100.0 filter_intensity_min: 10.0 filter_z_max: 30.0 filter_z_min: -1.0 force_2d: false grid_search_angle_range: 90.0 grid_search_angle_step: 10 lidar_loc_odom_th: 0.5 init_with_fp: true min_init_confidence: 0.8 update_dynamic_cloud: true update_err_h: 0.2 update_err_v: 0.2 update_kf_dis: 2.0 update_lidar_loc_score: 2.5 loc_on_kf: true try_self_extrap: false maps: delete_when_unload: false dyn_cloud_policy: persistent load_dyn_cloud: true load_map_size: 2 max_pts_dyn_chunk: 100000 save_dyn_when_quit: true save_dyn_when_unload: false unload_map_size: 4 with_dyn_area: false pgo: dr_ang_noise: 0.5 dr_pos_noise: 1.0 dr_pos_noise_ratio: 1.0 lidar_loc_ang_noise: 1.0 lidar_loc_outlier_th: 5.0 lidar_loc_pos_noise: 0.3 lidar_odom_ang_noise: 1.0 lidar_odom_pos_noise: 0.3 pgo_frame_converge_ang_th: 1.0 pgo_frame_converge_pos_th: 0.05 rtk_fix_ang_noise: 2.0 rtk_fix_pos_noise: 1.0 rtk_height_noise_ratio: 10.0 rtk_other_ang_noise: 10.0 rtk_other_pos_noise: 5.0 rtk_outlier_th: 1.0 smooth_factor: 0.01 roi: height_max: 10.0 height_min: -10.0 range_max: 70 range_min: 3.0 ui: car_length: 2.0 car_width: 1.6 car_height: 1.7 scans: 1 ================================================ FILE: config/default_utbm.yaml ================================================ # 针对utbm的配置 common: dataset: "utbm" lidar_topic: "/velodyne_points" livox_lidar_topic: "/livox/lidar" imu_topic: "/imu/data" # fasterlio 参数 fasterlio: lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 blind: 0.5 # 盲区，小于盲区距离的不会处理 time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效 skip_lidar_num: 0 # 点云的跳帧情况 point_filter_num: 10 # 点云采样参数 max_iteration: 6 # 最大迭代次数 filter_size_scan: 0.5 # 对scan进行的降采样 filter_size_map: 0.5 ivox_grid_resolution: 0.5 # default=0.2 ivox_nearby_type: 18 # 6, 18, 26 esti_plane_threshold: 0.1 # default=0.1 use_aa: true # use anderson-acceleration acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ -0.5, 1.4, 1.5 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] kf_dis_th: 1.0 kf_angle_th: 5.0 enable_icp_part: true kalman_R: 0.1 min_pts: 300 g2p5: esti_floor: false # 是否动态估计地面 lidar_height: 0.0 # 雷达安装高度 min_th_floor: 0.10 # 距离地面这个高度的障碍物被扫入栅格地图中 max_th_floor: 1.24 # 距离地面这个高度的障碍物被扫入栅格地图中 grid_map_resolution: 0.1 # 栅格地图分辨率 system: with_loop_closing: true # 是否需要回环检测 with_ui: true # 是否需要3D UI with_2dui: false # 是否需要2D UI with_g2p5: true # 是否需要栅格地图 step_on_kf: false # 是否在关键帧处暂停 with_lidar_loc: true with_lo: true with_pgo: true enable_lidar_loc_skip: true enable_lidar_loc_rviz: false enable_lidar_odom_skip: false lidar_loc_skip_num: 4 lidar_odom_skip_num: 2 map_path: ./data/new_map/ pub_tf: true loop_closing: loop_kf_gap: 20 # 每隔多少个关键帧检查一次 min_id_interval: 20 # 被检查的关键帧ID间隔 closest_id_th: 50 # 历史关键帧与当前帧的ID间隔 max_range: 30.0 # 候选帧的最大距离 ndt_score_th: 1.0 # ndt位姿分值 with_height: true ### localization ================================================================================================ lidar_loc: enable_dynamic_polygon: false enable_parking_static: true enable_icp_adjust: false filter_intensity_max: 100.0 filter_intensity_min: 10.0 filter_z_max: 30.0 filter_z_min: -1.0 force_2d: false grid_search_angle_range: 180.0 grid_search_angle_step: 60 lidar_loc_odom_th: 0.5 init_with_fp: true min_init_confidence: 1.8 update_dynamic_cloud: true update_err_h: 0.2 update_err_v: 0.2 update_kf_dis: 2.0 update_lidar_loc_score: 2.5 loc_on_kf: true try_self_extrap: false maps: delete_when_unload: false dyn_cloud_policy: persistent load_dyn_cloud: true load_map_size: 2 max_pts_dyn_chunk: 100000 save_dyn_when_quit: true save_dyn_when_unload: false unload_map_size: 4 with_dyn_area: false pgo: dr_ang_noise: 0.5 dr_pos_noise: 1.0 dr_pos_noise_ratio: 1.0 lidar_loc_ang_noise: 1.0 lidar_loc_outlier_th: 5.0 lidar_loc_pos_noise: 0.3 lidar_odom_ang_noise: 1.0 lidar_odom_pos_noise: 0.3 pgo_frame_converge_ang_th: 1.0 pgo_frame_converge_pos_th: 0.05 rtk_fix_ang_noise: 2.0 rtk_fix_pos_noise: 1.0 rtk_height_noise_ratio: 10.0 rtk_other_ang_noise: 10.0 rtk_other_pos_noise: 5.0 rtk_outlier_th: 1.0 smooth_factor: 0.01 roi: height_max: 0.5 height_min: -2.0 range_max: 70 range_min: 3.0 sdr: esti_bias_when_parking: true imu_dt_expected: 0.05 imu_dt_max: 0.2 imu_dt_min: 0.01 wheel_dt_effective: 0.5 enable_speed_dead_zone: true speed_dead_zone: 0.1 ui: car_length: 2.0 car_width: 1.6 car_height: 1.7 scans: 1 ================================================ FILE: config/default_vbr.yaml ================================================ # 针对NCLT的配置 common: dataset: "vbr" lidar_topic: "/ouster/points" livox_lidar_topic: "/livox/lidar" imu_topic: "/imu/data" # fasterlio 参数 fasterlio: lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, scan_line: 32 blind: 0.5 # 盲区，小于盲区距离的不会处理 time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效 skip_lidar_num: 0 # 点云的跳帧情况 point_filter_num: 10 # 点云采样参数 max_iteration: 6 # 最大迭代次数 filter_size_scan: 0.5 # 对scan进行的降采样 filter_size_map: 0.5 ivox_grid_resolution: 0.5 # default=0.2 ivox_nearby_type: 18 # 6, 18, 26 esti_plane_threshold: 0.1 # default=0.1 use_aa: false # use anderson-acceleration acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ 0, 0, 0 ] extrinsic_R: [ 1, 0, 0, 0, 1, 0, 0, 0, 1 ] keep_first_imu_estimation: false kf_dis_th: 1.0 kf_angle_th: 5.0 enable_icp_part: false plane_icp_weight: 500.0 min_pts: 500 imu_filter: false proj_kfs: false g2p5: esti_floor: false # 是否动态估计地面 lidar_height: 0.0 # 雷达安装高度 floor_height: -1.2 # 默认地面高度 min_th_floor: 0.50 # 距离地面这个高度的障碍物被扫入栅格地图中 max_th_floor: 1.0 # 距离地面这个高度的障碍物被扫入栅格地图中 grid_map_resolution: 0.2 # 栅格地图分辨率 system: with_loop_closing: true # 是否需要回环检测 with_ui: true # 是否需要3D UI with_2dui: false # 是否需要2D UI with_g2p5: true # 是否需要栅格地图 step_on_kf: false # 是否在关键帧处暂停 enable_lidar_loc_skip: false enable_lidar_loc_rviz: false enable_lidar_odom_skip: false lidar_loc_skip_num: 4 lidar_odom_skip_num: 0 map_path: ./data/new_map/ pub_tf: true loop_closing: loop_kf_gap: 20 # 每隔多少个关键帧检查一次 min_id_interval: 20 # 被检查的关键帧ID间隔 closest_id_th: 50 # 历史关键帧与当前帧的ID间隔 max_range: 30.0 # 候选帧的最大距离 ndt_score_th: 1.0 # ndt位姿分值 with_height: true ### localization ================================================================================================ lidar_loc: enable_dynamic_polygon: false enable_parking_static: true enable_icp_adjust: false filter_intensity_max: 100.0 filter_intensity_min: 10.0 filter_z_max: 30.0 filter_z_min: -1.0 force_2d: false grid_search_angle_range: 180.0 grid_search_angle_step: 60 lidar_loc_odom_th: 0.5 init_with_fp: true min_init_confidence: 0.8 update_dynamic_cloud: true update_err_h: 0.2 update_err_v: 0.2 update_kf_dis: 2.0 update_lidar_loc_score: 2.5 loc_on_kf: true try_self_extrap: false maps: delete_when_unload: false dyn_cloud_policy: persistent load_dyn_cloud: true load_map_size: 2 max_pts_dyn_chunk: 100000 save_dyn_when_quit: true save_dyn_when_unload: false unload_map_size: 4 with_dyn_area: false pgo: dr_ang_noise: 0.5 dr_pos_noise: 1.0 dr_pos_noise_ratio: 1.0 lidar_loc_ang_noise: 1.0 lidar_loc_outlier_th: 5.0 lidar_loc_pos_noise: 0.3 lidar_odom_ang_noise: 1.0 lidar_odom_pos_noise: 0.3 pgo_frame_converge_ang_th: 1.0 pgo_frame_converge_pos_th: 0.05 rtk_fix_ang_noise: 2.0 rtk_fix_pos_noise: 1.0 rtk_height_noise_ratio: 10.0 rtk_other_ang_noise: 10.0 rtk_other_pos_noise: 5.0 rtk_outlier_th: 1.0 smooth_factor: 0.01 roi: height_max: 0.5 height_min: -2.0 range_max: 70 range_min: 3.0 ui: car_length: 2.0 car_width: 1.6 car_height: 1.7 scans: 1 ================================================ FILE: doc/g2p5_vbr.pgm ================================================ P5 2272 2528 255 ================================================ FILE: docker/Dockerfile ================================================ FROM ros:humble-perception LABEL maintainer="pengfei.guo2001@gmail.com" LABEL description="A docker for lightning-lm" # 环境变量 ENV ROS_WS=/root/slam_ws RUN printf "deb https://mirrors.ustc.edu.cn/ubuntu/ jammy main restricted universe multiverse\n\ deb-src https://mirrors.ustc.edu.cn/ubuntu/ jammy main restricted universe multiverse\n\ deb https://mirrors.ustc.edu.cn/ubuntu/ jammy-updates main restricted universe multiverse\n\ deb-src https://mirrors.ustc.edu.cn/ubuntu/ jammy-updates main restricted universe multiverse\n\ deb https://mirrors.ustc.edu.cn/ubuntu/ jammy-backports main restricted universe multiverse\n\ deb-src https://mirrors.ustc.edu.cn/ubuntu/ jammy-backports main restricted universe multiverse\n\ deb https://mirrors.ustc.edu.cn/ubuntu/ jammy-security main restricted universe multiverse\n\ deb-src https://mirrors.ustc.edu.cn/ubuntu/ jammy-security main restricted universe multiverse\n\ deb http://archive.canonical.com/ubuntu/ jammy partner\n\ deb-src http://archive.canonical.com/ubuntu/ jammy partner\n" > /etc/apt/sources.list && \ \ # 现在使用新源更新 apt-get update && \ rm -rf /var/lib/apt/lists/* RUN apt update && apt install -y \ cmake \ make \ g++ \ gcc \ libssl-dev \ libboost-all-dev \ libboost-dev \ libeigen3-dev \ libpcl-dev \ libgoogle-glog-dev \ libopencv-dev \ libyaml-cpp-dev \ libgflags-dev \ pcl-tools \ liboctomap-dev \ libassimp-dev \ liburdfdom-dev \ libepoxy-dev \ doxygen \ python3-pip \ python3-wheel \ python3-setuptools \ ros-${ROS_DISTRO}-pcl-conversions \ ros-${ROS_DISTRO}-pcl-ros \ ros-${ROS_DISTRO}-octomap \ ros-${ROS_DISTRO}-pinocchio \ zip \ && rm -rf /var/lib/apt/lists/* # 安装hpp-fcl库 WORKDIR /tmp RUN git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git && \ cd hpp-fcl && \ mkdir build && cd build && \ cmake .. \ -DCMAKE_BUILD_TYPE=Release \ -DCMAKE_INSTALL_PREFIX=/usr/local \ -DBUILD_WITH_COLLISION_SUPPORT=ON \ -DBUILD_PYTHON_INTERFACE=OFF && \ make && \ make install && \ cd / && \ rm -rf /tmp/hpp-fcl # 安装Pangolin库 WORKDIR /tmp RUN git clone --recurse-submodules https://github.com/stevenlovegrove/Pangolin.git && \ cd Pangolin && \ mkdir build && cd build && \ cmake .. \ -DCMAKE_BUILD_TYPE=Release \ -DCMAKE_INSTALL_PREFIX=/usr/local \ -DBUILD_WITH_COLLISION_SUPPORT=ON \ -DBUILD_PYTHON_INTERFACE=OFF && \ make && \ make install && \ cd / && \ rm -rf /tmp/Pangolin # 创建 ROS 工作空间 WORKDIR ${ROS_WS} # 一键式构建命令 RUN /bin/bash -c "mkdir -p src && \ source /opt/ros/${ROS_DISTRO}/setup.bash && \ git clone https://github.com/gaoxiang12/lightning-lm.git src/lightning-lm&& \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ # 将source命令写入bashrc（持久化配置） echo 'source /${ROS_WS}/install/setup.bash' >> ~/.bashrc && \ echo 'source /opt/ros/${ROS_DISTRO}/setup.bash' >> ~/.bashrc" ================================================ FILE: docker/README.md ================================================ # lightning-lm Docker 这是为 lightning-lm 项目准备的 Docker 指南，分为构建和运行两部分 ## 构建 可以在本地构建或者在云端构建然后拉取，如果需要在本地构建，则在安装好docker环境之后执行 ```bash cd docker docker build -t lighting_lm_image . ``` 经过若干分钟的构建（这个过程会持续半个小时以上，甚至有失败的可能），就可以完成在本地的镜像构建，当然您也可以自行指定构建出的镜像的名称 ## 云端镜像拉取 为了方便大家快速使用，本人基于腾讯云完成了镜像的构建和托管，并且向大家开放下载，因为镜像源为国内源，所以可以以非常快的速度拉取，甚至可以实现两分钟内拉取并成功运行，这里给出拉取指令 ```bash docker pull docker.cnb.cool/gpf2025/slam:demo ``` 同时给出存放镜像的[腾讯云网站](https://cnb.cool/gpf2025/slam/-/packages/docker/slam)，便于大家查看版本号变化 ## 容器运行 当有了镜像之后就可以运行，这里建议大家将下载好的数据集挂载到容器上，这样容器中就可以播放对应的数据，并且不需要进行复制操作，这里给出运行指令及指令的解释 ```bash docker run -it --rm \ -e "DISPLAY=$DISPLAY" \ #可视化用 -v "/tmp/.X11-unix:/tmp/.X11-unix:rw" \ #可视化用 -v "$HOME/.Xauthority:/root/.Xauthority:rw" \ #可视化用 -v "$(pwd)/nclt.db3:/tmp/nclt.db3" \ #将本地数据包挂载到镜像中使用，根据具体情况来 --gpus all \ #使用主机GPU -e "QT_X11_NO_MITSHM=1" \#可视化用 --network=host \ docker.cnb.cool/gpf2025/slam:demo #要运行的镜像名 ``` ================================================ FILE: package.xml ================================================ lightning 0.0.0 TODO: Package description gaoxiang TODO: License declaration ament_cmake rosidl_default_generators rosidl_default_runtime scrubber_common agibot_robot rclcpp std_msgs geometry_msgs sensor_msgs nav_msgs std_srvs tf2 tf2_ros pcl_conversions pcl_ros message_filters visualization_msgs rosidl_interface_packages ament_lint_auto ament_lint_common ament_cmake ================================================ FILE: pcd/.gitkeep ================================================ ================================================ FILE: scripts/install_dep.sh ================================================ sudo apt install libopencv-dev libpcl-dev pcl-tools libyaml-cpp-dev libgoogle-glog-dev libgflags-dev ros-humble-pcl-conversions ================================================ FILE: scripts/merge_bags.py ================================================ import os.path import sys import rclpy from orca.orca_platform import datadir from rosbag2_py import SequentialReader, SequentialWriter, StorageOptions, ConverterOptions import zstandard as zstd from pathlib import Path import shutil def merge_bags(input_bags, output_bag): # 初始化写入器 if os.path.exists(output_bag): shutil.rmtree(output_bag) writer = SequentialWriter() storage_options = StorageOptions(uri=output_bag, storage_id='sqlite3') converter_options = ConverterOptions('', '') writer.open(storage_options, converter_options) # 创建话题类型缓存，避免重复添加 known_topics = {} # 遍历所有输入包 for input_bag in input_bags: # 初始化读取器 reader = SequentialReader() print('converting ', input_bag) reader.open(StorageOptions(uri=input_bag, storage_id=''), ConverterOptions('', '')) # 如果这是第一个包，或者需要动态发现话题，可以在这里处理 # 获取当前包的话题和类型 topic_types = reader.get_all_topics_and_types() for topic_type in topic_types: if topic_type.name not in known_topics: known_topics[topic_type.name] = topic_type.type writer.create_topic(topic_type) # 读取并写入消息 while reader.has_next(): (topic, data, t) = reader.read_next() writer.write(topic, data, t) print("合并完成！") def convert_ros1_bag(input_file, output_file=None): """ 转换ros1 bags Args: input_file: 输入的.zstd或.db3.zst文件路径 output_file: 输出的解压文件路径，如果为None则自动生成 """ if output_file is None: if input_file.endswith('.bag'): output_file = input_file.rsplit('.', 1)[0] else: output_file = input_file + '.decompressed' try: # 读取压缩文件并解压 print('rosbags-convert --src ' + input_file + ' --dst ' + output_file) os.system('rosbags-convert --src ' + input_file + ' --dst ' + output_file) print(f"转换成功: {input_file} -> {output_file}") return output_file except Exception as e: print(f"解压失败: {e}") return None def decompress_db3_zstd(input_file, output_file=None): """ 解压zstd压缩的db3文件 Args: input_file: 输入的.zstd或.db3.zst文件路径 output_file: 输出的解压文件路径，如果为None则自动生成 """ if output_file is None: if input_file.endswith('.zstd') or input_file.endswith('.zst'): output_file = input_file.rsplit('.', 1)[0] else: output_file = input_file + '.decompressed' # 创建解压器 dctx = zstd.ZstdDecompressor() try: # 读取压缩文件并解压 with open(input_file, 'rb') as compressed_file: with open(output_file, 'wb') as decompressed_file: dctx.copy_stream(compressed_file, decompressed_file) print(f"解压成功: {input_file} -> {output_file}") return output_file except Exception as e: print(f"解压失败: {e}") return None def find_ros1_bags(root_folder): """查找所有ROS 2数据包文件""" root_path = Path(root_folder) bag_files = [] # 查找可能的ROS 2数据包文件 patterns = ['*.bag'] for pattern in patterns: for file_path in root_path.rglob(pattern): file_info = { 'name': file_path.name, 'path': str(file_path.absolute()), 'size': file_path.stat().st_size, 'type': 'rosbag' } bag_files.append(file_info) return bag_files def find_ros2_bags(root_folder): """查找所有ROS 2数据包文件""" root_path = Path(root_folder) bag_files = [] # 查找可能的ROS 2数据包文件 patterns = ['*.db3.zstd'] for pattern in patterns: for file_path in root_path.rglob(pattern): file_info = { 'name': file_path.name, 'path': str(file_path.absolute()), 'size': file_path.stat().st_size, 'type': 'rosbag' } bag_files.append(file_info) return bag_files if __name__ == '__main__': if len(sys.argv) != 2: print('Usage: python merge_bags.py path_to_your_dir') exit(1) if not os.path.exists(sys.argv[1]): print('目录不存在') exit(1) data_dir = sys.argv[1] bags = find_ros1_bags(data_dir) print('找到了', len(bags), '个数据包') merged_bags = [] print('转换数据包中') for bag in bags: convert_ros1_bag(bag["path"]) filename = os.path.splitext(os.path.basename(bag["path"]))[0] print(bag["path"].rsplit('.', 1)[0]) db3_name = bag["path"].rsplit('.', 1)[0] + '/' + filename + '.db3' print('db3 should be ', db3_name) merged_bags.append(db3_name) sorted(merged_bags) merge_bags(merged_bags, data_dir + "/merged") ================================================ FILE: scripts/save_default_map.sh ================================================ ros2 service call /lightning/save_map lightning/srv/SaveMap "{map_id: new_map}" ================================================ FILE: src/CMakeLists.txt ================================================ add_library(${PROJECT_NAME}.libs SHARED common/nav_state.cc common/constant.h common/options.cc common/params.cc io/yaml_io.cc io/file_io.cc core/lio/eskf.cc core/lio/laser_mapping.cc core/lio/pointcloud_preprocess.cc core/loop_closing/loop_closing.cc core/g2p5/g2p5_map.cc core/g2p5/g2p5.cc core/g2p5/g2p5_subgrid.cc core/maps/tiled_map.cc core/maps/tiled_map_chunk.cc core/localization/localization_result.cc core/localization/lidar_loc/lidar_loc.cc core/localization/lidar_loc/pclomp/ndt_omp.cpp core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.cpp core/localization/pose_graph/pgo.cc core/localization/pose_graph/pgo_impl.cc core/localization/pose_graph/pose_extrapolator.cc core/localization/localization.cpp utils/timer.cc utils/pointcloud_utils.cc ui/pangolin_window.cc ui/pangolin_window_impl.cc ui/ui_car.cc ui/ui_cloud.cc ui/ui_trajectory.cc core/system/slam.cc core/system/loc_system.cc wrapper/bag_io.cc ) ament_target_dependencies( ${PROJECT_NAME}.libs "rclcpp" "std_msgs" "geometry_msgs" "sensor_msgs" "nav_msgs" "std_srvs" "message_filters" "pcl_conversions" "tf2_ros" "tf2" ) add_dependencies(${PROJECT_NAME}.libs livox_interfaces2__rosidl_typesupport_cpp ${PROJECT_NAME}__rosidl_typesupport_cpp ) include_directories(${PROJECT_SOURCE_DIR}/src/core/miao) add_subdirectory(core/miao) target_link_libraries(${PROJECT_NAME}.libs ${third_party_libs} ${PCL_LIBRARIES} miao.core miao.utils livox_interfaces2__rosidl_typesupport_cpp ${PROJECT_NAME}__rosidl_typesupport_cpp ) install(TARGETS ${PROJECT_NAME}.libs miao.core miao.utils DESTINATION lib) add_subdirectory(app) ================================================ FILE: src/app/CMakeLists.txt ================================================ add_executable(run_frontend_offline run_frontend_offline.cc ) target_link_libraries(run_frontend_offline ${PROJECT_NAME}.libs ${third_party_libs} ) add_executable(run_loop_offline run_loop_offline.cc ) target_link_libraries(run_loop_offline ${PROJECT_NAME}.libs ${third_party_libs} ) add_executable(run_slam_offline run_slam_offline.cc ) target_link_libraries(run_slam_offline ${PROJECT_NAME}.libs ${third_party_libs} ) add_executable(run_slam_online run_slam_online.cc ) target_link_libraries(run_slam_online ${PROJECT_NAME}.libs ${third_party_libs} ) add_executable(run_loc_offline run_loc_offline.cc ) target_link_libraries(run_loc_offline ${PROJECT_NAME}.libs ${third_party_libs} ) add_executable(run_loc_online run_loc_online.cc ) target_link_libraries(run_loc_online ${PROJECT_NAME}.libs ${third_party_libs} ) add_executable(test_ui test_ui.cc ) target_link_libraries(test_ui ${PROJECT_NAME}.libs ${third_party_libs} ) install(TARGETS run_slam_offline run_slam_online run_loc_offline run_loc_online DESTINATION lib/${PROJECT_NAME}) ================================================ FILE: src/app/run_frontend_offline.cc ================================================ // // Created by xiang on 25-3-18. // #include #include #include "core/g2p5/g2p5.h" #include "core/lio/laser_mapping.h" #include "ui/pangolin_window.h" #include "wrapper/bag_io.h" #include "wrapper/ros_utils.h" #include DEFINE_string(input_bag, "", "输入数据包"); DEFINE_string(config, "./config/default.yaml", "配置文件"); DEFINE_bool(show_grid_map, false, "是否展示栅格地图"); /// 运行一个LIO前端，带可视化 int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); if (FLAGS_input_bag.empty()) { LOG(ERROR) << "未指定输入数据"; return -1; } using namespace lightning; RosbagIO rosbag(FLAGS_input_bag); LaserMapping lio; if (!lio.Init(FLAGS_config)) { LOG(ERROR) << "failed to init lio"; return -1; }; g2p5::G2P5::Options map_opt; map_opt.online_mode_ = false; g2p5::G2P5 map(map_opt); map.Init(FLAGS_config); auto ui = std::make_shared(); ui->Init(); lio.SetUI(ui); Keyframe::Ptr cur_kf = nullptr; rosbag .AddImuHandle("imu_raw", [&lio](IMUPtr imu) { lio.ProcessIMU(imu); return true; }) .AddPointCloud2Handle("points_raw", [&](sensor_msgs::msg::PointCloud2::SharedPtr cloud) { lio.ProcessPointCloud2(cloud); lio.Run(); auto kf = lio.GetKeyframe(); if (cur_kf != kf) { cur_kf = kf; // pcl::io::savePCDFile("./data/" + std::to_string(cur_kf->GetID()) + ".pcd", // *cur_kf->GetCloud()); map.PushKeyframe(cur_kf); if (FLAGS_show_grid_map) { cv::Mat image = map.GetNewestMap()->ToCV(); cv::imshow("map", image); cv::waitKey(10); } } return true; }) .Go(); lio.SaveMap(); cv::Mat image = map.GetNewestMap()->ToCV(); cv::imwrite("./data/map.png", image); Timer::PrintAll(); ui->Quit(); LOG(INFO) << "done"; return 0; } ================================================ FILE: src/app/run_loc_offline.cc ================================================ // // Created by xiang on 25-3-18. // #include #include #include "core/localization/localization.h" #include "ui/pangolin_window.h" #include "utils/timer.h" #include "wrapper/bag_io.h" #include "wrapper/ros_utils.h" #include "io/yaml_io.h" DEFINE_string(input_bag, "", "输入数据包"); DEFINE_string(config, "./config/default.yaml", "配置文件"); DEFINE_string(map_path, "./data/new_map/", "地图路径"); /// 运行定位的测试 int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); if (FLAGS_input_bag.empty()) { LOG(ERROR) << "未指定输入数据"; return -1; } using namespace lightning; RosbagIO rosbag(FLAGS_input_bag); loc::Localization::Options options; options.online_mode_ = false; loc::Localization loc(options); loc.Init(FLAGS_config, FLAGS_map_path); lightning::YAML_IO yaml(FLAGS_config); std::string lidar_topic = yaml.GetValue("common", "lidar_topic"); std::string imu_topic = yaml.GetValue("common", "imu_topic"); rosbag .AddImuHandle(imu_topic, [&loc](IMUPtr imu) { loc.ProcessIMUMsg(imu); usleep(1000); return true; }) .AddPointCloud2Handle(lidar_topic, [&loc](sensor_msgs::msg::PointCloud2::SharedPtr cloud) { loc.ProcessLidarMsg(cloud); usleep(1000); return true; }) .AddLivoxCloudHandle("/livox/lidar", [&loc](livox_ros_driver2::msg::CustomMsg::SharedPtr cloud) { loc.ProcessLivoxLidarMsg(cloud); usleep(1000); return true; }) .Go(); Timer::PrintAll(); loc.Finish(); LOG(INFO) << "done"; return 0; } ================================================ FILE: src/app/run_loc_online.cc ================================================ // // Created by xiang on 25-3-18. // #include #include #include "core/system/loc_system.h" #include "ui/pangolin_window.h" #include "wrapper/ros_utils.h" DEFINE_string(config, "./config/default.yaml", "配置文件"); /// 运行定位的测试 int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning; rclcpp::init(argc, argv); LocSystem::Options opt; LocSystem loc(opt); if (!loc.Init(FLAGS_config)) { LOG(ERROR) << "failed to init loc"; } /// 默认起点开始定位 loc.SetInitPose(SE3()); loc.Spin(); rclcpp::shutdown(); return 0; } ================================================ FILE: src/app/run_loop_offline.cc ================================================ // // Created by xiang on 25-3-18. // #include #include #include "core/lio/laser_mapping.h" #include "core/loop_closing/loop_closing.h" #include "ui/pangolin_window.h" #include "wrapper/bag_io.h" #include "wrapper/ros_utils.h" DEFINE_string(input_bag, "", "输入数据包"); DEFINE_string(config, "./config/default.yaml", "配置文件"); /// 运行一个LIO前端，带可视化 int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); if (FLAGS_input_bag.empty()) { LOG(ERROR) << "未指定输入数据"; return -1; } using namespace lightning; RosbagIO rosbag(FLAGS_input_bag); LaserMapping lio; if (!lio.Init(FLAGS_config)) { LOG(ERROR) << "failed to init lio"; return -1; }; auto ui = std::make_shared(); ui->Init(); lio.SetUI(ui); auto loop = std::make_shared(); loop->Init(FLAGS_config); Keyframe::Ptr cur_kf = nullptr; rosbag .AddImuHandle("imu_raw", [&lio](IMUPtr imu) { lio.ProcessIMU(imu); return true; }) .AddPointCloud2Handle("points_raw", [&lio, &cur_kf, &loop](sensor_msgs::msg::PointCloud2::SharedPtr cloud) { lio.ProcessPointCloud2(cloud); lio.Run(); auto kf = lio.GetKeyframe(); if (cur_kf != kf) { cur_kf = kf; loop->AddKF(kf); } return true; }) .Go(); lio.SaveMap(); Timer::PrintAll(); ui->Quit(); LOG(INFO) << "done"; return 0; } ================================================ FILE: src/app/run_slam_offline.cc ================================================ // // Created by xiang on 25-3-18. // #include #include #include "core/system/slam.h" #include "ui/pangolin_window.h" #include "utils/timer.h" #include "wrapper/bag_io.h" #include "wrapper/ros_utils.h" #include "io/yaml_io.h" DEFINE_string(input_bag, "", "输入数据包"); DEFINE_string(config, "./config/default.yaml", "配置文件"); /// 运行一个LIO前端，带可视化 int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); if (FLAGS_input_bag.empty()) { LOG(ERROR) << "未指定输入数据"; return -1; } using namespace lightning; RosbagIO rosbag(FLAGS_input_bag); SlamSystem::Options options; options.online_mode_ = false; SlamSystem slam(options); /// 实时模式好像掉帧掉的比较厉害？ if (!slam.Init(FLAGS_config)) { LOG(ERROR) << "failed to init slam"; return -1; } slam.StartSLAM("new_map"); lightning::YAML_IO yaml(FLAGS_config); std::string lidar_topic = yaml.GetValue("common", "lidar_topic"); std::string imu_topic = yaml.GetValue("common", "imu_topic"); rosbag /// IMU 的处理 .AddImuHandle(imu_topic, [&slam](IMUPtr imu) { slam.ProcessIMU(imu); return true; }) /// lidar 的处理 .AddPointCloud2Handle(lidar_topic, [&slam](sensor_msgs::msg::PointCloud2::SharedPtr msg) { slam.ProcessLidar(msg); return true; }) /// livox 的处理 .AddLivoxCloudHandle("/livox/lidar", [&slam](livox_ros_driver2::msg::CustomMsg::SharedPtr cloud) { slam.ProcessLidar(cloud); return true; }) .Go(); slam.SaveMap(""); Timer::PrintAll(); LOG(INFO) << "done"; return 0; } ================================================ FILE: src/app/run_slam_online.cc ================================================ // // Created by xiang on 25-3-18. // #include #include #include "core/system/slam.h" #include "utils/timer.h" #include "wrapper/bag_io.h" #include "wrapper/ros_utils.h" DEFINE_string(config, "./config/default.yaml", "配置文件"); /// 运行一个LIO前端，带可视化 int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning; /// 需要rclcpp::init rclcpp::init(argc, argv); SlamSystem::Options options; options.online_mode_ = true; SlamSystem slam(options); if (!slam.Init(FLAGS_config)) { LOG(ERROR) << "failed to init slam"; return -1; } slam.StartSLAM("new_map"); slam.Spin(); Timer::PrintAll(); rclcpp::shutdown(); LOG(INFO) << "done"; return 0; } ================================================ FILE: src/app/test_ui.cc ================================================ // // Created by xiang on 25-8-27. // #include "ui/pangolin_window.h" #include #include #include #include int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; google::ParseCommandLineFlags(&argc, &argv, true); lightning::ui::PangolinWindow ui; ui.Init(); while (!ui.ShouldQuit()) { sleep(1); } ui.Quit(); return 0; } ================================================ FILE: src/common/constant.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_CONSTANT_H #define LIGHTNING_CONSTANT_H #include "common/eigen_types.h" namespace lightning::constant { /// 各类常量定义 constexpr double kDEG2RAD = 0.017453292519943; // deg -> rad constexpr double kRAD2DEG = 57.295779513082323; // rad -> deg constexpr double kPI = M_PI; // pi constexpr double kPI_2 = kPI / 2.0; // pi/2 // 地理参数 constexpr double kGRAVITY = 9.80665; // g constexpr double G_m_s2 = 9.806; // 重力大小 } // namespace lightning::constant #endif // LIGHTNING_CONSTANT_H ================================================ FILE: src/common/eigen_types.h ================================================ // // Created by gaoxiang on 2020/8/10. // #pragma once #ifndef LIGHTNING_EIGEN_TYPES_H #define LIGHTNING_EIGEN_TYPES_H #include #include #include #include #include #include "Sophus/se2.hpp" #include "Sophus/se3.hpp" namespace lightning { /// 数值类型缩写 using IdType = size_t; // pose represented as sophus structs using SO2 = Sophus::SO2d; using SE2 = Sophus::SE2d; using SE3 = Sophus::SE3d; using SO3 = Sophus::SO3d; /// numerical types /// alias for eigen struct using Vec2f = Eigen::Vector2f; using Vec3f = Eigen::Vector3f; using Vec4f = Eigen::Matrix; using Vec6f = Eigen::Matrix; using Vec2d = Eigen::Vector2d; using Vec3d = Eigen::Vector3d; using Vec4d = Eigen::Vector4d; using Vec6d = Eigen::Matrix; using Vec7d = Eigen::Matrix; using Vec12d = Eigen::Matrix; using Vec15d = Eigen::Matrix; using VecXd = Eigen::Matrix; using Mat2f = Eigen::Matrix; using Mat3f = Eigen::Matrix; using Mat4f = Eigen::Matrix; using Mat6f = Eigen::Matrix; using Mat1d = Eigen::Matrix; using Mat2d = Eigen::Matrix; using Mat3d = Eigen::Matrix; using Mat4d = Eigen::Matrix; using Mat6d = Eigen::Matrix; using Mat12d = Eigen::Matrix; using Mat36f = Eigen::Matrix; using Mat23f = Eigen::Matrix; using Mat66f = Eigen::Matrix; using MatXd = Eigen::Matrix; template using VectorN = Eigen::Matrix; using Vector2 = VectorN<2>; using Vector3 = VectorN<3>; using Vector4 = VectorN<4>; using Vector6 = VectorN<6>; using Vector7 = VectorN<7>; using VectorX = VectorN; template using MatrixN = Eigen::Matrix; using Matrix2 = MatrixN<2>; using Matrix3 = MatrixN<3>; using Matrix4 = MatrixN<4>; using MatrixX = MatrixN; using H6d = Eigen::Matrix; using H1d = Eigen::Matrix; using Vec2i = Eigen::Vector2i; using Vec3i = Eigen::Vector3i; using Aff3d = Eigen::Affine3d; using Quatd = Eigen::Quaternion; using Trans3d = Eigen::Translation3d; using AngAxisd = Eigen::AngleAxis; using Quat = Quatd; using Aff3f = Eigen::Affine3f; using Quatf = Eigen::Quaternion; using Trans3f = Eigen::Translation3f; using AngAxisf = Eigen::AngleAxis; using SE3f = Sophus::SE3f; } // namespace lightning #endif ================================================ FILE: src/common/functional_points.h ================================================ // // Created by xiang on 23-10-12. // #pragma once #include "common/eigen_types.h" namespace lightning { /// 功能点定义 struct FunctionalPoint { FunctionalPoint() {} FunctionalPoint(const std::string& name, const SE3& pose) : name_(name), pose_(pose) {} std::string name_; SE3 pose_; }; } // namespace lightning ================================================ FILE: src/common/imu.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_IMU_H #define LIGHTNING_IMU_H #include "common/eigen_types.h" namespace lightning { /// 内部定义的里程计观测，如果需要ROS，则从sensor_msg/Imu 的消息转换过来 struct IMU { double timestamp = 0; Vec3d angular_velocity = Vec3d::Zero(); Vec3d linear_acceleration = Vec3d::Zero(); }; using IMUPtr = std::shared_ptr; } // namespace lightning #endif // LIGHTNING_IMU_H ================================================ FILE: src/common/keyframe.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_KEYFRAME_H #define LIGHTNING_KEYFRAME_H #include "common/eigen_types.h" #include "common/nav_state.h" #include "common/point_def.h" #include "common/std_types.h" namespace lightning { /// 关键帧描述 /// NOTE: 在添加后端后，需要加锁 class Keyframe { public: using Ptr = std::shared_ptr; Keyframe() {} Keyframe(unsigned long id, CloudPtr cloud, NavState state) : id_(id), cloud_(cloud), state_(state), pose_lio_(state.GetPose()) { timestamp_ = state_.timestamp_; pose_opt_ = pose_lio_; } unsigned long GetID() const { return id_; } CloudPtr GetCloud() const { return cloud_; } SE3 GetLIOPose() { UL lock(data_mutex_); return pose_lio_; } void SetLIOPose(const SE3& pose) { UL lock(data_mutex_); pose_lio_ = pose; // also set opt pose_opt_ = pose_lio_; } SE3 GetOptPose() { UL lock(data_mutex_); return pose_opt_; } void SetOptPose(const SE3& pose) { UL lock(data_mutex_); pose_opt_ = pose; } void SetState(NavState s) { UL lock(data_mutex_); state_ = s; } NavState GetState() { UL lock(data_mutex_); return state_; } protected: unsigned long id_ = 0; double timestamp_ = 0; CloudPtr cloud_ = nullptr; /// 降采样之后的点云 std::mutex data_mutex_; SE3 pose_lio_; // 前端的pose SE3 pose_opt_; // 后端优化后的pose NavState state_; // 卡尔曼滤波器状态 }; } // namespace lightning #endif // LIGHTNING_KEYFRAME_H ================================================ FILE: src/common/loop_candidate.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_LOOP_CANDIDATE_H #define LIGHTNING_LOOP_CANDIDATE_H #include "common/eigen_types.h" namespace lightning { /** * 回环检测候选帧 */ struct LoopCandidate { LoopCandidate() {} LoopCandidate(uint64_t id1, uint64_t id2) : idx1_(id1), idx2_(id2) {} uint64_t idx1_ = 0; uint64_t idx2_ = 0; SE3 Tij_; double ndt_score_ = 0.0; }; } // namespace lightning #endif // LIGHTNING_LOOP_CANDIDATE_H ================================================ FILE: src/common/measure_group.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_MEASURE_GROUP_H #define LIGHTNING_MEASURE_GROUP_H #include #include "common/imu.h" #include "common/odom.h" #include "common/point_def.h" namespace lightning { /// 同步获取的消息 struct MeasureGroup { double timestamp_ = 0; // scan的开始时间 double lidar_begin_time_ = 0; double lidar_end_time_ = 0; std::deque imu_; // 两个scan之间的IMU测量 std::deque odom_; // 两个scan之间的odom测量 CloudPtr scan_raw_ = nullptr; // 原始传感器的扫描数据 CloudPtr scan_ = nullptr; // 选点、距离过滤之后的数据 CloudPtr scan_undist_ = nullptr; // 去畸变后的数据 CloudPtr scan_undist_raw_ = nullptr; // 去畸变后的原始数据 }; } // namespace lightning #endif // LIGHTNING_MEASURE_GROUP_H ================================================ FILE: src/common/nav_state.cc ================================================ // // Created by xiang on 2022/6/21. // #include "common/nav_state.h" namespace lightning { /// 矢量变量的维度 const std::vector NavState::vect_states_{ {NavState::kPosIdx, NavState::kPosIdx, NavState::kBlockDim}, // pos {NavState::kVelIdx, NavState::kVelIdx, NavState::kBlockDim}, // vel {NavState::kBgIdx, NavState::kBgIdx, NavState::kBlockDim}, // bg }; /// SO3 变量的维度 const std::vector NavState::SO3_states_{ {NavState::kRotIdx, NavState::kRotIdx, NavState::kBlockDim}, // rot }; } // namespace lightning ================================================ FILE: src/common/nav_state.h ================================================ // // Created by xiang on 2022/2/15. // #pragma once #include "common/eigen_types.h" #include #include namespace lightning { /** * 重构之后的状态变量 * 显式写出各维度状态 * * 虽然我觉得有些地方还是啰嗦了点。。 * * 4个3维块，共12维 */ struct NavState { constexpr static int dim = 12; // 状态变量维度 constexpr static int full_dim = 12; // 误差变量维度 constexpr static int kBlockDim = 3; constexpr static int kPosIdx = 0; constexpr static int kRotIdx = 3; constexpr static int kVelIdx = 6; constexpr static int kBgIdx = 9; using VectState = Eigen::Matrix; // 矢量形式 using FullVectState = Eigen::Matrix; // 全状态矢量形式 NavState() = default; bool operator<(const NavState& other) { return timestamp_ < other.timestamp_; } FullVectState ToState() { FullVectState ret; ret.block(kPosIdx, 0) = pos_; ret.block(kRotIdx, 0) = rot_.log(); ret.block(kVelIdx, 0) = vel_; ret.block(kBgIdx, 0) = bg_; return ret; } void FromVectState(const FullVectState& state) { pos_ = state.block(kPosIdx, 0); rot_ = SO3::exp(state.block(kRotIdx, 0)); vel_ = state.block(kVelIdx, 0); bg_ = state.block(kBgIdx, 0); } // 运动过程 inline FullVectState get_f(const Vec3d& gyro, const Vec3d& acce) const { FullVectState res = FullVectState::Zero(); // 减零偏 Vec3d omega = gyro - bg_; Vec3d a_inertial = rot_ * acce; // 加计零偏不再参与在线估计 for (int i = 0; i < 3; i++) { res(i) = vel_[i]; res(i + kRotIdx) = omega[i]; res(i + kVelIdx) = a_inertial[i] + grav_[i]; } return res; } /// 运动方程对状态的雅可比 inline Eigen::Matrix df_dx(const Vec3d& acce) const { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.block(kPosIdx, kVelIdx) = Mat3d::Identity(); Vec3d acc = acce; // Vec3d omega = gyro - bg_; cov.block(kVelIdx, kRotIdx) = -rot_.matrix() * SO3::hat(acc); cov.block(kRotIdx, kBgIdx) = -Eigen::Matrix3d::Identity(); return cov; } /// 运动方程对噪声的雅可比 inline Eigen::Matrix df_dw() const { Eigen::Matrix cov = Eigen::Matrix::Zero(); cov.block(kVelIdx, 3) = -rot_.matrix(); cov.block(kRotIdx, 0) = -Eigen::Matrix3d::Identity(); cov.block(kBgIdx, 6) = Eigen::Matrix3d::Identity(); return cov; } /// 递推 void oplus(const FullVectState& vec, double dt) { timestamp_ += dt; pos_ += vec.middleRows(kPosIdx, kBlockDim) * dt; rot_ = rot_ * SO3::exp(vec.middleRows(kRotIdx, kBlockDim) * dt); // vel_ += vec.middleRows(kVelIdx, kBlockDim) * dt; bg_ += vec.middleRows(kBgIdx, kBlockDim) * dt; } /** * 广义减法, this - other * @param result 减法结果 * @param other 另一个状态变量 */ VectState boxminus(const NavState& other) { VectState result; result.block(kPosIdx, 0) = pos_ - other.pos_; result.block(kRotIdx, 0) = (other.rot_.inverse() * rot_).log(); result.block(kVelIdx, 0) = vel_ - other.vel_; result.block(kBgIdx, 0) = bg_ - other.bg_; return result; } /** * 广义加法 this = this+dx * @param dx 增量 */ NavState boxplus(const VectState& dx) { NavState ret; ret.timestamp_ = timestamp_; ret.pos_ = pos_ + dx.middleRows(kPosIdx, kBlockDim); ret.rot_ = rot_ * SO3::exp(dx.middleRows(kRotIdx, kBlockDim)); ret.vel_ = vel_ + dx.middleRows(kVelIdx, kBlockDim); ret.bg_ = bg_ + dx.middleRows(kBgIdx, kBlockDim); ret.grav_ = grav_; return ret; } /// 各个子变量所在维度信息 struct MetaInfo { MetaInfo(int idx, int vdim, int dof) : idx_(idx), dim_(vdim), dof_(dof) {} int idx_ = 0; // 变量所在索引 int dim_ = 0; // 变量维度 int dof_ = 0; // 自由度 }; static const std::vector vect_states_; // 矢量变量的维度 static const std::vector SO3_states_; // SO3 变量的维度 friend inline std::ostream& operator<<(std::ostream& os, const NavState& s) { os << std::setprecision(18) << s.pos_.transpose() << " " << s.rot_.unit_quaternion().coeffs().transpose() << " " << s.vel_.transpose() << " " << s.bg_.transpose() << " " << s.grav_.transpose(); return os; } inline SE3 GetPose() const { return SE3(rot_, pos_); } inline SO3 GetRot() const { return rot_; } inline void SetPose(const SE3& pose) { rot_ = pose.so3(); pos_ = pose.translation(); } inline Vec3d Getba() const { return Vec3d::Zero(); } inline Vec3d Getbg() const { return bg_; } inline Vec3d GetVel() const { return vel_; } void SetVel(const Vec3d& v) { vel_ = v; } double timestamp_ = 0.0; // 时间戳 double confidence_ = 0.0; // 定位置信度 bool pose_is_ok_ = true; // 定位是否有效 bool lidar_odom_reliable_ = true; // lio是否有效 bool is_parking_ = false; // 是否在停车 Vec3d pos_ = Vec3d::Zero(); // 位置 SO3 rot_; // 旋转 Vec3d vel_ = Vec3d::Zero(); // 速度 Vec3d bg_ = Vec3d::Zero(); // 陀螺零偏 Vec3d grav_ = Vec3d(0.0, 0.0, -9.81); // 固定重力向量（不参与状态估计） }; } // namespace lightning ================================================ FILE: src/common/odom.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_ODOM_H #define LIGHTNING_ODOM_H #include "common/eigen_types.h" namespace lightning { /// 内部定义的里程计观测，如果需要ROS，则从odom的消息转换过来 struct Odom { double timestamp_ = 0; SE3 pose; Vec3d linear; // 线速度 Vec3d angular; // 角速度（若有） }; using OdomPtr = std::shared_ptr; } // namespace lightning #endif // LIGHTNING_ODOM_H ================================================ FILE: src/common/options.cc ================================================ // // Created by xiang on 24-4-8. // #include namespace lightning { namespace debug { /// debug and save bool flg_exit = false; // ctrl-c中断 bool flg_pause = false; // 暂停 bool flg_next = false; // 暂停后，放行单个消息(单步调试) float play_speed = 10.0; } // namespace debug namespace lo { float lidar_time_interval = 0.1; // 雷达的扫描时间 bool use_dr_rotation = true; int relative_pose_check_cloud_size = 2; double parking_speed = 0.05; double parking_count = 5; double kf_pose_check_distance = 0.5; double kf_pose_check_angle = 5.0; double pose2d_roll_limit = 8.0; double pose2d_pitch_limit = 10.0; double pose2d_relative_z_limit = 0.25; } // namespace lo namespace fasterlio { int NUM_MAX_ITERATIONS = 8; float ESTI_PLANE_THRESHOLD = 0.1; } // namespace fasterlio namespace map { std::string map_path = ""; // 地图路径 Vec3d map_origin = Vec3d::Zero(); // 地图原点 } // namespace map // ui namespace ui { int pgo_res_rows = 16; // pgo发送滑窗数据给ui的矩阵的行数 float opacity = 0.2; // 点云透明度 } // namespace ui // pgo namespace pgo { double lidar_loc_pos_noise = 0.3; // lidar定位位置噪声 // 0.3 double lidar_loc_ang_noise = 1.0 * constant::kDEG2RAD; // lidar定位角度噪声 double lidar_loc_outlier_th = 30.0; // lidar定位异常阈值 double lidar_odom_pos_noise = 0.3; // LidarOdom相对定位位置噪声 double lidar_odom_ang_noise = 1.0 * constant::kDEG2RAD; // LidarOdom相对定位角度噪声 double lidar_odom_outlier_th = 0.01; // LidarOdom异常值检测 double dr_pos_noise = 1.0; // DR相对定位位置噪声 // 0.05 double dr_ang_noise = 0.5 * constant::kDEG2RAD; // DR相对定位角度噪声 double dr_pos_noise_ratio = 1.0; // DR位置噪声倍率 double pgo_frame_converge_pos_th = 0.05; // PGO帧位置收敛阈值 double pgo_frame_converge_ang_th = 1.0 * constant::kDEG2RAD; // PGO帧姿态收敛阈值 double pgo_smooth_factor = 0.01; // PGO帧平滑因子 } // namespace pgo // lidar_loc namespace lidar_loc { int grid_search_angle_step = 20; // 角度网格搜索步数（关键参数） double grid_search_angle_range = 20.0; // 角度搜索半径(角度制，关键参数,左右各有) } // namespace lidar_loc } // namespace lightning ================================================ FILE: src/common/options.h ================================================ // // Created by xiang on 24-4-8. // #pragma once #ifndef LIGHTNING_MODULE_OPTIONS_H #define LIGHTNING_MODULE_OPTIONS_H #include #include #include #include /// 配置参数 namespace lightning { namespace debug { /// debug and save extern bool flg_exit; // ctrl-c中断 extern bool flg_pause; // 暂停 extern bool flg_next; // 暂停后，放行单个消息(单步调试) extern float play_speed; // 播放速度 inline void SigHandle(int sig) { debug::flg_exit = true; rclcpp::shutdown(); } } // namespace debug namespace lo { extern float lidar_time_interval; // 雷达的扫描时间 extern bool use_dr_rotation; // 姿态预测是否使用dr extern int relative_pose_check_cloud_size; // pose校验相对帧数,不可小于2 extern double parking_speed; // 驻车判别速度阈值 extern double parking_count; // 驻车判别心跳次数 extern double kf_pose_check_distance; // 触发校验距离阈值 extern double kf_pose_check_angle; // 触发校验角度阈值 extern double pose2d_roll_limit; // 当前帧触发2d roll角度阈值 extern double pose2d_pitch_limit; // 当前帧触发2d roll角度阈值 extern double pose2d_relative_z_limit; // 当前帧触发2d 相邻帧z值波动阈值 } // namespace lo /// 地图配置 namespace map { extern std::string map_path; // 地图路径 extern Vec3d map_origin; // 地图原点 } // namespace map /// PGO 配置 namespace pgo { constexpr int PGO_MAX_FRAMES = 5; // PGO所持的最大帧数 constexpr int PGO_MAX_SIZE_OF_RELATIVE_POSE_QUEUE = 10000; // PGO 相对定位队列最大长度 constexpr int PGO_MAX_SIZE_OF_RTK_POSE_QUEUE = 200; // PGO RTK观测队列最大长度 constexpr double PGO_DISTANCE_TH_LAST_FRAMES = 2.5; // PGO 滑窗时，最近两帧的最小距离 constexpr double PGO_ANGLE_TH_LAST_FRAMES = 10 * M_PI / 360.0; // PGO 滑窗时，最近两帧的最小角度 /// 噪声参数 /// 宁松勿紧避矛盾 extern double lidar_loc_pos_noise; // lidar定位位置噪声 extern double lidar_loc_ang_noise; // lidar定位角度噪声 extern double lidar_loc_outlier_th; // lidar定位异常阈值 extern double lidar_odom_pos_noise; // LidarOdom相对定位位置噪声 extern double lidar_odom_ang_noise; // LidarOdom相对定位角度噪声 extern double lidar_odom_outlier_th; // LidarOdom异常值检测 extern double dr_pos_noise; // DR相对定位位置噪声 extern double dr_ang_noise; // DR相对定位角度噪声 extern double dr_pos_noise_ratio; // DR位置噪声倍率 extern double pgo_frame_converge_pos_th; // PGO帧位置收敛阈值 extern double pgo_frame_converge_ang_th; // PGO帧姿态收敛阈值 extern double pgo_smooth_factor; // PGO平滑因子 } // namespace pgo // ui namespace ui { extern int pgo_res_rows; // pgo发送滑窗数据给ui的矩阵的行数 extern float opacity; // 点云透明度 } // namespace ui // lidar_loc namespace lidar_loc { extern int grid_search_angle_step; // 角度网格搜索步数（关键参数） extern double grid_search_angle_range; // 角度搜索半径(角度制，关键参数,左右各有) } // namespace lidar_loc /// fasterlio 配置 namespace fasterlio { /// fixed params constexpr double INIT_TIME = 0.1; constexpr int NUM_MATCH_POINTS = 5; // required matched points in current constexpr int MIN_NUM_MATCH_POINTS = 3; // minimum matched points in current /// configurable params extern int NUM_MAX_ITERATIONS; // max iterations of ekf extern float ESTI_PLANE_THRESHOLD; // plane threshold } // namespace fasterlio } // namespace lightning #endif ================================================ FILE: src/common/params.cc ================================================ // // Created by xiang on 25-3-26. // #include "common/params.h" #include "core/lightning_math.hpp" #include "io/yaml_io.h" namespace lightning { Params::Params(const std::string& yaml_path) { yaml_path_ = yaml_path; std::cout << "Loading params from " << yaml_path << std::endl; auto yaml = YAML::LoadFile(yaml_path); Vec3d lidar_T_wrt_IMU; Mat3d lidar_R_wrt_IMU; std::vector Tol_ini; Sophus::SE3d Toi; try { std::string front_type = yaml["frontend_type"].as(); frontend_ = FrontEndType::FASTER_LIO; process_cloud_in_step = yaml["process_cloud_in_step"].as(); online_mode = yaml["online_mode"].as(); with_ui_ = yaml["with_ui"].as(); enable_backend_ = yaml["enable_backend"].as(); enable_frontend_log_ = yaml["enable_frontend_log"].as(); enable_backend_log_ = yaml["enable_backend_log"].as(); is_vis_occupancy_map_ = yaml["is_vis_occupancy_map"].as(); enable_balm_ = yaml["enable_balm"].as(); point_cloud_topic_ = yaml["pointCloudTopic"].as(); imu_topic_ = yaml["imuTopic"].as(); odom_topic_ = yaml["odomTopic"].as(); save_pcd_dir_ = yaml["savePCDDirectory"].as(); baselink_frame_ = yaml["agi_sam"]["baselinkFrame"].as(); odom_frame_ = yaml["agi_sam"]["odometryFrame"].as(); map_frame_ = yaml["agi_sam"]["mapFrame"].as(); save_map_resolution_ = yaml["agi_sam"]["save_map_resolution"].as(); std::string sensorStr = yaml["agi_sam"]["sensor"].as(); if (sensorStr == "velodyne") { sensor_ = SensorType::VELODYNE; } else if (sensorStr == "ouster") { sensor_ = SensorType::OUSTER; } else if (sensorStr == "livox") { sensor_ = SensorType::LIVOX; } else if (sensorStr == "mid360") { sensor_ = SensorType::MID360; } else { } lidar_min_range_ = yaml["agi_sam"]["lidarMinRange"].as(); lidar_max_range_ = yaml["agi_sam"]["lidarMaxRange"].as(); kf_dist_th_ = yaml["agi_sam"]["surroundingkeyframeAddingDistThreshold"].as(); kf_angle_th_ = yaml["agi_sam"]["surroundingkeyframeAddingAngleThreshold"].as(); imu_type_ = yaml["agi_sam"]["imuType"].as(); use_fasterlio_undistort_ = yaml["use_fasterlio_undistort"].as(); relative_cloud_pt_time_ = yaml["relative_cloud_pt_time"].as(); Tol_ini = yaml["mapping"]["Tol"].as>(); auto extrinT = yaml["mapping"]["extrinsic_T"].as>(); auto extrinR = yaml["mapping"]["extrinsic_R"].as>(); lidar_T_wrt_IMU = math::VecFromArray(extrinT); lidar_R_wrt_IMU = math::MatFromArray(extrinR); auto Tol_eig = math::Mat4FromArray(Tol_ini); SE3 Tol = SE3(Eigen::Quaterniond(Tol_eig.block<3, 3>(0, 0)).normalized(), Tol_eig.block<3, 1>(0, 3)); Toi = Tol * Sophus::SE3d(lidar_R_wrt_IMU, lidar_T_wrt_IMU).inverse(); Toi_ = Toi; Tllold_used_input_ = Tol; SE3 Til = SE3(Eigen::Quaterniond(lidar_R_wrt_IMU).normalized(), lidar_T_wrt_IMU); Til = Til * Tllold_used_input_.inverse(); R_imu_lidar_ = Til.rotationMatrix(); R_lidar_imu_ = R_imu_lidar_.transpose(); t_imu_lidar_ = Til.translation(); ext_qrpy_ = Quatd(R_lidar_imu_); } catch (const YAML::Exception& e) { std::cerr << "YAML parsing error at line " << e.mark.line + 1 << ", column " << e.mark.column + 1 << ": " << e.what() << std::endl; // 退出程序 std::exit(EXIT_FAILURE); } } } // namespace lightning ================================================ FILE: src/common/params.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_PARAMS_H #define LIGHTNING_PARAMS_H #include #include #include "common/eigen_types.h" namespace lightning { constexpr double GRAVITY_ = 9.7946; // Gravity const in Shanghai/China /// 激光类型 enum class SensorType { VELODYNE, // 威力登 OUSTER, // ouster LIVOX, // 览沃 MID360 }; enum class FrontEndType { FASTER_LIO, // 使用faster-lio作为前端 }; struct Params { Params() {} // not set Params(const std::string& yaml_path); // load from yaml std::string yaml_path_; // yaml文件路径 FrontEndType frontend_ = FrontEndType::FASTER_LIO; // 前端类型 bool process_cloud_in_step = false; // 是否单步调试 bool online_mode = false; // 是否在线模式 bool with_ui_ = false; // 是否带可视化UI int max_imu_init_count_ = 20; // IMU初始化使用的数据 bool use_fasterlio_undistort_ = true; // 是否使用fasterlio中的去畸变（与predict过程绑定） bool relative_cloud_pt_time_ = false; // if true: it_cloud->time(some point tm) should not - pcl_beg_time(0th point tm) bool enable_backend_ = false; // 是否打开后端回环 bool enable_frontend_log_ = true; // 是否打开前端log bool enable_backend_log_ = true; // 是否打开后端log bool is_vis_occupancy_map_ = false; // 是否显示occu map bool enable_skip_cloud_ = false; bool frontend_record_kf_ = true; // 前端是否记录关键帧（仅建图时需要，定位时不需要） int ivox_capacity_ = 1000000; // 前端局部地图的缓存大小（定位时取更小的值) int skip_cloud_number_ = 4; bool enable_balm_ = false; // Topics std::string point_cloud_topic_; // 输入点云话题 std::string imu_topic_; // 输入imu话题 std::string odom_topic_; // 输入odom话题 // Frames std::string lidar_frame_; std::string baselink_frame_; std::string odom_frame_; std::string map_frame_; // Save pcd std::string save_pcd_dir_; float save_map_resolution_ = 0.2; // Lidar Sensor Configuration SensorType sensor_; float lidar_min_range_ = 0; float lidar_max_range_ = 0; float kf_dist_th_ = 0; float kf_angle_th_ = 0; // IMU double imu_scale_ = 1.0; Mat3d R_imu_lidar_; //; R_imu_lidar, IMU->Lidar, or same P in Lidar->in IMU Mat3d R_lidar_imu_; //; R_imu_lidar.transpose() Vec3d t_imu_lidar_; //; t_imu_lidar, IMU->Lidar // for Livox_imu int imu_type_ = 0; Quatd ext_qrpy_; // odom to imu SE3 Toi_; // trans input data(l means lidar) // only used in input stage, will update Til/T_imu_lidar before also in input stage SE3 Tllold_used_input_; }; } // namespace lightning #endif // LIGHTNING_PARAMS_H ================================================ FILE: src/common/point_def.h ================================================ // // Created by xiang on 25-3-12. // #pragma once #ifndef LIGHTNING_POINT_DEF_H #define LIGHTNING_POINT_DEF_H #include "common/eigen_types.h" #include #include /// 地图点云的一些定义 /// clang-format off struct PointXYZIT { PCL_ADD_POINT4D PCL_ADD_INTENSITY double time; PointXYZIT() {} EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, time, time)) struct PointRobotSense { PCL_ADD_POINT4D PCL_ADD_INTENSITY double timestamp = 0; PointRobotSense() {} EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; POINT_CLOUD_REGISTER_POINT_STRUCT(PointRobotSense, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp)) namespace velodyne_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D float intensity; float time; std::uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace velodyne_ros // clang-format off POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) (float, time, time)(std::uint16_t, ring, ring)) namespace ouster_ros { struct EIGEN_ALIGN16 Point { PCL_ADD_POINT4D EIGEN_MAKE_ALIGNED_OPERATOR_NEW float intensity; uint32_t t; uint16_t reflectivity; uint8_t ring; uint16_t ambient; uint32_t range; }; } // namespace ouster_ros // clang-format off POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) // use std::uint32_t to avoid conflicting with pcl::uint32_t (std::uint32_t, t, t) (std::uint16_t, reflectivity, reflectivity) (std::uint8_t, ring, ring) (std::uint16_t, ambient, ambient) (std::uint32_t, range, range) ) // clang-format on namespace lightning { /// 各类点云的缩写 using PointType = PointXYZIT; using PointCloudType = pcl::PointCloud; using CloudPtr = PointCloudType::Ptr; using PointVec = std::vector>; inline Vec3f ToVec3f(const PointType& pt) { return pt.getVector3fMap(); } inline Vec3d ToVec3d(const PointType& pt) { return pt.getVector3fMap().cast(); } /// 带ring, range等其他信息的全量信息点云 struct FullPointType { PCL_ADD_POINT4D float range = 0; float radius = 0; uint8_t intensity = 0; uint8_t ring = 0; uint8_t angle = 0; double time = 0; float height = 0; inline FullPointType() {} EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; /// 全量点云的定义 using FullPointCloudType = pcl::PointCloud; using FullCloudPtr = FullPointCloudType::Ptr; inline Vec3f ToVec3f(const FullPointType& pt) { return pt.getVector3fMap(); } inline Vec3d ToVec3d(const FullPointType& pt) { return pt.getVector3fMap().cast(); } using PointVector = std::vector>; constexpr double G_m_s2 = 9.81; // Gravity const in GuangDong/China } // namespace lightning #endif // LIGHTNING_POINT_DEF_H ================================================ FILE: src/common/pose_rpy.h ================================================ // // Created by xiang on 25-7-7. // #ifndef LIGHTNING_POSE_RPY_H #define LIGHTNING_POSE_RPY_H namespace lightning{ /// 以欧拉角表达的Pose template struct PoseRPY { PoseRPY() = default; PoseRPY(T xx, T yy, T zz, T r, T p, T y) : x(xx), y(yy), z(zz), roll(r), pitch(p), yaw(y) {} T x = 0; T y = 0; T z = 0; T roll = 0; T pitch = 0; T yaw = 0; }; using PoseRPYD = PoseRPY; using PoseRPYF = PoseRPY; } #endif // LIGHTNING_POSE_RPY_H ================================================ FILE: src/common/s2.hpp ================================================ // // Created by xiang on 2022/2/15. // #pragma once #include "core/lightning_math.hpp" namespace lightning { /** * sphere 2 * 本身是三维的，更新量是二维的一种东东 * 底下那堆计算我反正看不懂，别问我是啥，问就是照抄的 * * @todo S2的长度应当可以外部指定 */ struct S2 { using SO3 = Sophus::SO3d; static constexpr int den_ = 98090; static constexpr int num_ = 10000; static constexpr double length_ = double(den_) / double(num_); Vec3d vec_; public: S2() { vec_ = length_ * Vec3d(1, 0, 0); } S2(const Vec3d &vec) : vec_(vec) { vec_.normalize(); vec_ = vec_ * length_; } double operator[](int idx) const { return vec_[idx]; } Eigen::Matrix S2_hat() const { Eigen::Matrix skew_vec; skew_vec << double(0), -vec_[2], vec_[1], vec_[2], double(0), -vec_[0], -vec_[1], vec_[0], double(0); return skew_vec; } /** * S2_Mx: the partial derivative of x.boxplus(u) w.r.t. u */ Eigen::Matrix S2_Mx(const Eigen::Matrix &delta) const { Eigen::Matrix res; Eigen::Matrix Bx = S2_Bx(); if (delta.norm() < 1e-5) { res = -SO3::hat(vec_) * Bx; } else { Vec3d Bu = Bx * delta; SO3 exp_delta = math::exp(Bu, 0.5f); /** * Derivation of d(Exp(Bx dx)x)/d(dx)=d(Exp(Bu)x)/d(dx): * d(Exp(Bu)x)/d(dx)=d(Exp(Bu)x)/d(Bu) Bx; then * d(Exp(Bu)x)/d(Bu)=d[Exp(Bu+dBu)x]/d(dBu)=d[Exp(Jl(Bu)dBu)Exp(Bu)x]/d(dBu)=d[(Jl(Bu)dBu)^Exp(Bu)x]/d(dBu) * =d[-(Exp(Bu)x)^Jl(Bu)dBu]/d(dBu)=-Exp(Bu)x^Exp(-Bu)Jl(Bu); * for Exp(x+dx)=Exp(x)Exp(Jr(x)dx)=Exp(Jl(x)dx)Exp(x)=Exp(x)Exp(Exp(-x)Jl(x)dx) => * Exp(-x)Jl(x)=Jr(x)=Jl(-x) => * =-Exp(Bu)x^Jl(-Bu) => d(Exp(Bu)x)/d(dx)= -Exp(Bu) x^ Jl(Bu)^T Bx or A_matrix is just Jl() */ res = -exp_delta.matrix() * SO3::hat(vec_) * math::A_matrix(Bu).transpose() * Bx; } return res; } /** * Bx两个列向量为正切空间的局部坐标系 * * S2 = [a b c], l = length * Bx = [ * -b -c * l-bb/(l+a) -bc/(l+a) * -bc/(l+a) l-cc/(l+a) * ] / l * Derivation of origin MTK: (112) Rv = [v Bx] = [x -r 0; y xc -s; z xs c] * where c=cos(alpha),s=sin(alpha),r=sqrt(y^2+z^2),||v||=1: * For y-axis or (0,1,0) in local frame is mapped to (-r;xc;xs) in world frame, * meaning x/OG(or gravity vector) projects to A of yz plane, * and this projecting line will intersect the perpendicular plane of OG at O at the point B, * then from OB we can found OC with ||OC||=1, which is just the y-axis, then we get its coordinate in world frame: * ∠AOG+∠AGO=pi/2=∠AOG+∠AOB => ∠AOB=∠AGO, for sin(∠AGO)=r/||v||=r => ||OC||*sin(∠AOB)=r;||OC||*cos(∠AOB)=x => * (-r;xc;xs) is the y-axis coordinate in world frame, then z-axis is easy to get * Derivation of current MTK with S2 = [x y z], ||S2|| = 1: * just a rotation of origin one around x/OG axis to meet y-axis coordinate in word frame to be (-y;a;c), * then z-axis must be (+-z;b;d) for x^+y^+z^=1, where a,b,c,d is just variable to be solved * for current MTK chooses clockwise rotation(meaning -z): * Rv=[x -y -z; * y a b; * z c d] * then for -xy+ya+zc=0=xy-ya-zb => b=c * for yz+ab+cd=0; b=c => a=-d-yz/c; for -xz+yb+zd=0; b=c => d=x-yc/z * for -xy+ya+zc=0; a=-d-yz/c=yc/z-x-yz/c => -xy+y^2c/z-xy-y^2z/c+zc=0 => (z+y^2/z)c^2 -2xy c -y^2z = 0 => * c=[2xy +- sqrt(4x^2y^2 + 4(z+y^2/z)y^2z)]/[2(z+y^2/z)]; for z^2+y^2=1-x^2 => * c=[xy +- y]z/(1-x^2), for rotation is clocewise, thus c<0 => c=(xy-y)z/(1-x^2)=-yz/(1+x) * then b,a,d is easy to get and also if ||S2||=l, it is easy to prove c=-ylzl/(l+lx)/l=-bc/(l+a)/l */ Eigen::Matrix S2_Bx() const { Eigen::Matrix res; if (vec_[0] + length_ > 1e-5) { res << -vec_[1], -vec_[2], length_ - vec_[1] * vec_[1] / (length_ + vec_[0]), -vec_[2] * vec_[1] / (length_ + vec_[0]), -vec_[2] * vec_[1] / (length_ + vec_[0]), length_ - vec_[2] * vec_[2] / (length_ + vec_[0]); res /= length_; } else { res = Eigen::Matrix::Zero(); res(1, 1) = -1; res(2, 0) = 1; } return res; } /** * S2_Nx: the partial derivative of x.boxminus(y) w.r.t. x, where x and y belong to S2 * S2_Nx_yy: simplified S2_Nx when x is equal to y */ Eigen::Matrix S2_Nx_yy() const { Eigen::Matrix res; Eigen::Matrix Bx = S2_Bx(); res = 1 / length_ / length_ * Bx.transpose() * SO3::hat(vec_); return res; } void oplus(const Vec3d &delta, double scale = 1.0) { vec_ = math::exp(delta, scale * 0.5) * vec_; } /** * 广义减 * @param res * @param other */ Vec2d boxminus(const S2 &other) const { Vec2d res; double v_sin = (SO3::hat(vec_) * other.vec_).norm(); double v_cos = vec_.transpose() * other.vec_; double theta = std::atan2(v_sin, v_cos); if (v_sin < 1e-5) { if (std::fabs(theta) > 1e-5) { res[0] = 3.1415926; res[1] = 0; } else { res[0] = 0; res[1] = 0; } } else { S2 other_copy = other; Eigen::Matrix Bx = other_copy.S2_Bx(); res = theta / v_sin * Bx.transpose() * SO3::hat(other.vec_) * vec_; } return res; } /** * 广义加 * @param delta * @param scale */ void boxplus(const Vec2d &delta, double scale = 1) { Eigen::Matrix Bx = S2_Bx(); SO3 res = math::exp(Bx * delta, scale / 2); vec_ = res.matrix() * vec_; } }; } // namespace lightning ================================================ FILE: src/common/std_types.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_STD_TYPES_H #define LIGHTNING_STD_TYPES_H #include #include namespace lightning { using UL = std::unique_lock; } #endif // LIGHTNING_STD_TYPES_H ================================================ FILE: src/common/timed_pose.h ================================================ // // Created by xiang on 25-7-7. // #ifndef LIGHTNING_TIMED_POSE_H #define LIGHTNING_TIMED_POSE_H #include "common/eigen_types.h" namespace lightning { /// 带时间的pose struct TimedPose { TimedPose() {} TimedPose(double t, const SE3& pose) : timestamp_(t), pose_(pose) {} double timestamp_ = 0; SE3 pose_; bool operator<(const TimedPose& timed_pose) const { return ((timestamp_ - timed_pose.timestamp_) < 0); }; }; } // namespace lightning #endif // LIGHTNING_TIMED_POSE_H ================================================ FILE: src/core/g2p5/g2p5.cc ================================================ // // Created by xiang on 25-6-23. // #include "core/g2p5/g2p5.h" #include "common/constant.h" #include #include #include #include #include #include #include "utils/timer.h" #include "yaml-cpp/yaml.h" namespace lightning::g2p5 { G2P5::~G2P5() { Quit(); } void G2P5::Quit() { quit_flag_ = true; if (options_.online_mode_) { draw_frontend_map_thread_.Quit(); } if (draw_backend_map_thread_.joinable()) { draw_backend_map_thread_.join(); } } void G2P5::PushKeyframe(Keyframe::Ptr kf) { UL lock(kf_mutex_); all_keyframes_.emplace_back(kf); if (options_.online_mode_) { draw_frontend_map_thread_.AddMessage(kf); } else { RenderFront(kf); } } void G2P5::RenderFront(Keyframe::Ptr kf) { { UL lock(frontend_mutex_); frontend_current_ = kf; } { UL lock{newest_map_mutex_}; lightning::Timer::Evaluate([&]() { AddKfToMap({kf}, frontend_map_); }, "G2P5 Occupancy Mapping", true); newest_map_ = frontend_map_; } /// 向外回调 if (map_update_cb_) { map_update_cb_(newest_map_); } } void G2P5::RedrawGlobalMap() { backend_redraw_flag_ = true; } void G2P5::RenderBack() { while (!quit_flag_) { while (!backend_redraw_flag_ && !quit_flag_) { sleep(1); } if (quit_flag_) { break; } is_busy_ = true; /// 后端重绘被触发 backend_redraw_flag_ = false; /// 重新绘制整张地图，如果中间过程又被重绘了，则退出 std::vector all_keyframes; { UL lock(kf_mutex_); all_keyframes = all_keyframes_; } if (all_keyframes.empty()) { is_busy_ = false; continue; } G2P5Map::Options opt; opt.resolution_ = options_.grid_map_resolution_; backend_map_ = std::make_shared(opt); auto cur_kf = all_keyframes.begin(); bool abort = false; for (; cur_kf != all_keyframes.end(); ++cur_kf) { AddKfToMap({*cur_kf}, backend_map_); if (backend_redraw_flag_) { LOG(INFO) << "backend redraw triggered in process, abort"; abort = true; break; } if (quit_flag_) { abort = true; break; } } if (abort) { /// 继续重绘 is_busy_ = false; continue; } /// 绘制过程中前端可能发生了更新，要保证后端绘制和前端的一致性 int cur_idx = all_keyframes.back()->GetID(); while (true) { Keyframe::Ptr frontend_kf = nullptr; { UL lock(frontend_mutex_); frontend_kf = frontend_current_; } if (cur_idx == frontend_kf->GetID()) { break; } int frontend_idx = frontend_kf->GetID(); std::vector kfs; { UL lock(kf_mutex_); for (int i = cur_idx + 1; i <= frontend_idx; ++i) { kfs.emplace_back(all_keyframes_[i]); } } AddKfToMap(kfs, backend_map_); cur_idx = frontend_idx; } { /// 同步前后端地图，替换newest map UL lock{newest_map_mutex_}; frontend_map_ = backend_map_; newest_map_ = frontend_map_; } /// 向外回调 if (map_update_cb_) { map_update_cb_(newest_map_); } is_busy_ = false; } LOG(INFO) << "backend render quit"; } bool G2P5::ResizeMap(const std::vector &kfs, G2P5MapPtr &map) { /// 重设地图大小 float init_min_x, init_min_y, init_max_x, init_max_y; float min_x, min_y, max_x, max_y; map->GetMinAndMax(init_min_x, init_min_y, init_max_x, init_max_y); min_x = init_min_x; min_y = init_min_y; max_x = init_max_x; max_y = init_max_y; /// 从后往前迭代 for (auto it = kfs.begin(); it != kfs.end(); ++it) { if (quit_flag_) { return true; } auto kf = *it; SE3 pose = kf->GetOptPose(); auto cloud = kf->GetCloud(); if (pose.translation().x() < min_x) { min_x = pose.translation().x(); } if (pose.translation().y() < min_y) { min_y = pose.translation().y(); } if (pose.translation().x() > max_x) { max_x = pose.translation().x(); } if (pose.translation().y() > max_y) { max_y = pose.translation().y(); } if (min_x > max_x || min_y > max_y) { return false; } for (size_t i = 0; i < cloud->points.size(); i += 10) { float range = cloud->points[i].getVector3fMap().norm(); if (range > options_.usable_scan_range_ || range <= 0.01 || std::isnan(range)) { continue; } Vec3d point = pose * cloud->points[i].getVector3fMap().cast(); if ((point.x() - 1) < min_x) { min_x = point.x() - 1; } if ((point.y() - 1) < min_y) { min_y = point.y() - 1; } if ((point.x() + 1) > max_x) { max_x = point.x() + 1; } if ((point.y() + 1) > max_y) { max_y = point.y() + 1; } } } if (min_x > max_x || min_y > max_y) { return false; } /// resize map if necessary if (min_x < init_min_x || min_y < init_min_y || max_x > init_max_x || max_y > init_max_y) { min_x = (min_x > init_min_x) ? init_min_x : min_x; min_y = (min_y > init_min_y) ? init_min_y : min_y; max_x = (max_x < init_max_x) ? init_max_x : max_x; max_y = (max_y < init_max_y) ? init_max_y : max_y; float r = map->GetGridResolution(); min_x = static_cast((floor)(min_x / r)) * r; min_y = static_cast((floor)(min_y / r)) * r; max_x = static_cast((ceil)(max_x / r)) * r; max_y = static_cast((ceil)(max_y / r)) * r; map->Resize(min_x, min_y, max_x, max_y); // LOG(INFO) << "map resized to " << min_x << ", " << min_y << ", " << max_x << ", " << max_y; } return true; } bool G2P5::AddKfToMap(const std::vector &kfs, G2P5MapPtr &map) { /// 如果需要，更新地图大小 ResizeMap(kfs, map); for (const auto &kf : kfs) { Convert3DTo2DScan(kf, map); } return true; } G2P5MapPtr G2P5::GetNewestMap() { UL lock{newest_map_mutex_}; LOG(INFO) << "getting newest map"; if (newest_map_ == nullptr) { return nullptr; } return newest_map_->MakeDeepCopy(); } void G2P5::Convert3DTo2DScan(Keyframe::Ptr kf, G2P5MapPtr &map) { // 3D转2D算法 if (options_.esti_floor_) { if (!DetectPlaneCoeffs(kf)) { /// 如果动态检测失败，就用之前的参数 floor_coeffs_ = Vec4d(0, 0, 1, -options_.default_floor_height_); } else { if (options_.verbose_) { LOG(INFO) << "floor coeffs: " << floor_coeffs_.transpose(); } } } else { floor_coeffs_ = Vec4d(0, 0, 1, -options_.default_floor_height_); } // step 1. 计算每个方向上发出射线上的高度分布, // NOTE 转成整形的360度是有精度损失的 std::vector> rays(360); // map键值：距离-相对高度（以距离排序） std::vector angle_distance_height(360, Vec2d::Zero()); // 每个角度上的距离-高度值 std::vector pts_3d; /// 距离地面0.3 ～ 1.2米之间的点云，激光坐标系下 SE3 Twb = kf->GetOptPose(); double min_th = options_.min_th_floor_; double max_th = options_.max_th_floor_; /// 把激光系下的点云转到当前submap坐标系 auto cloud = kf->GetCloud(); CloudPtr obstacle_cloud(new PointCloudType); /// 黑色点的处理方式：所有在障碍物范围内的都是黑色点 int cnt_valid = 0; for (size_t i = 0; i < cloud->points.size(); ++i) { const auto &pt = cloud->points[i]; if (quit_flag_) { return; } Vec3d pc = Vec3d(pt.x, pt.y, pt.z); Vec4d pn = Vec4d(pt.x, pt.y, pt.z, 1); // 计算激光点所在角度方向以及高度 Vec2d p = pc.head<2>(); double dis = p.norm(); if (dis > options_.usable_scan_range_) { continue; } double dis_floor = pn.dot(floor_coeffs_); /// 该点到地面的距离 double dangle = atan2(p[1], p[0]) * constant::kRAD2DEG; int angle = int(round(dangle) + 360) % 360; if (dis_floor > min_th) { if (dis_floor < max_th) { // 特别矮的和特别高的都不计入 pts_3d.emplace_back(pc); rays[angle].insert({dis, dis_floor}); /// 设置黑点 Vec3d p_world = Twb * pc; map->SetHitPoint(p_world[0], p_world[1], true, dis_floor); cnt_valid++; obstacle_cloud->points.push_back(pt); } } else if (dis_floor > -min_th) { // 地面附近或者地面以下 rays[angle].insert({dis, dis_floor}); cnt_valid++; } } // if (options_.verbose_) { // obstacle_cloud->is_dense = false; // obstacle_cloud->height = 1; // obstacle_cloud->width = obstacle_cloud->size(); // pcl::io::savePCDFile("./data/obs.pcd", *obstacle_cloud); // } // LOG(INFO) << "valid obs: " << cnt_valid << ", total: " << cloud->size(); std::vector floor_esti_data; // 地面高度估计值 // step 2, 考察每个方向上的分布曲线 // 正常场景中，每个方向由较低高度开始（地面），转到较高的高度（物体） // 如若不是，那么可能发生了遮挡或进入盲区 constexpr double default_ray_distance = -1; const double floor_rh = floor_coeffs_[3]; for (int i = 0; i < 360; ++i) { if (quit_flag_) { return; } if (rays[i].size() < 2) { // 该方向测量数据很少，在16线中是不太可能出现的（至少地面上应该有线），出现，则说明有严重遮挡或失效，认为该方向取一个默认的最小距离（车宽） angle_distance_height[i] = Vec2d(default_ray_distance, floor_rh); continue; } /// 取距离和高度 for (auto iter = rays[i].rbegin(); iter != rays[i].rend(); ++iter) { if (iter->second < options_.min_th_floor_) { angle_distance_height[i] = Vec2d(iter->first, iter->second); continue; } auto next_iter = iter; next_iter++; if (next_iter != rays[i].rend()) { if (iter->second > options_.min_th_floor_ && next_iter->second < options_.min_th_floor_) { // 当前点是障碍但下一个点不是 angle_distance_height[i] = Vec2d(iter->first, iter->second); break; } } else { angle_distance_height[i] = Vec2d(iter->first, iter->second); } } } // 以2D scan方式添加白色点 SetWhitePoints(angle_distance_height, kf, map); } void G2P5::SetWhitePoints(const std::vector &pt2d, Keyframe::Ptr kf, G2P5MapPtr &map) { assert(pt2d.size() == 360); SE3 pose = kf->GetOptPose(); Vec3d orig = pose.translation(); for (int i = 0; i < 360; ++i) { if (quit_flag_) { return; } double angle = float(i) * constant::kDEG2RAD; float r = pt2d[i][0]; float h = pt2d[i][1]; Vec3d p_local(r * cos(angle), r * sin(angle), h); Vec3d p_world = pose * p_local; /// 某方向无测量值时，认为无效 if (r <= 0 || r > options_.usable_scan_range_) { /// 比较近时，涂白 if (r < 0.1) { map->SetMissPoint(p_world[0], p_world[1], orig[0], orig[1], h, options_.lidar_height_); } continue; } map->SetMissPoint(p_world[0], p_world[1], orig[0], orig[1], h, options_.lidar_height_); } } bool G2P5::DetectPlaneCoeffs(Keyframe::Ptr kf) { pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::SACSegmentation seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.25); CloudPtr cloud(new PointCloudType); for (auto &pt : kf->GetCloud()->points) { if (pt.z < options_.lidar_height_ + options_.default_floor_height_) { cloud->points.push_back(pt); } } cloud->width = cloud->points.size(); cloud->height = 1; cloud->is_dense = false; if (cloud->size() < 200) { // LOG(ERROR) << "not enough points cloud->size(): " << cloud->size(); return false; } // if (options_.verbose_) { // pcl::io::savePCDFile("./data/floor_candi.pcd", *cloud); // } seg.setInputCloud(cloud); seg.segment(*inliers, *coefficients); if (coefficients->values[2] < 0.99) { LOG(ERROR) << "floor is not horizontal. "; return false; } if (inliers->indices.size() < 100) { LOG(ERROR) << "cannot get enough points on floor: " << inliers->indices.size(); return false; } for (int i = 0; i < 4; ++i) { floor_coeffs_[i] = coefficients->values[i]; } cloud->clear(); return true; } void G2P5::Init(std::string yaml_path) { auto yaml = YAML::LoadFile(yaml_path); options_.esti_floor_ = yaml["g2p5"]["esti_floor"].as(); options_.min_th_floor_ = yaml["g2p5"]["min_th_floor"].as(); options_.max_th_floor_ = yaml["g2p5"]["max_th_floor"].as(); options_.lidar_height_ = yaml["g2p5"]["lidar_height"].as(); options_.grid_map_resolution_ = yaml["g2p5"]["grid_map_resolution"].as(); options_.default_floor_height_ = yaml["g2p5"]["floor_height"].as(); G2P5Map::Options opt; opt.resolution_ = options_.grid_map_resolution_; frontend_map_ = std::make_shared(opt); if (options_.online_mode_) { draw_frontend_map_thread_.SetProcFunc([this](Keyframe::Ptr kf) { RenderFront(kf); }); draw_frontend_map_thread_.Start(); } draw_backend_map_thread_ = std::thread([this]() { RenderBack(); }); } } // namespace lightning::g2p5 ================================================ FILE: src/core/g2p5/g2p5.h ================================================ // // Created by xiang on 25-6-23. // #ifndef LIGHTNING_G2P5_H #define LIGHTNING_G2P5_H #include "common/eigen_types.h" #include "common/keyframe.h" #include "core/g2p5/g2p5_map.h" #include "core/system/async_message_process.h" #include namespace lightning::g2p5 { /** * 地图渲染部分+3转2的部分 * * g2p5 是一个2.5D地图模块，它计算从雷达中心点出发，打到障碍物部分的射线，然后进行occupancy map投影。 * 可以很方便地将g2p5转换成opencv或者ros格式的地图进行发布和存储 * * g2p5假设雷达是水平安装的。在雷达水平面以下，存在一个地面的平面（floor），地面的高度应该低于雷达安装高度 * 这个地面参数可以动态估计，也可以事先指定 * 比如，我们默认雷达水平面为z=0，雷达安装在1m的高度上，那么地面默认为z=-1.0 * 在地面上方一定区间内的点云才会参与栅格地图的计算，默认取 (min_floor, max_floor) * 之间的点云，比如地面以上的0.5至1.0米的障碍物 那么取 min_floor = 0.5, max_floor = 1.0 * * 如果想过滤掉天花板，应该将max_floor适当设小一点 * * 如果是这个区间的障碍物，g2p5会模拟一条从雷达高度的点射向障碍物点，并计算沿途的射线高度 * 如果栅格中的物体高度低于此射线，则不会被刷新；如果高于此射线，则会被刷白 * * 在lightning-lm中，栅格地图仅用于显示和存储（输出给导航），并不用于定位，所以默认不需要过高的分辨率 * * 地图渲染主要有两个步骤组成： * 1. 前端在计算完关键帧时，会立即更新到最新的地图上，此时应该发布最新的地图 * 2. 后端在发现回环时，触发一次地图重绘过程。由于重绘时间较长，需要在重绘完成后再放到1, * 此时1仍然是活跃的，仍然会发布地图 * * g2p5的3转2目前角分辨率精度较低，但速度很快，正常情况一帧点云应该在10ms以内 */ class G2P5 { public: struct Options { Options() {} bool online_mode_ = true; // 是否为在线模式 bool esti_floor_ = false; // 是否需要估计地面 double lidar_height_ = 0.0; // 雷达的安装高度 double default_floor_height_ = -1.0; // 默认的地面高度(通常在雷达下方) double min_th_floor_ = 0.5; /// 距离地面这个高的障碍物讲被扫入栅格地图中 double max_th_floor_ = 1.2; /// 距离地面这个高的障碍物讲被扫入栅格地图中 float usable_scan_range_ = 50.0; // 用于计算栅格地图的最远障碍物距离 double grid_map_resolution_ = 0.1; // 栅格地图的分辨率，室外建议为0.1，室内可以用0.05 bool verbose_ = true; }; explicit G2P5(Options options = Options()) : options_(options) {} ~G2P5(); using MapUpdateCallback = std::function; /// 从yaml中读取配置信息 void Init(std::string yaml_path); /// 当地图发生改变时，可以向外发布，在这里设置它的回调 void SetMapUpdateCallback(MapUpdateCallback func) { map_update_cb_ = func; } /// 退出渲染器 void Quit(); /// 增加一个关键帧，该关键帧会放到关键帧队列中 void PushKeyframe(Keyframe::Ptr kf); /// 触发一次重绘，如果程序正在重绘过程中，再次触发会导致当前重绘过程停止 void RedrawGlobalMap(); /// 获取最新的地图 G2P5MapPtr GetNewestMap(); /// 设置是否需要最快渲染速度 bool SetParallelRendering(bool enable_parallel = false) { parallel_render_ = enable_parallel; return true; } /// 查看是否仍然有没绘制完的关键帧 bool IsBusy() { return is_busy_; } private: /// 在已有的map上增加一些关键帧 bool AddKfToMap(const std::vector &kfs, G2P5MapPtr &map); /// 对地图进行缩放 bool ResizeMap(const std::vector &kfs, G2P5MapPtr &map); /// 渲染前端的地图 void RenderFront(Keyframe::Ptr kf); /// 渲染后端的地图 void RenderBack(); /// 3D 点云转2D scan void Convert3DTo2DScan(Keyframe::Ptr kf, G2P5MapPtr &map); /// 检测地面参数 bool DetectPlaneCoeffs(Keyframe::Ptr kf); void SetWhitePoints(const std::vector &ang_distance_height, Keyframe::Ptr kf, G2P5MapPtr &map); Options options_; std::atomic_bool parallel_render_ = false; // 是否需要并行渲染 std::atomic_bool is_busy_ = false; // 是否仍有未渲染完的 MapUpdateCallback map_update_cb_; // 地图更新的回调函数 std::atomic_bool quit_flag_ = false; // 退出标记位 std::mutex kf_mutex_; std::vector all_keyframes_; // 全部关键帧 std::mutex newest_map_mutex_; G2P5MapPtr newest_map_ = nullptr; /// 前端相关 std::mutex frontend_mutex_; // 前端锁 G2P5MapPtr frontend_map_ = nullptr; // 前端最新绘制的地图 Keyframe::Ptr frontend_current_ = nullptr; // 前端正在绘制的关键帧 sys::AsyncMessageProcess draw_frontend_map_thread_; // 前端绘制地图的线程 /// 后端相关 std::thread draw_backend_map_thread_; // 后端重绘的线程 std::atomic_bool backend_redraw_flag_ = false; // 后端重绘的flag G2P5MapPtr backend_map_ = nullptr; // 正在绘制的地图 Vec4d floor_coeffs_ = Vec4d(0, 0, 1.0, 1.0); // 地面方程参数 }; } // namespace lightning::g2p5 #endif // LIGHTNING_G2P5_H ================================================ FILE: src/core/g2p5/g2p5_grid_data.h ================================================ // // Created by xiang on 25-6-24. // #ifndef LIGHTNING_G2P5_GRID_DATA_H #define LIGHTNING_G2P5_GRID_DATA_H namespace lightning::g2p5 { /// 2.5D 子地图定义 /// 子网格中的数据内容 struct GridData { explicit GridData(unsigned int occupySum = 0, unsigned int visitSum = 0) : hit_cnt_(occupySum), visit_cnt_(visitSum) {} unsigned int hit_cnt_ = 0; // 占据的计数 unsigned int visit_cnt_ = 0; // 总共的计数 float height_ = 10000; // 相对地面的最低高度，可以从上方穿过但不能从下方穿过 }; } // namespace lightning::g2p5 #endif // LIGHTNING_G2P5_GRID_DATA_H ================================================ FILE: src/core/g2p5/g2p5_map.cc ================================================ // // Created by xiang on 25-6-23. // #include "core/g2p5/g2p5_map.h" #include #include #include namespace lightning::g2p5 { bool G2P5Map::Init(const float &temp_min_x, const float &temp_min_y, const float &temp_max_x, const float &temp_max_y) { ReleaseResources(); min_x_ = temp_min_x; min_y_ = temp_min_y; max_x_ = temp_max_x; max_y_ = temp_max_y; grid_size_x_ = ceil((max_x_ - min_x_) / grid_reso_); grid_size_y_ = ceil((max_y_ - min_y_) / grid_reso_); if (grid_size_x_ <= 0 || grid_size_y_ <= 0) { return false; } grids_ = new SubGrid *[grid_size_x_]; for (int xi = 0; xi < grid_size_x_; ++xi) { grids_[xi] = new SubGrid[grid_size_y_]; } return true; } std::shared_ptr G2P5Map::MakeDeepCopy() { std::shared_ptr ret(new G2P5Map(options_)); ret->min_x_ = min_x_; ret->min_y_ = min_y_; ret->max_x_ = max_x_; ret->max_y_ = max_y_; ret->grid_size_x_ = grid_size_x_; ret->grid_size_y_ = grid_size_y_; ret->grids_ = new SubGrid *[grid_size_x_]; for (int xi = 0; xi < grid_size_x_; ++xi) { ret->grids_[xi] = new SubGrid[grid_size_y_]; for (int yi = 0; yi < grid_size_y_; ++yi) { ret->grids_[xi][yi] = this->grids_[xi][yi]; } } return ret; } bool G2P5Map::Resize(const float &temp_min_x, const float &temp_min_y, const float &temp_max_x, const float &temp_max_y) { int temp_grid_size_x = ceil((temp_max_x - temp_min_x) / grid_reso_) + 1; int temp_grid_size_y = ceil((temp_max_y - temp_min_y) / grid_reso_) + 1; auto **new_grids = new SubGrid *[temp_grid_size_x]; for (int xi = 0; xi < temp_grid_size_x; ++xi) { new_grids[xi] = new SubGrid[temp_grid_size_y]; } int min_grid_x = (int)round((temp_min_x - min_x_) / grid_reso_); int min_grid_y = (int)round((temp_min_y - min_y_) / grid_reso_); int max_grid_x = (int)ceil((temp_max_x - min_x_) / grid_reso_); int max_grid_y = (int)ceil((temp_max_y - min_y_) / grid_reso_); int dx = min_grid_x < 0 ? 0 : min_grid_x; int dy = min_grid_y < 0 ? 0 : min_grid_y; int Dx = max_grid_x < this->grid_size_x_ ? max_grid_x : this->grid_size_x_; int Dy = max_grid_y < this->grid_size_y_ ? max_grid_y : this->grid_size_y_; for (int x = dx; x < Dx; x++) { for (int y = dy; y < Dy; y++) { assert((x - min_grid_x) >= 0 && (x - min_grid_x) < temp_grid_size_x); assert((y - min_grid_y) >= 0 && (y - min_grid_y) < temp_grid_size_y); assert((x) >= 0 && (x) < temp_grid_size_x); assert((y) >= 0 && (y) < temp_grid_size_y); new_grids[x - min_grid_x][y - min_grid_y] = this->grids_[x][y]; } delete[] this->grids_[x]; } delete[] this->grids_; this->grids_ = new_grids; this->min_x_ = temp_min_x; this->min_y_ = temp_min_y; this->max_x_ = temp_max_x; this->max_y_ = temp_max_y; this->grid_size_x_ = temp_grid_size_x; this->grid_size_y_ = temp_grid_size_y; return true; } G2P5Map::~G2P5Map() { if (grids_ != nullptr) { for (int xi = 0; xi < grid_size_x_; ++xi) { delete[] grids_[xi]; } delete[] grids_; grids_ = nullptr; } } void G2P5Map::SetHitPoint(const float &px, const float &py, const bool &if_hit, float height) { if (grids_ == nullptr) { return; } if (px < min_x_ || px > max_x_ || py < min_y_ || py > max_y_) { return; } int x_index = floor((px - min_x_) / options_.resolution_); int y_index = floor((py - min_y_) / options_.resolution_); UpdateCell(Vec2i(x_index, y_index), if_hit, height); } void G2P5Map::UpdateCell(const Vec2i &point_index, const bool &if_hit, float height) { if (grids_ == nullptr) { return; } int x_index = point_index.x(); int y_index = point_index.y(); int xi = (x_index >> SUB_GRID_SIZE); int yi = (y_index >> SUB_GRID_SIZE); if (xi < 0 || xi > (grid_size_x_ - 1) || yi < 0 || yi > (grid_size_y_ - 1)) { return; } int sub_index_i = x_index - (xi << SUB_GRID_SIZE); int sub_index_j = y_index - (yi << SUB_GRID_SIZE); if (sub_index_i < 0 || sub_index_i > (sub_grid_width_ - 1) || sub_index_j < 0 || sub_index_j > (sub_grid_width_ - 1)) { return; } grids_[xi][yi].SetGridHitPoint(if_hit, sub_index_i, sub_index_j, height); } void G2P5Map::SetMissPoint(const float &point_x, const float &point_y, const float &laser_origin_x, const float &laser_origin_y, float height, float lidar_height) { if (grids_ == nullptr) { return; } int point_x_index = floor(point_x / options_.resolution_); int point_y_index = floor(point_y / options_.resolution_); int xi_lidar = floor(laser_origin_x / options_.resolution_); int yi_lidar = floor(laser_origin_y / options_.resolution_); float k = 0; int sign = 1; int diff_y = point_y_index - yi_lidar; int diff_x = point_x_index - xi_lidar; /// 整数的直线填充算法 if (diff_y == 0 && diff_x == 0) { return; } if (!GetDataIndex(laser_origin_x, laser_origin_y, xi_lidar, yi_lidar)) { return; } std::vector updated_pts; std::vector heights; if (std::abs(diff_y) > std::abs(diff_x)) { if (diff_y == 0) { return; } k = float(diff_x) / diff_y; float dh = (lidar_height - height) / diff_y; sign = diff_y > 0 ? 1 : -1; for (int j = sign; j != diff_y; j += sign) { int i = float(j * k); int x_index = xi_lidar + i; int y_index = yi_lidar + j; updated_pts.emplace_back(Vec2i(x_index, y_index)); heights.emplace_back(lidar_height - j * dh); } } else { if (diff_x == 0) { return; } k = float(diff_y) / diff_x; sign = diff_x > 0 ? 1 : -1; float dh = (lidar_height - height) / diff_x; for (int i = sign; i != diff_x; i += sign) { int j = float(i * k); int x_index = xi_lidar + i; int y_index = yi_lidar + j; updated_pts.emplace_back(Vec2i(x_index, y_index)); heights.emplace_back(lidar_height - i * dh); } } for (int i = 0; i < updated_pts.size(); ++i) { UpdateCell(updated_pts[i], false, heights[i]); } } bool G2P5Map::GetDataIndex(const float x, const float y, int &x_index, int &y_index) { if (x > max_x_ || x < min_x_ || y > max_y_ || y < min_y_) { return false; } x_index = floor((x - min_x_) / options_.resolution_); y_index = floor((y - min_y_) / options_.resolution_); return true; } void G2P5Map::ReleaseResources() { if (grids_ != nullptr) { for (int xi = 0; xi < grid_size_x_; ++xi) { delete[] grids_[xi]; } delete[] grids_; grids_ = nullptr; } min_x_ = min_y_ = 10000; max_x_ = max_y_ = -10000; grid_size_x_ = grid_size_y_ = 0; malloc_trim(0); } nav_msgs::msg::OccupancyGrid G2P5Map::ToROS() { nav_msgs::msg::OccupancyGrid occu_map; int image_width = grid_size_x_ * sub_grid_width_; int image_height = grid_size_y_ * sub_grid_width_; occu_map.info.resolution = static_cast(options_.resolution_); occu_map.info.width = static_cast(image_width); occu_map.info.height = static_cast(image_height); occu_map.info.origin.position.x = min_x_; occu_map.info.origin.position.y = min_y_; int grid_map_size = occu_map.info.width * occu_map.info.height; int grid_map_size_1 = grid_map_size - 1; occu_map.data.resize(grid_map_size); std::fill(occu_map.data.begin(), occu_map.data.end(), -1); int tmp_area = 0; int index_min, index_max; for (int bxi = 0; bxi < grid_size_x_; ++bxi) { for (int byi = 0; byi < grid_size_y_; ++byi) { if (grids_[bxi][byi].IsEmpty()) { continue; } for (int sxi = 0; sxi < sub_grid_width_; ++sxi) { for (int syi = 0; syi < sub_grid_width_; ++syi) { int x = (bxi << SUB_GRID_SIZE) + sxi; int y = (byi << SUB_GRID_SIZE) + syi; if (x >= 0 && x < image_width && y >= 0 && y < image_height) { unsigned int hit_cnt = 0, visit_cnt = 0; grids_[bxi][byi].GetHitAndVisit(sxi, syi, hit_cnt, visit_cnt); float occ = (visit_cnt > 3) ? (float)hit_cnt / (float)visit_cnt : -1; /// 注意这里有转置符号 if (occ < 0) { continue; } else if (occ > options_.occupancy_ratio_) { tmp_area++; occu_map.data[MapIdx(image_width, x, y)] = 100; } else { int index = MapIdx(image_width, x, y); index_min = std::max(0, index - 1); index_max = std::min(grid_map_size_1, index + 1); for (auto extend = index_min; extend <= index_max; extend++) { if (occu_map.data[extend] < 0) { //-1 tmp_area++; occu_map.data[extend] = 0; } } } } else { continue; } } } } } return occu_map; } cv::Mat G2P5Map::ToCV() { int image_width = grid_size_x_ * sub_grid_width_; int image_height = grid_size_y_ * sub_grid_width_; // 定义要映射的颜色值 cv::Vec3b black_color = cv::Vec3b(0, 0, 0); cv::Vec3b white_color = cv::Vec3b(255, 255, 255); cv::Vec3b other_color = cv::Vec3b(127, 127, 127); cv::Mat image(image_height, image_width, CV_8UC3, other_color); int image_height_1 = image_height - 1; int image_width_1 = image_width - 1; int index_y_min, index_y_max, index_x_min, index_x_max; for (int bxi = 0; bxi < grid_size_x_; ++bxi) { for (int byi = 0; byi < grid_size_y_; ++byi) { if (grids_[bxi][byi].IsEmpty()) { continue; } for (int sxi = 0; sxi < sub_grid_width_; ++sxi) { for (int syi = 0; syi < sub_grid_width_; ++syi) { int x = (bxi << SUB_GRID_SIZE) + sxi; int y = (byi << SUB_GRID_SIZE) + syi; if (x >= 0 && x < image_width && y >= 0 && y < image_height) { unsigned int hit_cnt = 0, visit_cnt = 0; grids_[bxi][byi].GetHitAndVisit(sxi, syi, hit_cnt, visit_cnt); float occ = visit_cnt ? (hit_cnt == 0 ? 0 : (float)hit_cnt / (float)visit_cnt) : -1; // assert(occ <= 1.0); /// 注意这里有转置符号 if (occ < 0) { continue; } else if (occ > options_.occupancy_ratio_) { // 0.49 image.at(y, x) = black_color; } else { index_y_min = std::max(0, y - 1); index_y_max = std::min(image_height_1, y + 1); index_x_min = std::max(0, x - 1); index_x_max = std::min(image_width_1, x + 1); for (int extend_y = index_y_min; extend_y <= index_y_max; extend_y++) { for (int extend_x = index_x_min; extend_x <= index_x_max; extend_x++) { if (image.at(extend_y, extend_x) == other_color) { image.at(extend_y, extend_x) = white_color; } } } } } else { continue; } } } } } /// cv的需要 flip y 才能转到俯视视角 cv::Mat image_flip; cv::flip(image, image_flip, 1); return image_flip; } } // namespace lightning::g2p5 ================================================ FILE: src/core/g2p5/g2p5_map.h ================================================ // // Created by xiang on 25-6-23. // #ifndef LIGHTNING_G2P5_MAP_H #define LIGHTNING_G2P5_MAP_H #include "common/eigen_types.h" #include "common/std_types.h" #include "core/g2p5/g2p5_subgrid.h" #include #include #include namespace lightning::g2p5 { /** * g2p5自定义栅格地图数据 * 主要用于对外的绘制，并没有匹配部分 * 数据存储在grids_中，是一个二维数组，内部还有4层subgrids * x为行指针，y为列指针，y优先增长 * * 可以通过ToCV 或者 ToROS 转换成OpenCV格式或者ROS格式进行显示和存储 */ class G2P5Map { public: struct Options { float resolution_ = 0.05; /// 子网格的最高分辨率 float occupancy_ratio_ = 0.3; // 占用比例 }; G2P5Map(Options options) : options_(options) { grid_reso_ = options_.resolution_ * sub_grid_width_; grids_ = nullptr; } ~G2P5Map(); /// 子网格的大小 static inline constexpr int SUB_GRID_SIZE = SubGrid::SUB_GRID_SIZE; /// 由自身内容创建一个深拷贝 std::shared_ptr MakeDeepCopy(); /// 转换至ros occupancy grid nav_msgs::msg::OccupancyGrid ToROS(); /// 转换至opencv::Mat cv::Mat ToCV(); /// 清空内部数据 void ReleaseResources(); bool Init(const float &temp_min_x, const float &temp_min_y, const float &temp_max_x, const float &temp_max_y); bool Resize(const float &temp_min_x, const float &temp_min_y, const float &temp_max_x, const float &temp_max_y); void SetHitPoint(const float &px, const float &py, const bool &if_hit, float height); /** * 白色区域的直线填充算法 * 除了2D填充以外，还需要给出雷达高度和目标位置高度 */ void SetMissPoint(const float &point_x, const float &point_y, const float &laser_origin_x, const float &laser_origin_y, float height, float lidar_height); bool IsObstacle(const Vec2i &point) { // 判断点是否属于障碍物区域，可以是通过查找 grid 中是否已经有墙体或物体数据 int xi = point.x() >> SUB_GRID_SIZE; int yi = point.y() >> SUB_GRID_SIZE; if (xi < 0 || xi >= grid_size_x_ || yi < 0 || yi >= grid_size_y_) { return false; } return true; } void UpdateCell(const Vec2i &point_index, const bool &if_hit, float height); bool GetDataIndex(const float x, const float y, int &x_index, int &y_index); bool IsEmpty() { return (grids_ == nullptr); } inline void GetMinAndMax(float &min_x, float &min_y, float &max_x, float &max_y) { min_x = min_x_; min_y = min_y_; max_x = max_x_; max_y = max_y_; } inline void SetMinAndMax(float &min_x, float &min_y, float &max_x, float &max_y) { min_x_ = min_x; min_y_ = min_y; max_x_ = max_x; max_y_ = max_y; } float GetGridResolution() const { return options_.resolution_; } private: inline int MapIdx(int sx, int x, int y) { return (sx) * (y) + (x); } private: float grid_reso_ = 0.0; /// subgrid的栅格分辨率 float min_x_ = 0, min_y_ = 0, max_x_ = 0, max_y_ = 0; int grid_size_x_ = 0, grid_size_y_ = 0; inline static const int sub_grid_width_ = (1 << SUB_GRID_SIZE); Options options_; SubGrid **grids_ = nullptr; // 实际存储数据的位置 }; typedef std::shared_ptr G2P5MapPtr; } // namespace lightning::g2p5 #endif // LIGHTNING_G2P5_MAP_H ================================================ FILE: src/core/g2p5/g2p5_subgrid.cc ================================================ // // Created by xiang on 25-6-24. // #include "core/g2p5/g2p5_subgrid.h" #include #include namespace lightning::g2p5 { SubGrid::SubGrid(int num_x, int num_y) { if (num_x * num_y == 0) { grid_data_ = nullptr; return; } grid_data_ = new GridData[num_x * num_y]; } void SubGrid::MallocGrid() { if (grid_data_ != nullptr) { return; } grid_data_ = new GridData[width_2_]; } SubGrid::~SubGrid() { if (grid_data_ != nullptr) { delete[] grid_data_; grid_data_ = nullptr; } } SubGrid::SubGrid(const SubGrid &other) { if (grid_data_ != nullptr) { delete[] grid_data_; } grid_data_ = nullptr; if (other.grid_data_ != nullptr) { MallocGrid(); memcpy(grid_data_, other.grid_data_, sizeof(GridData) * width_2_); } } SubGrid &SubGrid::operator=(const SubGrid &other) { if (this != &other) { if (grid_data_ != nullptr) { delete[] grid_data_; } grid_data_ = nullptr; if (other.grid_data_ != nullptr) { MallocGrid(); memcpy(grid_data_, other.grid_data_, sizeof(GridData) * width_2_); } } return *this; } } // namespace lightning::g2p5 ================================================ FILE: src/core/g2p5/g2p5_subgrid.h ================================================ // // Created by xiang on 25-6-24. // #ifndef LIGHTNING_G2P5_SUBGRID_H #define LIGHTNING_G2P5_SUBGRID_H #include "common/std_types.h" #include "core/g2p5/g2p5_grid_data.h" namespace lightning::g2p5 { /// 子网格定义，坐标系与MapToGrid类似 class SubGrid { public: static inline constexpr int SUB_GRID_SIZE = 4; SubGrid(int num_x = 0, int num_y = 0); ~SubGrid(); SubGrid(const SubGrid &other); SubGrid &operator=(const SubGrid &other); /// 设置某个网格的占据或者非占据 void SetGridHitPoint(bool hit, int sub_xi, int sub_yi, float height) { UL lock(data_mutex_); if (grid_data_ == nullptr) { MallocGrid(); } int index = sub_xi + sub_yi * width_; GridData &cell = grid_data_[index]; if (hit) { // 占据，黑色 if (height < cell.height_) { cell.height_ = height; cell.hit_cnt_ += 1; cell.visit_cnt_ += 1; } } else { // 非占据，白色 if (cell.hit_cnt_ > 3) { // 本身为黑色，黑色刷白有高度要求 if (height < cell.height_) { cell.visit_cnt_ += 1; } } else { cell.visit_cnt_ += 1; cell.height_ = height; } } } void RemoveCarNoise(int sub_xi, int sub_yi) { UL lock(data_mutex_); if (grid_data_ == nullptr) { MallocGrid(); } int index = sub_xi + sub_yi * width_; grid_data_[index].visit_cnt_ += 4; grid_data_[index].hit_cnt_ = 0; } /// 判定数据是否为空 bool IsEmpty() { UL lock(data_mutex_); return grid_data_ == nullptr; } /// 获取hit cnt和visit cnt void GetHitAndVisit(int sx, int sy, unsigned int &hit_cnt, unsigned int &visit_cnt) { UL lock(data_mutex_); GridData &d = grid_data_[sx + sy * width_]; hit_cnt = d.hit_cnt_; visit_cnt = d.visit_cnt_; } private: // 分配这个grid的内存 void MallocGrid(); /// 子网格的大小 static inline constexpr int width_ = (1 << SUB_GRID_SIZE); static inline constexpr int width_2_ = (1 << SUB_GRID_SIZE) * (1 << SUB_GRID_SIZE); std::mutex data_mutex_; GridData *grid_data_ = nullptr; }; } // namespace lightning::g2p5 #endif // LIGHTNING_G2P5_SUBGRID_H ================================================ FILE: src/core/ivox3d/hilbert.hpp ================================================ // 2021-09-18, https://github.com/spectral3d/hilbert_hpp is under MIT license. // Copyright (c) 2019 David Beynon // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal // in the Software without restriction, including without limitation the rights // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // copies of the Software, and to permit persons to whom the Software is // furnished to do so, subject to the following conditions: // // The above copyright notice and this permission notice shall be included in all // copies or substantial portions of the Software. // // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. #pragma once #include #include #include #include #include // Version 1.1 - 29 August 2019 // // N dimensional hilbert curve encoding & decoding based on the paper // "Programming the Hilbert Curve" by John Skilling. // // The interface assumes an std::array of some unsigned integer type. This // contains an index in lexographic order, or a set of coordinates on the // hilbert curve. // // Two implementations are included. // // hilbert::v1 contains a fairly straightforward implementation of the paper, // with standard looping constructs. // // hilbert::v2 performs uses template metaprogramming to unroll loops and // theoretically improve performance on some systems. // // v1 produces smaller code, which may be more performant on machines with // small caches. v2 produces relatively large code that seems more efficient // on modern systems for dimension up to about 100. // // v2 should be built with -O3 for best results. -O0 is extremely slow // on most systems. // // Interface is as follows: // // Find the position of a point on an N dimensional Hilbert Curve. // // Based on the paper "Programming the Hilbert Curve" by John Skilling. // // Index is encoded with most significant objects first. Lexographic // sort order. // template // std::array // IndexToPosition(std::array const &in); // Find the index of a point on an N dimensional Hilbert Curve. // // Based on the paper "Programming the Hilbert Curve" by John Skilling. // // Index is encoded with most significant objects first. Lexographic // sort order. // template // std::array // PositionToIndex(std::array const &in); // namespace hilbert { // Fairly straightforward implementation. Loops are loops and code mostly // does what one would expect. namespace v1 { namespace internal { // Extract bits from transposed form. // // e.g. // // a d g j a b c d // b e h k -> e f g h // c f i l i j k l // template std::array UntransposeBits(std::array const &in) { const size_t bits = std::numeric_limits::digits; const T high_bit(T(1) << (bits - 1)); const size_t bit_count(bits * N); std::array out; std::fill(out.begin(), out.end(), 0); // go through all bits in input, msb first. Shift distances are // from msb. for (size_t b = 0; b < bit_count; b++) { size_t src_bit, dst_bit, src, dst; src = b % N; dst = b / bits; src_bit = b / N; dst_bit = b % bits; out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit); } return out; } // Pack bits into transposed form. // // e.g. // // a b c d a d g j // e f g h -> b e h k // i j k l c f i l // template std::array TransposeBits(std::array const &in) { const size_t bits = std::numeric_limits::digits; const T high_bit(T(1) << (bits - 1)); const size_t bit_count(bits * N); std::array out; std::fill(out.begin(), out.end(), 0); // go through all bits in input, msb first. Shift distances // are from msb. for (size_t b = 0; b < bit_count; b++) { size_t src_bit, dst_bit, src, dst; src = b / bits; dst = b % N; src_bit = b % bits; dst_bit = b / N; out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit); } return out; } } // namespace internal // // Public interfaces. // // Find the position of a point on an N dimensional Hilbert Curve. // // Based on the paper "Programming the Hilbert Curve" by John Skilling. // // Index is encoded with most significant objects first. Lexographic // sort order. template std::array IndexToPosition(std::array const &in) { // First convert index to transpose. std::array out(internal::TransposeBits(in)); // Initial gray encoding of transposed vector. { T tmp = out[N - 1] >> 1; for (size_t n = N - 1; n; n--) { out[n] ^= out[n - 1]; } out[0] ^= tmp; } // Apply transforms to gray code. { T cur_bit(2), low_bits; while (cur_bit) { low_bits = cur_bit - 1; size_t n(N); do { n--; if (out[n] & cur_bit) { // flip low bits of X out[0] ^= low_bits; } else { // swap low bits with X T t((out[n] ^ out[0]) & low_bits); out[n] ^= t; out[0] ^= t; } } while (n); cur_bit <<= 1; } } return out; } // Find the index of a point on an N dimensional Hilbert Curve. // // Based on the paper "Programming the Hilbert Curve" by John Skilling. // // Index is encoded with most significant objects first. Lexographic // sort order. template std::array PositionToIndex(std::array const &in) { const size_t bits = std::numeric_limits::digits; std::array out(in); // reverse transforms to convert into transposed gray code. { T cur_bit(T(1) << (bits - 1)), low_bits; do { low_bits = cur_bit - 1; for (size_t n = 0; n < N; n++) { if (out[n] & cur_bit) { // flip low bits of X out[0] ^= low_bits; } else { // swap low bits with X T t((out[n] ^ out[0]) & low_bits); out[n] ^= t; out[0] ^= t; } } cur_bit >>= 1; } while (low_bits > 1); } // Remove gray code from transposed vector. { T cur_bit(T(1) << (bits - 1)), t(0); for (size_t n = 1; n < N; n++) { out[n] ^= out[n - 1]; } do { if (out[N - 1] & cur_bit) { t ^= (cur_bit - 1); } cur_bit >>= 1; } while (cur_bit > 1); for (auto &v : out) { v ^= t; } } return internal::UntransposeBits(out); } } // namespace v1 // Implementation using metaprogramming to unroll most loops. // Optimised performance should be superior to v1 provided all code remains // in cache etc. // // At some value of N v1 should overtake v2. namespace v2 { namespace internal { // Metaprogramming guts. Unrolled loops, abandon all hope etc. namespace tmp { template T TransposeBits2(std::array const &, std::integral_constant, std::integral_constant) { return T(0); } template T TransposeBits2(std::array const &in, std::integral_constant, std::integral_constant) { const size_t size = std::numeric_limits::digits; const size_t src = ((D + ((size - B) * N)) / size); const size_t src_bit = size - (1 + ((D + ((size - B) * N)) % size)); const T src_bit_val = T(1) << src_bit; const size_t dst_bit = (B - 1); const T dst_bit_val = T(1) << dst_bit; // Multiply rather than shift to avoid clang implicit // conversion warning. T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val; return bit + TransposeBits2(in, std::integral_constant(), std::integral_constant()); } template void TransposeBits(std::array const &, std::array &, std::integral_constant) {} template void TransposeBits(std::array const &in, std::array &out, std::integral_constant) { out[D - 1] = TransposeBits2(in, std::integral_constant(), std::integral_constant::digits>()); TransposeBits(in, out, std::integral_constant()); } template T UntransposeBits2(std::array const &, std::integral_constant, std::integral_constant) { return T(0); } template T UntransposeBits2(std::array const &in, std::integral_constant, std::integral_constant) { const size_t size = std::numeric_limits::digits; const size_t src = ((D * size) + (size - B)) % N; const size_t src_bit = size - (((((D * size) + (size - B))) / N) + 1); const T src_bit_val = T(1) << src_bit; const size_t dst_bit(B - 1); const T dst_bit_val = T(1) << dst_bit; // Multiply rather than shift to avoid clang implicit // conversion warning. T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val; return bit + UntransposeBits2(in, std::integral_constant(), std::integral_constant()); } template void UntransposeBits(std::array const &, std::array &, std::integral_constant) {} template void UntransposeBits(std::array const &in, std::array &out, std::integral_constant) { out[D - 1] = UntransposeBits2(in, std::integral_constant(), std::integral_constant::digits>()); UntransposeBits(in, out, std::integral_constant()); } template void ApplyGrayCode1(std::array const &in, std::array &out, std::integral_constant) { out[0] ^= in[N - 1] >> 1; } template void ApplyGrayCode1(std::array const &in, std::array &out, std::integral_constant) { out[I] ^= out[I - 1]; ApplyGrayCode1(in, out, std::integral_constant()); } // Remove a gray code from a transposed vector template void RemoveGrayCode1(std::array &, std::integral_constant) {} // xor array values with previous values. template void RemoveGrayCode1(std::array &in, std::integral_constant) { const size_t src_idx = N - (D + 1); const size_t dst_idx = N - D; in[dst_idx] ^= in[src_idx]; RemoveGrayCode1(in, std::integral_constant()); } template T RemoveGrayCode2(T, std::integral_constant) { return T(0); } template T RemoveGrayCode2(T v, std::integral_constant) { const T cur_bit(T(1) << (B - 1)); const T low_bits(cur_bit - 1); if (v & cur_bit) { return low_bits ^ RemoveGrayCode2(v, std::integral_constant()); } else { return RemoveGrayCode2(v, std::integral_constant()); } } template void GrayToHilbert2(std::array &, std::integral_constant, std::integral_constant) {} template void GrayToHilbert2(std::array &out, std::integral_constant, std::integral_constant) { const size_t n(I - 1); const T cur_bit(T(1) << (std::numeric_limits::digits - B)); const T low_bits(cur_bit - 1); if (out[n] & cur_bit) { // flip low bits of X out[0] ^= low_bits; } else { // swap low bits with X T t((out[n] ^ out[0]) & low_bits); out[n] ^= t; out[0] ^= t; } GrayToHilbert2(out, std::integral_constant(), std::integral_constant()); } template void GrayToHilbert(std::array &, std::integral_constant) {} template void GrayToHilbert(std::array &out, std::integral_constant) { GrayToHilbert2(out, std::integral_constant(), std::integral_constant()); GrayToHilbert(out, std::integral_constant()); } template void HilbertToGray2(std::array &, std::integral_constant, std::integral_constant) {} template void HilbertToGray2(std::array &out, std::integral_constant, std::integral_constant) { const size_t cur_bit(T(1) << B); const size_t low_bits(cur_bit - 1); const size_t n(N - I); if (out[n] & cur_bit) { // flip low bits of X out[0] ^= low_bits; } else { // swap low bits with X T t((out[n] ^ out[0]) & low_bits); out[n] ^= t; out[0] ^= t; } HilbertToGray2(out, std::integral_constant(), std::integral_constant()); } template void HilbertToGray(std::array &, std::integral_constant) {} template void HilbertToGray(std::array &out, std::integral_constant) { HilbertToGray2(out, std::integral_constant(), std::integral_constant()); HilbertToGray(out, std::integral_constant()); } template void ApplyMaskToArray(std::array &, T, std::integral_constant) {} template void ApplyMaskToArray(std::array &a, T mask, std::integral_constant) { a[I - 1] ^= mask; ApplyMaskToArray(a, mask, std::integral_constant()); } } // namespace tmp // Pack bits into transposed form. // // e.g. // // a b c d a d g j // e f g h -> b e h k // i j k l c f i l // template std::array TransposeBits(std::array const &in) { std::array out; std::fill(out.begin(), out.end(), 0); tmp::TransposeBits(in, out, std::integral_constant()); return out; } // Extract bits from transposed form. // e.g. // // a d g j a b c d // b e h k -> e f g h // c f i l i j k l // template std::array UntransposeBits(std::array const &in) { std::array out; std::fill(out.begin(), out.end(), 0); tmp::UntransposeBits(in, out, std::integral_constant()); return out; } // Apply a gray code to a transformed vector. template std::array ApplyGrayCode(std::array const &in) { std::array out(in); tmp::ApplyGrayCode1(in, out, std::integral_constant()); return out; } template std::array RemoveGrayCode(std::array const &in) { const size_t bits = std::numeric_limits::digits; std::array out(in); // Remove gray code from transposed vector. { // xor values with prev values. tmp::RemoveGrayCode1(out, std::integral_constant()); // create a mask. T t = tmp::RemoveGrayCode2(out[N - 1], std::integral_constant()); // Apply mask to output. tmp::ApplyMaskToArray(out, t, std::integral_constant()); } return out; } // Generate code to convert from a transposed gray code to a hilbert // code. template std::array GrayToHilbert(std::array const &in) { std::array out(in); tmp::GrayToHilbert(out, std::integral_constant::digits - 1>()); return out; } // Generate code to convert from a hilbert code to a transposed gray // code. template std::array HilbertToGray(std::array const &in) { std::array out(in); tmp::HilbertToGray(out, std::integral_constant::digits - 1>()); return out; } } // namespace internal // // Public interfaces. // // Find the position of a point on an N dimensional Hilbert Curve. // // Based on the paper "Programming the Hilbert Curve" by John Skilling. // // Index is encoded with most significant objects first. Lexographic // sort order. template std::array IndexToPosition(std::array const &in) { // First convert index to transpose. return internal::GrayToHilbert(internal::ApplyGrayCode(internal::TransposeBits(in))); } // Find the index of a point on an N dimensional Hilbert Curve. // // Based on the paper "Programming the Hilbert Curve" by John Skilling. // // Index is encoded with most significant objects first. Lexographic // sort order. template std::array PositionToIndex(std::array const &in) { return internal::UntransposeBits(internal::RemoveGrayCode(internal::HilbertToGray(in))); } } // namespace v2 } // namespace hilbert ================================================ FILE: src/core/ivox3d/ivox3d.h ================================================ // // Created by xiang on 2021/9/16. // #pragma once #include #include #include #include #include #include "common/eigen_types.h" #include "core/ivox3d/ivox3d_node.hpp" #include "core/lightning_math.hpp" namespace lightning { enum class IVoxNodeType { DEFAULT, // linear ivox PHC, // phc ivox }; /// traits for NodeType template struct IVoxNodeTypeTraits {}; template struct IVoxNodeTypeTraits { using NodeType = IVoxNode; }; template struct IVoxNodeTypeTraits { using NodeType = IVoxNodePhc; }; template class IVox { public: using KeyType = Eigen::Matrix; using PtType = Eigen::Matrix; using NodeType = typename IVoxNodeTypeTraits::NodeType; using PointVector = std::vector>; using DistPoint = typename NodeType::DistPoint; enum class NearbyType { CENTER, // center only NEARBY6, NEARBY18, NEARBY26, }; struct Options { float resolution_ = 0.2; // ivox resolution float inv_resolution_ = 10.0; // inverse resolution NearbyType nearby_type_ = NearbyType::NEARBY6; // nearby range std::size_t capacity_ = 1000000; // capacity }; /** * constructor * @param options ivox options */ explicit IVox(Options options) : options_(options) { options_.inv_resolution_ = 1.0 / options_.resolution_; GenerateNearbyGrids(); } /** * add points * @param points_to_add */ void AddPoints(const PointVector& points_to_add); /// get nn bool GetClosestPoint(const PointType& pt, PointType& closest_pt); /// get nn with condition bool GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num = 5, double max_range = 5.0); /// get nn in cloud bool GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud); /// get number of points size_t NumPoints() const; /// get number of valid grids size_t NumValidGrids() const; /// get statistics of the points std::vector StatGridPoints() const; private: /// generate the nearby grids according to the given options void GenerateNearbyGrids(); /// position to grid KeyType Pos2Grid(const PtType& pt) const; Options options_; std::unordered_map>::iterator, math::hash_vec> grids_map_; // voxel hash map std::list> grids_cache_; // voxel cache std::vector nearby_grids_; // nearbys }; template bool IVox::GetClosestPoint(const PointType& pt, PointType& closest_pt) { std::vector candidates; auto key = Pos2Grid(math::ToEigen(pt)); std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &candidates, &pt, this](const KeyType& delta) { auto dkey = key + delta; auto iter = grids_map_.find(dkey); if (iter != grids_map_.end()) { DistPoint dist_point; bool found = iter->second->second.NNPoint(pt, dist_point); if (found) { candidates.emplace_back(dist_point); } } }); if (candidates.empty()) { return false; } auto iter = std::min_element(candidates.begin(), candidates.end()); closest_pt = iter->Get(); return true; } template bool IVox::GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num, double max_range) { std::vector candidates; candidates.reserve(max_num * nearby_grids_.size()); auto key = Pos2Grid(math::ToEigen(pt)); // #define INNER_TIMER #ifdef INNER_TIMER static std::unordered_map> stats; if (stats.empty()) { stats["knn"] = std::vector(); stats["nth"] = std::vector(); } #endif for (const KeyType& delta : nearby_grids_) { auto dkey = key + delta; auto iter = grids_map_.find(dkey); if (iter != grids_map_.end()) { iter->second->second.KNNPointByCondition(candidates, pt, max_num, max_range); } } if (candidates.empty()) { return false; } #ifdef INNER_TIMER auto t1 = std::chrono::high_resolution_clock::now(); #endif if (candidates.size() <= max_num) { } else { std::nth_element(candidates.begin(), candidates.begin() + max_num - 1, candidates.end()); candidates.resize(max_num); } std::nth_element(candidates.begin(), candidates.begin(), candidates.end()); #ifdef INNER_TIMER auto t2 = std::chrono::high_resolution_clock::now(); auto nth = std::chrono::duration_cast(t2 - t1).count(); stats["nth"].emplace_back(nth); constexpr int STAT_PERIOD = 100000; if (!stats["nth"].empty() && stats["nth"].size() % STAT_PERIOD == 0) { for (auto& it : stats) { const std::string& key = it.first; std::vector& stat = it.second; int64_t sum_ = std::accumulate(stat.begin(), stat.end(), 0); int64_t num_ = stat.size(); stat.clear(); std::cout << "inner_" << key << "(ns): sum=" << sum_ << " num=" << num_ << " ave=" << 1.0 * sum_ / num_ << " ave*n=" << 1.0 * sum_ / STAT_PERIOD << std::endl; } } #endif closest_pt.clear(); for (auto& it : candidates) { closest_pt.emplace_back(it.Get()); } return closest_pt.empty() == false; } template size_t IVox::NumValidGrids() const { return grids_map_.size(); } template size_t IVox::NumPoints() const { int ret = 0; for (auto& g : grids_map_) { ret += g.second->second.Size(); } return ret; } template void IVox::GenerateNearbyGrids() { if (options_.nearby_type_ == NearbyType::CENTER) { nearby_grids_.emplace_back(KeyType::Zero()); } else if (options_.nearby_type_ == NearbyType::NEARBY6) { nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0), KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)}; } else if (options_.nearby_type_ == NearbyType::NEARBY18) { nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0), KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1), KeyType(1, 1, 0), KeyType(-1, 1, 0), KeyType(1, -1, 0), KeyType(-1, -1, 0), KeyType(1, 0, 1), KeyType(-1, 0, 1), KeyType(1, 0, -1), KeyType(-1, 0, -1), KeyType(0, 1, 1), KeyType(0, -1, 1), KeyType(0, 1, -1), KeyType(0, -1, -1)}; } else if (options_.nearby_type_ == NearbyType::NEARBY26) { nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0), KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1), KeyType(1, 1, 0), KeyType(-1, 1, 0), KeyType(1, -1, 0), KeyType(-1, -1, 0), KeyType(1, 0, 1), KeyType(-1, 0, 1), KeyType(1, 0, -1), KeyType(-1, 0, -1), KeyType(0, 1, 1), KeyType(0, -1, 1), KeyType(0, 1, -1), KeyType(0, -1, -1), KeyType(1, 1, 1), KeyType(-1, 1, 1), KeyType(1, -1, 1), KeyType(1, 1, -1), KeyType(-1, -1, 1), KeyType(-1, 1, -1), KeyType(1, -1, -1), KeyType(-1, -1, -1)}; } else { LOG(ERROR) << "Unknown nearby_type!"; } } template bool IVox::GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud) { std::vector index(cloud.size()); for (int i = 0; i < cloud.size(); ++i) { index[i] = i; } closest_cloud.resize(cloud.size()); std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&cloud, &closest_cloud, this](size_t idx) { PointType pt; if (GetClosestPoint(cloud[idx], pt)) { closest_cloud[idx] = pt; } else { closest_cloud[idx] = PointType(); } }); return true; } template void IVox::AddPoints(const PointVector& points_to_add) { std::for_each(points_to_add.begin(), points_to_add.end(), [this](const auto& pt) { auto key = Pos2Grid(math::ToEigen(pt)); auto iter = grids_map_.find(key); if (iter == grids_map_.end()) { PointType center; center.getVector3fMap() = key.template cast() * options_.resolution_; grids_cache_.push_front({key, NodeType(center, options_.resolution_)}); grids_map_.insert({key, grids_cache_.begin()}); grids_cache_.front().second.InsertPoint(pt); if (grids_map_.size() >= options_.capacity_) { grids_map_.erase(grids_cache_.back().first); grids_cache_.pop_back(); } } else { iter->second->second.InsertPoint(pt); grids_cache_.splice(grids_cache_.begin(), grids_cache_, iter->second); grids_map_[key] = grids_cache_.begin(); } }); } template Eigen::Matrix IVox::Pos2Grid(const IVox::PtType& pt) const { return (pt * options_.inv_resolution_).array().round().template cast(); } template std::vector IVox::StatGridPoints() const { int num = grids_cache_.size(), valid_num = 0, max = 0, min = 100000000; int sum = 0, sum_square = 0; for (auto& it : grids_cache_) { int s = it.second.Size(); valid_num += s > 0; max = s > max ? s : max; min = s < min ? s : min; sum += s; sum_square += s * s; } float ave = float(sum) / num; float stddev = num > 1 ? sqrt((float(sum_square) - num * ave * ave) / (num - 1)) : 0; return std::vector{float(valid_num), ave, float(max), float(min), stddev}; } } // namespace lightning ================================================ FILE: src/core/ivox3d/ivox3d_node.hpp ================================================ #include #include #include #include #include #include "core/ivox3d/hilbert.hpp" #include "core/lightning_math.hpp" namespace lightning { template class IVoxNode { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW struct DistPoint; IVoxNode() = default; IVoxNode(const PointT& center, const float& side_length) {} /// same with phc void InsertPoint(const PointT& pt); inline bool Empty() const; inline std::size_t Size() const; inline PointT GetPoint(const std::size_t idx) const; int KNNPointByCondition(std::vector& dis_points, const PointT& point, const int& K, const double& max_range); private: std::vector points_; }; template class IVoxNodePhc { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW struct DistPoint; struct PhcCube; IVoxNodePhc() = default; IVoxNodePhc(const PointT& center, const float& side_length, const int& phc_order = 6); void InsertPoint(const PointT& pt); void ErasePoint(const PointT& pt, const double erase_distance_th_); inline bool Empty() const; inline std::size_t Size() const; PointT GetPoint(const std::size_t idx) const; bool NNPoint(const PointT& cur_pt, DistPoint& dist_point) const; int KNNPointByCondition(std::vector& dis_points, const PointT& cur_pt, const int& K = 5, const double& max_range = 5.0); private: uint32_t CalculatePhcIndex(const PointT& pt) const; private: std::vector phc_cubes_; PointT center_; float side_length_ = 0; int phc_order_ = 6; float phc_side_length_ = 0; float phc_side_length_inv_ = 0; Eigen::Matrix min_cube_; }; template struct IVoxNode::DistPoint { double dist = 0; IVoxNode* node = nullptr; int idx = 0; DistPoint() = default; DistPoint(const double d, IVoxNode* n, const int i) : dist(d), node(n), idx(i) {} PointT Get() { return node->GetPoint(idx); } inline bool operator()(const DistPoint& p1, const DistPoint& p2) { return p1.dist < p2.dist; } inline bool operator<(const DistPoint& rhs) { return dist < rhs.dist; } }; template void IVoxNode::InsertPoint(const PointT& pt) { points_.template emplace_back(pt); if (points_.size() >= 10) { points_.erase(points_.begin()); } } template bool IVoxNode::Empty() const { return points_.empty(); } template std::size_t IVoxNode::Size() const { return points_.size(); } template PointT IVoxNode::GetPoint(const std::size_t idx) const { return points_[idx]; } template int IVoxNode::KNNPointByCondition(std::vector& dis_points, const PointT& point, const int& K, const double& max_range) { std::size_t old_size = dis_points.size(); // #define INNER_TIMER #ifdef INNER_TIMER static std::unordered_map> stats; if (stats.empty()) { stats["dis"] = std::vector(); stats["put"] = std::vector(); stats["nth"] = std::vector(); } #endif for (const auto& pt : points_) { #ifdef INNER_TIMER auto t0 = std::chrono::high_resolution_clock::now(); #endif double d = math::distance2(pt, point); #ifdef INNER_TIMER auto t1 = std::chrono::high_resolution_clock::now(); #endif if (d < max_range * max_range) { dis_points.template emplace_back(DistPoint(d, this, &pt - points_.data())); } #ifdef INNER_TIMER auto t2 = std::chrono::high_resolution_clock::now(); auto dis = std::chrono::duration_cast(t1 - t0).count(); stats["dis"].emplace_back(dis); auto put = std::chrono::duration_cast(t2 - t1).count(); stats["put"].emplace_back(put); #endif } #ifdef INNER_TIMER auto t1 = std::chrono::high_resolution_clock::now(); #endif // sort by distance if (old_size + K >= dis_points.size()) { } else { std::nth_element(dis_points.begin() + old_size, dis_points.begin() + old_size + K - 1, dis_points.end()); dis_points.resize(old_size + K); } #ifdef INNER_TIMER auto t2 = std::chrono::high_resolution_clock::now(); auto nth = std::chrono::duration_cast(t2 - t1).count(); stats["nth"].emplace_back(nth); constexpr int STAT_PERIOD = 100000; if (!stats["nth"].empty() && stats["nth"].size() % STAT_PERIOD == 0) { for (auto& it : stats) { const std::string& key = it.first; std::vector& stat = it.second; int64_t sum_ = std::accumulate(stat.begin(), stat.end(), 0); int64_t num_ = stat.size(); stat.clear(); std::cout << "inner_" << key << "(ns): sum=" << sum_ << " num=" << num_ << " ave=" << 1.0 * sum_ / num_ << " ave*n=" << 1.0 * sum_ / STAT_PERIOD << std::endl; } } #endif return dis_points.size(); } template struct IVoxNodePhc::DistPoint { double dist = 0; IVoxNodePhc* node = nullptr; int idx = 0; DistPoint() {} DistPoint(const double d, IVoxNodePhc* n, const int i) : dist(d), node(n), idx(i) {} PointT Get() { return node->GetPoint(idx); } inline bool operator()(const DistPoint& p1, const DistPoint& p2) { return p1.dist < p2.dist; } inline bool operator<(const DistPoint& rhs) { return dist < rhs.dist; } }; template struct IVoxNodePhc::PhcCube { uint32_t idx = 0; pcl::CentroidPoint mean; PhcCube(uint32_t index, const PointT& pt) { mean.add(pt); } void AddPoint(const PointT& pt) { mean.add(pt); } PointT GetPoint() const { PointT pt; mean.get(pt); return std::move(pt); } }; template IVoxNodePhc::IVoxNodePhc(const PointT& center, const float& side_length, const int& phc_order) : center_(center), side_length_(side_length), phc_order_(phc_order) { assert(phc_order <= 8); phc_side_length_ = side_length_ / (std::pow(2, phc_order_)); phc_side_length_inv_ = (std::pow(2, phc_order_)) / side_length_; min_cube_ = center_.getArray3fMap() - side_length / 2.0; phc_cubes_.reserve(64); } template void IVoxNodePhc::InsertPoint(const PointT& pt) { uint32_t idx = CalculatePhcIndex(pt); PhcCube cube{idx, pt}; auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube, [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; }); if (it == phc_cubes_.end()) { phc_cubes_.emplace_back(cube); } else { if (it->idx == idx) { it->AddPoint(pt); } else { phc_cubes_.insert(it, cube); } } } template void IVoxNodePhc::ErasePoint(const PointT& pt, const double erase_distance_th_) { uint32_t idx = CalculatePhcIndex(pt); PhcCube cube{idx, pt}; auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube, [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; }); if (erase_distance_th_ > 0) { } if (it != phc_cubes_.end() && it->idx == idx) { phc_cubes_.erase(it); } } template bool IVoxNodePhc::Empty() const { return phc_cubes_.empty(); } template std::size_t IVoxNodePhc::Size() const { return phc_cubes_.size(); } template PointT IVoxNodePhc::GetPoint(const std::size_t idx) const { return phc_cubes_[idx].GetPoint(); } template bool IVoxNodePhc::NNPoint(const PointT& cur_pt, DistPoint& dist_point) const { if (phc_cubes_.empty()) { return false; } uint32_t cur_idx = CalculatePhcIndex(cur_pt); PhcCube cube{cur_idx, cur_pt}; auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube, [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; }); if (it == phc_cubes_.end()) { it--; dist_point = DistPoint(distance2(cur_pt, it->GetPoint()), this, it - phc_cubes_.begin()); } else if (it == phc_cubes_.begin()) { dist_point = DistPoint(distance2(cur_pt, it->GetPoint()), this, it - phc_cubes_.begin()); } else { auto last_it = it; last_it--; double d1 = distance2(cur_pt, it->GetPoint()); double d2 = distance2(cur_pt, last_it->GetPoint()); if (d1 > d2) { dist_point = DistPoint(d2, this, it - phc_cubes_.begin()); } else { dist_point = DistPoint(d1, this, it - phc_cubes_.begin()); } } return true; } template int IVoxNodePhc::KNNPointByCondition(std::vector& dis_points, const PointT& cur_pt, const int& K, const double& max_range) { uint32_t cur_idx = CalculatePhcIndex(cur_pt); PhcCube cube{cur_idx, cur_pt}; auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube, [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; }); const int max_search_cube_side_length = std::pow(2, std::ceil(std::log2(max_range * phc_side_length_inv_))); const int max_search_idx_th = 8 * max_search_cube_side_length * max_search_cube_side_length * max_search_cube_side_length; auto create_dist_point = [&cur_pt, this](typename std::vector::const_iterator forward_it) { double d = distance2(forward_it->GetPoint(), cur_pt); return DistPoint(d, this, forward_it - phc_cubes_.begin()); }; typename std::vector::const_iterator forward_it(it); typename std::vector::const_reverse_iterator backward_it(it); if (it != phc_cubes_.end()) { dis_points.emplace_back(create_dist_point(it)); forward_it++; } if (backward_it != phc_cubes_.rend()) { backward_it++; } auto forward_reach_boundary = [&]() { return forward_it == phc_cubes_.end() || forward_it->idx - cur_idx > max_search_idx_th; }; auto backward_reach_boundary = [&]() { return backward_it == phc_cubes_.rend() || cur_idx - backward_it->idx > max_search_idx_th; }; while (!forward_reach_boundary() && !backward_reach_boundary()) { if (forward_it->idx - cur_idx > cur_idx - backward_it->idx) { dis_points.emplace_back(create_dist_point(forward_it)); forward_it++; } else { dis_points.emplace_back(create_dist_point(backward_it.base())); backward_it++; } if (dis_points.size() > K) { break; } } if (forward_reach_boundary()) { while (!backward_reach_boundary() && dis_points.size() < K) { dis_points.emplace_back(create_dist_point(backward_it.base())); backward_it++; } } if (backward_reach_boundary()) { while (!forward_reach_boundary() && dis_points.size() < K) { dis_points.emplace_back(create_dist_point(forward_it)); forward_it++; } } return dis_points.size(); } template uint32_t IVoxNodePhc::CalculatePhcIndex(const PointT& pt) const { Eigen::Matrix eposf = (pt.getVector3fMap() - min_cube_) * phc_side_length_inv_; Eigen::Matrix eposi = eposf.template cast(); for (int i = 0; i < dim; ++i) { if (eposi(i, 0) < 0) { eposi(i, 0) = 0; } if (eposi(i, 0) > std::pow(2, phc_order_)) { eposi(i, 0) = std::pow(2, phc_order_) - 1; } } std::array apos{eposi(0), eposi(1), eposi(2)}; std::array tmp = hilbert::v2::PositionToIndex(apos); uint32_t idx = (uint32_t(tmp[0]) << 16) + (uint32_t(tmp[1]) << 8) + (uint32_t(tmp[2])); return idx; } } // namespace lightning ================================================ FILE: src/core/lightning_math.hpp ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_LIGHTNING_MATH_HPP #define LIGHTNING_LIGHTNING_MATH_HPP #pragma once #include #include #include #include #include #include #include #include #include #include "common/eigen_types.h" #include "common/options.h" #include "common/point_def.h" #include "common/pose_rpy.h" namespace lightning::math { template inline Eigen::Matrix SKEW_SYM_MATRIX(const Eigen::Matrix& v) { Eigen::Matrix m; m << 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0; return m; } template inline Eigen::Matrix SKEW_SYM_MATRIX(const T& v1, const T& v2, const T& v3) { Eigen::Matrix m; m << 0.0, -v3, v2, v3, 0.0, -v1, -v2, v1, 0.0; return m; } template Eigen::Matrix Exp(const Eigen::Matrix&& ang) { T ang_norm = ang.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_norm > 0.0000001) { Eigen::Matrix r_axis = ang / ang_norm; Eigen::Matrix K; K = SKEW_SYM_MATRIX(r_axis); return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const Eigen::Matrix& ang_vel, const Ts& dt) { T ang_vel_norm = ang_vel.norm(); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (ang_vel_norm > 0.0000001) { Eigen::Matrix r_axis = ang_vel / ang_vel_norm; Eigen::Matrix K; K = SKEW_SYM_MATRIX(r_axis); T r_ang = ang_vel_norm * dt; /// Roderigous Tranformation return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; } else { return Eye3; } } template Eigen::Matrix Exp(const T& v1, const T& v2, const T& v3) { T&& norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); if (norm > 0.00001) { Eigen::Matrix K; K = SKEW_SYM_MATRIX(v1 / norm, v2 / norm, v3 / norm); /// Roderigous Tranformation return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; } else { return Eye3; } } /* Logrithm of a Rotation Matrix */ template Eigen::Matrix Log(const Eigen::Matrix& R) { T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); Eigen::Matrix K(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); } template Eigen::Matrix RotMtoEuler(const Eigen::Matrix& rot) { T sy = sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)); bool singular = sy < 1e-6; T x, y, z; if (!singular) { x = atan2(rot(2, 1), rot(2, 2)); y = atan2(-rot(2, 0), sy); z = atan2(rot(1, 0), rot(0, 0)); } else { x = atan2(-rot(1, 2), rot(1, 1)); y = atan2(-rot(2, 0), sy); z = 0; } Eigen::Matrix ang(x, y, z); return ang; } // template // Eigen::Matrix RpyToRotM(const T r, const T p, const T y) { // auto q = tf::createQuaternionFromRPY(r, p, y); // Eigen::Quaterniond res; // tf::quaternionTFToEigen(q, res); // return res.toRotationMatrix().cast(); // } /// xt: 经验证，此转换与ros中tf::createQuaternionFromRPY结果一致 template Eigen::Matrix RpyToRotM2(const T r, const T p, const T y) { using AA = Eigen::AngleAxis; using Vec3 = Eigen::Matrix; return Eigen::Matrix(AA(y, Vec3::UnitZ()) * AA(p, Vec3::UnitY()) * AA(r, Vec3::UnitX())); } template inline Eigen::Matrix VecFromArray(const std::vector& v) { return Eigen::Matrix(v[0], v[1], v[2]); } template inline Eigen::Matrix VecFromArray(const boost::array& v) { return Eigen::Matrix(v[0], v[1], v[2]); } template inline Eigen::Matrix MatFromArray(const std::vector& v) { Eigen::Matrix m; m << v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]; return m; } template inline Eigen::Matrix MatFromArray(const boost::array& v) { Eigen::Matrix m; m << v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]; return m; } template T rad2deg(const T& radians) { return radians * 180.0 / M_PI; } template T deg2rad(const T& degrees) { return degrees * M_PI / 180.0; } /** * 将某个数限定在范围内 * @tparam T * @param num * @param min_limit * @param max_limit * @return */ template void limit_in_range(T&& num, T2&& min_limit, T2&& max_limit) { if (num < min_limit) { num = min_limit; } if (num >= max_limit) { num = max_limit; } } /** * 计算均值与对角线形式的协方差 * @tparam C 数据类型 * @tparam D 均值的数据类型 * @tparam Getter 获取被计算的字段 * @param data 数据的某种容器 * @param mean 均值 * @param cov_diag 对角形式的协方差阵j * @param getter 从数据中获取被计算字段的函数 */ template inline void ComputeMeanAndCovDiag(const C& data, D& mean, D& cov_diag, Getter&& getter) { size_t len = data.size(); if (len == 1) { mean = getter(data[0]); cov_diag = D::Zero().eval(); } else { // clang-format off mean = std::accumulate(data.begin(), data.end(), D::Zero().eval(), [&getter](const D& sum, const auto& data) -> D { return sum + getter(data); }) / len; cov_diag = std::accumulate(data.begin(), data.end(), D::Zero().eval(), [&mean, &getter](const D& sum, const auto& data) -> D { return sum + (getter(data) - mean).cwiseAbs2(); }) / (len); // clang-format on } } /** * 高斯分布的增量更新 * @see https://www.cnblogs.com/yoyaprogrammer/p/delta_variance.html * @param hist_n 历史数据个数 * @param hist_mean 历史均值 * @param hist_var2 历史方差 * @param curr_n 当前数据个数 * @param curr_mean 当前真值 * @param curr_var2 当前方差 * @param new_mean 合并后均值 * @param new_var2 合并后方差 */ inline void HistoryMeanAndVar(size_t hist_n, float hist_mean, float hist_var2, size_t curr_n, float curr_mean, float curr_var2, float& new_mean, float& new_var2) { new_mean = (hist_n * hist_mean + curr_n * curr_mean) / (hist_n + curr_n); new_var2 = (hist_n * (hist_var2 + (new_mean - hist_mean) * (new_mean - hist_mean)) + curr_n * (curr_var2 + (new_mean - curr_mean) * (new_mean - curr_mean))) / (hist_n + curr_n); } /** * Calculate cosine and sinc of sqrt(x2). * @param x2 the squared angle must be non-negative * @return a pair containing cos and sinc of sqrt(x2) */ template inline std::pair cos_sinc_sqrt(const scalar& x2) { using std::cos; using std::sin; using std::sqrt; static scalar const taylor_0_bound = boost::math::tools::epsilon(); static scalar const taylor_2_bound = sqrt(taylor_0_bound); static scalar const taylor_n_bound = sqrt(taylor_2_bound); assert(x2 >= 0 && "argument must be non-negative"); // FIXME check if bigger bounds are possible if (x2 >= taylor_n_bound) { // slow fall-back solution scalar x = sqrt(x2); return std::make_pair(cos(x), sin(x) / x); // x is greater than 0. } // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can // be calculated in parallel using SSE2 mulpd/addpd) // TODO Find optimal coefficients using Remez algorithm static scalar const inv[] = {1 / 3., 1 / 4., 1 / 5., 1 / 6., 1 / 7., 1 / 8., 1 / 9.}; scalar cosi = 1., sinc = 1; scalar term = -1 / 2. * x2; for (int i = 0; i < 3; ++i) { cosi += term; term *= inv[2 * i]; sinc += term; term *= -inv[2 * i + 1] * x2; } return std::make_pair(cosi, sinc); } inline SO3 exp(const Vec3d& vec, const double& scale = 1) { double norm2 = vec.squaredNorm(); std::pair cos_sinc = cos_sinc_sqrt(scale * scale * norm2); double mult = cos_sinc.second * scale; Vec3d result = mult * vec; return SO3(Quatd(cos_sinc.first, result[0], result[1], result[2])); } inline Eigen::Matrix PseudoInverse(const Eigen::Matrix& X) { Eigen::JacobiSVD> svd(X, Eigen::ComputeFullU | Eigen::ComputeFullV); Vec2d sv = svd.singularValues(); Eigen::Matrix U = svd.matrixU().block<3, 2>(0, 0); Eigen::Matrix V = svd.matrixV(); Eigen::Matrix U_adjoint = U.adjoint(); double tolerance = std::numeric_limits::epsilon() * 3 * std::abs(sv(0, 0)); sv(0, 0) = std::abs(sv(0, 0)) > tolerance ? 1.0 / sv(0, 0) : 0; sv(1, 0) = std::abs(sv(1, 0)) > tolerance ? 1.0 / sv(1, 0) : 0; return V * sv.asDiagonal() * U_adjoint; } /** * SO3 Jl()/JacobianL() * @param v * @return */ inline Eigen::Matrix A_matrix(const Vec3d& v) { Eigen::Matrix res; double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; double norm = std::sqrt(squaredNorm); if (norm < 1e-5) { res = Eigen::Matrix::Identity(); } else { res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * SO3::hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * SO3::hat(v) * SO3::hat(v); } return res; } /// SO3 Jlinv() inline Eigen::Matrix A_inv(const Vec3d& v) { Eigen::Matrix res; if (v.norm() > 1e-5) { res = Eigen::Matrix::Identity() - 0.5 * SKEW_SYM_MATRIX(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * SKEW_SYM_MATRIX(v) * SKEW_SYM_MATRIX(v) / v.squaredNorm(); } else { // res = Eigen::Matrix::Identity(); res = Eigen::Matrix::Identity() - 0.5 * SKEW_SYM_MATRIX(v) + ((1. / 12) + v.squaredNorm() * 1. / 720) * SKEW_SYM_MATRIX(v) * SKEW_SYM_MATRIX(v); } return res; } /// hash of vector template struct hash_vec { inline size_t operator()(const Eigen::Matrix& v) const; }; /// vec 2 hash /// @see Optimized Spatial Hashing for Collision Detection of Deformable Objects, Matthias Teschner et. al., VMV 2003 template <> inline size_t hash_vec<2>::operator()(const Eigen::Matrix& v) const { return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943)) % 10000000; } /// vec 3 hash template <> inline size_t hash_vec<3>::operator()(const Eigen::Matrix& v) const { return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943) ^ ((v[2]) * 83492791)) % 10000000; } /** * estimate a plane * @tparam T * @param pca_result * @param point * @param threshold * @return */ template inline bool esti_plane(Eigen::Matrix& pca_result, const PointVector& point, const T& threshold = 0.1f) { if (point.size() < fasterlio::MIN_NUM_MATCH_POINTS) { return false; } Eigen::Matrix normvec; if (point.size() == fasterlio::NUM_MATCH_POINTS) { Eigen::Matrix A; Eigen::Matrix b; A.setZero(); b.setOnes(); b *= -1.0f; for (int j = 0; j < fasterlio::NUM_MATCH_POINTS; j++) { A(j, 0) = point[j].x; A(j, 1) = point[j].y; A(j, 2) = point[j].z; } normvec = A.colPivHouseholderQr().solve(b); } else { Eigen::MatrixXd A(point.size(), 3); Eigen::VectorXd b(point.size(), 1); A.setZero(); b.setOnes(); b *= -1.0f; for (int j = 0; j < point.size(); j++) { A(j, 0) = point[j].x; A(j, 1) = point[j].y; A(j, 2) = point[j].z; } Eigen::MatrixXd n = A.colPivHouseholderQr().solve(b); normvec(0, 0) = n(0, 0); normvec(1, 0) = n(1, 0); normvec(2, 0) = n(2, 0); } T n = normvec.norm(); pca_result(0) = normvec(0) / n; pca_result(1) = normvec(1) / n; pca_result(2) = normvec(2) / n; pca_result(3) = 1.0 / n; for (const auto& p : point) { Eigen::Matrix temp = p.getVector4fMap(); temp[3] = 1.0; if (fabs(pca_result.dot(temp)) > threshold) { return false; } } return true; } /// 体素滤波 inline CloudPtr VoxelGrid(CloudPtr cloud, float voxel_size = 0.05) { pcl::VoxelGrid voxel; voxel.setLeafSize(voxel_size, voxel_size, voxel_size); voxel.setInputCloud(cloud); CloudPtr output(new PointCloudType); voxel.filter(*output); return output; } /// pcl 时间戳 inline double ToSec(uint64_t t) { return double(t) * 1e-9; } // convert from pcl point to eigen template inline Eigen::Matrix ToEigen(const PointType& pt) { return Eigen::Matrix(pt.x, pt.y, pt.z); } template <> inline Eigen::Matrix ToEigen(const pcl::PointXYZ& pt) { return pt.getVector3fMap(); } template <> inline Eigen::Matrix ToEigen(const PointXYZIT& pt) { return Vec2f(pt.x, pt.y); } template <> inline Eigen::Matrix ToEigen(const pcl::PointXYZI& pt) { return pt.getVector3fMap(); } template <> inline Eigen::Matrix ToEigen(const pcl::PointXYZINormal& pt) { return pt.getVector3fMap(); } /** * squared distance * @param p1 * @param p2 * @return */ inline float calc_dist(const PointType& p1, const PointType& p2) { return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); } inline float calc_dist(const Eigen::Vector3f& p1, const Eigen::Vector3f& p2) { return (p1 - p2).squaredNorm(); } // squared distance of two pcl points template inline double distance2(const PointT& pt1, const PointT& pt2) { Eigen::Vector3f d = pt1.getVector3fMap() - pt2.getVector3fMap(); return d.squaredNorm(); } template inline Eigen::Matrix Mat4FromArray(const std::vector& v) { Eigen::Matrix m; for (int i = 0; i < m.size(); ++i) m(i / 4, i % 4) = v[i]; return m; } /// 矢量比较 template struct less_vec { inline bool operator()(const Eigen::Matrix& v1, const Eigen::Matrix& v2) const; }; // 实现2D和3D的比较 template <> inline bool less_vec<2>::operator()(const Eigen::Matrix& v1, const Eigen::Matrix& v2) const { return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]); } template <> inline bool less_vec<3>::operator()(const Eigen::Matrix& v1, const Eigen::Matrix& v2) const { return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]) || (v1[0] == v2[0] && v1[1] == v2[1] && v1[2] < v2[2]); } /** * 计算一个容器内数据的均值与矩阵形式协方差 * @tparam C 容器类型 * @tparam int 　数据维度 * @tparam Getter 获取数据函数, 接收一个容器内数据类型，返回一个Eigen::Matrix 矢量类型 */ template void ComputeMeanAndCov(const C& data, Eigen::Matrix& mean, Eigen::Matrix& cov, Getter&& getter) { using D = Eigen::Matrix; using E = Eigen::Matrix; size_t len = data.size(); assert(len > 1); // clang-format off mean = std::accumulate(data.begin(), data.end(), Eigen::Matrix::Zero().eval(), [&getter](const D& sum, const auto& data) -> D { return sum + getter(data); }) / len; cov = std::accumulate(data.begin(), data.end(), E::Zero().eval(), [&mean, &getter](const E& sum, const auto& data) -> E { D v = getter(data) - mean; return sum + v * v.transpose(); }) / (len - 1); // clang-format on } /** * 高斯分布合并 * @tparam S scalar type * @tparam D dimension * @param hist_m 历史点数 * @param curr_n 当前点数 * @param hist_mean 历史均值 * @param hist_var 历史方差 * @param curr_mean 当前均值 * @param curr_var 当前方差 * @param new_mean 新的均值 * @param new_var 新的方差 */ template void UpdateMeanAndCov(int hist_m, int curr_n, const Eigen::Matrix& hist_mean, const Eigen::Matrix& hist_var, const Eigen::Matrix& curr_mean, const Eigen::Matrix& curr_var, Eigen::Matrix& new_mean, Eigen::Matrix& new_var) { assert(hist_m > 0); assert(curr_n > 0); new_mean = (hist_m * hist_mean + curr_n * curr_mean) / (hist_m + curr_n); new_var = (hist_m * (hist_var + (hist_mean - new_mean) * (hist_mean - new_mean).template transpose()) + curr_n * (curr_var + (curr_mean - new_mean) * (curr_mean - new_mean).template transpose())) / (hist_m + curr_n); } /// 将角度保持在正负PI以内 inline void KeepAngleInPI(double& angle) { while (angle < -M_PI) { angle = angle + 2 * M_PI; } while (angle > M_PI) { angle = angle - 2 * M_PI; } } inline void KeepAngleIn2PI(double& angle) { while (angle <= 0) { angle = angle + 2 * M_PI; } while (angle > 2 * M_PI) { angle = angle - 2 * M_PI; } } inline builtin_interfaces::msg::Time FromSec(double t) { builtin_interfaces::msg::Time ret; ret.sec = int32_t(t); ret.nanosec = int32_t((t - ret.sec) * 1e9); return ret; } /// 从pose中取出yaw pitch roll /// 使用的顺序是：roll pitch yaw ，最左是roll /// 由于欧拉角的多解性，Eigen在分解时选择最小化第一个（也就是roll）。否则，如果选择最小化yaw，那么roll和pitch可能出现大范围旋转，不符合实际 /// @see https://stackoverflow.com/questions/33895970/about-eulerangles-conversion-from-eigen-c-library /// 这个用在lego-loam的lidar odom里，符合它的定义 /// 2020.11 change back to tf to keep consist inline PoseRPYD SE3ToRollPitchYaw(const SE3& pose) { auto rot = pose.rotationMatrix(); tf2::Matrix3x3 temp_tf_matrix(rot(0, 0), rot(0, 1), rot(0, 2), rot(1, 0), rot(1, 1), rot(1, 2), rot(2, 0), rot(2, 1), rot(2, 2)); PoseRPYD output; output.x = pose.translation()[0]; output.y = pose.translation()[1]; output.z = pose.translation()[2]; temp_tf_matrix.getRPY(output.roll, output.pitch, output.yaw); return output; } /// 从xyz和yaw pitch roll转成SE3 /// 使用0,1,2顺序（从右读）,yaw最先旋转 inline SE3 XYZRPYToSE3(const PoseRPYD& pose) { using AA = Eigen::AngleAxisd; return SE3((AA(pose.yaw, Vec3d::UnitZ()) * AA(pose.pitch, Vec3d::UnitY()) * AA(pose.roll, Vec3d::UnitX())), Vec3d(pose.x, pose.y, pose.z)); } /** * pose 插值算法 * @tparam T 数据类型 * @tparam C 数据容器类型 * @tparam FT 获取时间函数 * @tparam FP 获取pose函数 * @param query_time 查找时间 * @param data 数据容器 * @param take_pose_func 从数据中取pose的谓词，接受一个数据，返回一个SE3 * @param result 查询结果 * @param best_match_iter 查找到的最近匹配 * * NOTE 要求query_time必须在data最大时间和最小时间之间(容许0.5s内误差) * data的map按时间排序 * @return */ template inline bool PoseInterp(double query_time, C&& data, FT&& take_time_func, FP&& take_pose_func, SE3& result, T& best_match, float time_th = 0.5, bool verbose = false) { if (data.size() <= 1) { if (verbose) { LOG(INFO) << "data size is too small for interp: " << data.size(); } return false; } double last_time = take_time_func(*data.rbegin()); double first_time = take_time_func(*data.begin()); if (query_time > last_time) { if (verbose) { LOG(WARNING) << "query time is later than last time."; } if (query_time < (last_time + time_th)) { // 如果时间差在0.5s之内，尚可接受 // 统一改成外推形式 if (data.size() == 1) { result = take_pose_func(*data.rbegin()); best_match = *data.rbegin(); // std::stringstream ss; // ss << ", r: " << result.translation().transpose(); // LOG_I(ss.str()); return true; } else { auto second = data.rbegin(); auto first = second; ++first; result = take_pose_func(*second); // 姿态取最后一帧的姿态 double time_first = take_time_func(*first); double time_second = take_time_func(*second); /// 防止末尾两个时间相等 if (time_second <= time_first) { result = take_pose_func(*data.rbegin()); best_match = *data.rbegin(); if (verbose) { // LOG_I("r: {}", result.translation().transpose()); } return true; } /// 防止末尾两个时间太近 while ((time_second - time_first) < 0.05) { first++; if (first == data.rend()) { result = take_pose_func(*data.rbegin()); best_match = *data.rbegin(); if (verbose) { // LOG_I("r: {}", result.translation().transpose()); } return true; } time_first = take_time_func(*first); } SE3 pose_first = take_pose_func(*first); SE3 pose_second = take_pose_func(*second); Vec6d xi = (pose_first.inverse() * pose_second).log(); double s = (query_time - time_second) / (time_second - time_first); result = result * SE3::exp(s * xi); best_match = *data.rbegin(); if (verbose) { // LOG_I("f: {}, s: {}, r: {}", pose_first.translation().transpose(), // pose_second.translation().transpose(), result.translation().transpose()); } return true; } } if (verbose) { LOG(WARNING) << "pose interp failed: " << query_time << ", " << (last_time + time_th) << ", " << (query_time < (last_time + time_th)); } return false; } else if (query_time < first_time) { if (query_time < (first_time - time_th)) { if (verbose) { LOG(WARNING) << "query time too early: " << ", " << query_time << ", " << (first_time - time_th); } return false; } else { result = take_pose_func(*data.begin()); best_match = *data.begin(); return true; } } auto match_iter = data.begin(); for (auto iter = data.begin(); iter != data.end(); ++iter) { auto next_iter = iter; next_iter++; if (next_iter == data.end() || (take_time_func(*iter) < query_time && take_time_func(*next_iter) >= query_time)) { match_iter = iter; break; } } auto match_iter_n = match_iter; match_iter_n++; if (match_iter_n == data.end() || take_time_func(*match_iter) >= query_time) { // 就一个，那就返回他 best_match = *match_iter; result = take_pose_func(*match_iter); return true; } double dt = take_time_func(*match_iter_n) - take_time_func(*match_iter); double s = (query_time - take_time_func(*match_iter)) / dt; // s=0 时为第一帧，s=1时为next // 出现了 dt为0的bug if (fabs(dt) < 1e-6) { best_match = *match_iter; result = take_pose_func(*match_iter); return true; } SE3 pose_first = take_pose_func(*match_iter); SE3 pose_next = take_pose_func(*match_iter_n); result = {pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion()), pose_first.translation() * (1 - s) + pose_next.translation() * s}; best_match = s < time_th ? *match_iter : *match_iter_n; return true; } } // namespace lightning::math #endif // LIGHTNING_MATH_HPP ================================================ FILE: src/core/lio/anderson_acceleration.h ================================================ #ifndef ANDERSONACCELERATION_H_ #define ANDERSONACCELERATION_H_ #include #include #include #include #include #include "common/eigen_types.h" namespace lightning { /** * AA 加速器 * @tparam S Scalar type * @tparam D dimension * @tparam m 允许前多少次迭代, 最多10次，不允许动态设置，只能编译期设置 */ template class AndersonAcceleration { public: using Scalar = S; using Vec = Eigen::Matrix; using MatDM = Eigen::Matrix; using MatDD = Eigen::Matrix; /** * 为g计算加速之后的结果 * @param g 输入的更新量 * @return 加速之后的更新量 */ Vec compute(const Vec& g) { assert(iter_ >= 0); Vec G = g; current_F_ = G - current_u_; if (iter_ == 0) { prev_dF_.col(0) = -current_F_; prev_dG_.col(0) = -G; current_u_ = G; } else { prev_dF_.col(col_idx_) += current_F_; prev_dG_.col(col_idx_) += G; Scalar eps = 1e-14; Scalar scale = std::max(eps, prev_dF_.col(col_idx_).norm()); dF_scale_(col_idx_) = scale; prev_dF_.col(col_idx_) /= scale; int m_k = std::min(m, iter_); if (m_k == 1) { theta_(0) = 0; Scalar dF_sqrnorm = prev_dF_.col(col_idx_).squaredNorm(); M_(0, 0) = dF_sqrnorm; Scalar dF_norm = std::sqrt(dF_sqrnorm); if (dF_norm > eps) { theta_(0) = (prev_dF_.col(col_idx_) / dF_norm).dot(current_F_ / dF_norm); } } else { // Update the normal equation matrix, for the column and row corresponding to the new dF column VecXd new_inner_prod = (prev_dF_.col(col_idx_).transpose() * prev_dF_.block(0, 0, D, m_k)).transpose(); M_.block(col_idx_, 0, 1, m_k) = new_inner_prod.transpose(); M_.block(0, col_idx_, m_k, 1) = new_inner_prod; // Solve normal equation cod_.compute(M_.block(0, 0, m_k, m_k)); theta_.head(m_k) = cod_.solve(prev_dF_.block(0, 0, D, m_k).transpose() * current_F_); } // Use rescaled theta to compute new u current_u_ = G - prev_dG_.block(0, 0, D, m_k) * ((theta_.head(m_k).array() / dF_scale_.head(m_k).array()).matrix()); col_idx_ = (col_idx_ + 1) % D; prev_dF_.col(col_idx_) = -current_F_; prev_dG_.col(col_idx_) = -G; } iter_++; return current_u_; } /** * 重置AA加速器 * @param u */ void reset(const Vec& u) { iter_ = 0; col_idx_ = 0; current_u_ = u; } /** * 初始化 anderson acceleration * @param u0: initial variable values */ void init(const Vec& u0) { current_u_.setZero(); current_F_.setZero(); prev_dG_.setZero(); prev_dF_.setZero(); M_.setZero(); theta_.setZero(); dF_scale_.setZero(); current_u_ = u0; iter_ = 0; col_idx_ = 0; } private: Vec current_u_ = Vec::Zero(); // 当前的更新 Vec current_F_ = Vec::Zero(); MatDM prev_dG_ = MatDM::Zero(); MatDM prev_dF_ = MatDM::Zero(); MatDD M_ = MatDD::Zero(); // Normal equations matrix for the computing theta Vec theta_ = Vec::Zero(); // theta value computed from normal equations Vec dF_scale_ = Vec::Zero(); // The scaling factor for each column of prev_dF Eigen::CompleteOrthogonalDecomposition cod_; // should use MatXd because iteration changes int iter_ = 0; // Iteration count since initialization int col_idx_ = -1; // Index for history matrix column to store the next value }; } // namespace lightning #endif /* ANDERSONACCELERATION_H_ */ ================================================ FILE: src/core/lio/eskf.cc ================================================ // // Created by xiang on 2022/2/15. // #include "core/lio/eskf.hpp" #include "core/lightning_math.hpp" #include #include namespace { using CovType = lightning::ESKF::CovType; void SymmetrizeAndFloorCovariance(CovType& P, double min_cov_diag) { P = 0.5 * (P + P.transpose()).eval(); for (int i = 0; i < P.rows(); ++i) { if (P(i, i) < min_cov_diag) { P(i, i) = min_cov_diag; } else if (P(i, i) > 100.0) { P(i, i) = 100.0; } for (int j = 0; j < P.cols(); ++j) { if (std::isnan(P(i, j)) || std::isinf(P(i, j))) { LOG(WARNING) << "find nan or inf in P: " << P(i, j); P(i, j) = 1.0; } } } } } // namespace namespace lightning { void ESKF::Predict(const double& dt, const ESKF::ProcessNoiseType& Q, const Vec3d& gyro, const Vec3d& acce) { Eigen::Matrix f_ = x_.get_f(gyro, acce); // 调用get_f 获取 速度 角速度 加速度 Eigen::Matrix f_x_ = x_.df_dx(acce); Eigen::Matrix f_w_ = x_.df_dw(); Eigen::Matrix f_w_final = Eigen::Matrix::Zero(); NavState x_before = x_; x_.oplus(f_, dt); F_x1_ = CovType::Identity(); // set f_x_final CovType f_x_final = CovType::Zero(); for (auto st : x_.vect_states_) { int idx = st.idx_; int dim = st.dim_; int dof = st.dof_; for (int i = 0; i < state_dim_; i++) { for (int j = 0; j < dof; j++) { f_x_final(idx + j, i) = f_x_(dim + j, i); } } for (int i = 0; i < process_noise_dim_; i++) { for (int j = 0; j < dof; j++) { f_w_final(idx + j, i) = f_w_(dim + j, i); } } } Mat3d res_temp_SO3; Vec3d seg_SO3; for (auto st : x_.SO3_states_) { int idx = st.idx_; int dim = st.dim_; for (int i = 0; i < 3; i++) { seg_SO3(i) = -1 * f_(dim + i) * dt; } F_x1_.block<3, 3>(idx, idx) = math::exp(seg_SO3, 0.5).matrix(); res_temp_SO3 = math::A_matrix(seg_SO3); for (int i = 0; i < state_dim_; i++) { f_x_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_.block<3, 1>(dim, i)); } for (int i = 0; i < process_noise_dim_; i++) { f_w_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_.block<3, 1>(dim, i)); } } F_x1_ += f_x_final * dt; P_ = (F_x1_)*P_ * (F_x1_).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose(); P_ *= options_.predict_cov_inflation_; SymmetrizeAndFloorCovariance(P_, options_.min_cov_diag_); } /** * 原版的迭代过程中，收敛次数大于1才会结果，所以需要两次收敛。 * 在未收敛时，实际上不会计算最近邻，也就回避了一次ObsModel的计算 * 如果这边对每次迭代都计算最近邻的话，时间明显会变长一些，并不是非常合理。。 * * @param obs * @param R */ void ESKF::Update(ESKF::ObsType obs, const double& R) { custom_obs_model_.valid_ = true; custom_obs_model_.converge_ = true; CovType P_propagated = P_; Eigen::Matrix K_r; Eigen::Matrix K_H; StateVecType dx_current = StateVecType::Zero(); // 本轮迭代的dx NavState start_x = x_; // 迭代的起点 NavState last_x = x_; int converged_times = 0; double last_lidar_res = 0; double init_res = 0.0; static double iterated_num = 0; static double update_num = 0; update_num += 1; for (int i = -1; i < maximum_iter_; i++) { custom_obs_model_.valid_ = true; /// 计算observation function，主要是residual_, h_x_, s_ /// x_ 在每次迭代中都是更新的，线性化点也会更新 if (obs == ObsType::LIDAR || obs == ObsType::WHEEL_SPEED_AND_LIDAR) { lidar_obs_func_(x_, custom_obs_model_); } else if (obs == ObsType::WHEEL_SPEED) { wheelspeed_obs_func_(x_, custom_obs_model_); } else if (obs == ObsType::ACC_AS_GRAVITY) { acc_as_gravity_obs_func_(x_, custom_obs_model_); } else if (obs == ObsType::GPS) { gps_obs_func_(x_, custom_obs_model_); } else if (obs == ObsType::BIAS) { bias_obs_func_(x_, custom_obs_model_); } if (custom_obs_model_.valid_ == false) { x_ = last_x; P_ = P_propagated; return; } if (use_aa_ && i > -1 && (obs == ObsType::LIDAR || obs == ObsType::WHEEL_SPEED_AND_LIDAR) && custom_obs_model_.lidar_residual_mean_ >= last_lidar_res * 1.01) { x_ = last_x; break; } iterated_num += 1; if (!custom_obs_model_.valid_) { continue; } if (i == -1) { init_res = custom_obs_model_.lidar_residual_mean_; if (init_res < 1e-9) { init_res = 1e-9; // 可能有零 } } iterations_ = i + 2; // i从-1开始计 final_res_ = custom_obs_model_.lidar_residual_mean_ / init_res; StateVecType dx = x_.boxminus(start_x); // 当前x与起点之间的dx dx_current = dx; // P_ = P_propagated; /// 更新P 和 dx /// P = J*P*J^T /// dx = J * dx for (auto it : x_.SO3_states_) { int idx = it.idx_; Vec3d seg_SO3 = dx.block<3, 1>(idx, 0); Mat3d res_temp_SO3 = math::A_matrix(seg_SO3).transpose(); // 小块的J阵, SO3上的雅可比？ dx_current.block<3, 1>(idx, 0) = res_temp_SO3 * dx.block<3, 1>(idx, 0); /// P 上面有SO3的行 进行转换 for (int j = 0; j < state_dim_; j++) { P_.block<3, 1>(idx, j) = res_temp_SO3 * (P_.block<3, 1>(idx, j)); } /// P 上面有SO3的列 进行转换 for (int j = 0; j < state_dim_; j++) { P_.block<1, 3>(j, idx) = (P_.block<1, 3>(j, idx)) * res_temp_SO3.transpose(); } } Mat6d HTH = custom_obs_model_.HTH_; Vec6d HTr = custom_obs_model_.HTr_; Mat6d HTH_sym = 0.5 * (HTH + HTH.transpose()); Eigen::SelfAdjointEigenSolver eigen_solver(HTH_sym); if (eigen_solver.info() != Eigen::Success) { LOG(WARNING) << "Failed to decompose ESKF observation information matrix."; continue; } const Vec6d eigen_values = eigen_solver.eigenvalues(); const Mat6d eigen_vectors = eigen_solver.eigenvectors(); const double max_eigen_value = std::max(1e-12, eigen_values.maxCoeff()); const double degeneracy_threshold = max_eigen_value * options_.degeneracy_threshold_ratio_; // LOG(INFO) << "eigen values of HTH: " << eigen_values.transpose(); Vec6d observable_mask = Vec6d::Zero(); int nullity = 0; for (int k = 0; k < observable_mask.size(); ++k) { if (eigen_values(k) > degeneracy_threshold) { observable_mask(k) = 1.0; } else { nullity++; } } const Mat6d observable_projector = eigen_vectors * observable_mask.asDiagonal() * eigen_vectors.transpose(); const Mat6d HTH_eff = observable_projector * HTH_sym * observable_projector; const Vec6d HTr_eff = observable_projector * HTr; CovType P_temp = (P_ / R).inverse(); // P阵上面已经更新 /// 现在问题是这个权重太大，导致整体过于依赖先验 ... // P_temp.setIdentity(); P_temp.block(0, 0) += HTH_eff; CovType Q_inv = P_temp.inverse(); // Q inv // Q*H^T * R^-1 * r = K * r // <-- K -----> K_r = Q_inv.template block(0, 0) * HTr_eff; // K_H = Q^-1 H^T R^-1 H // <-- K -> K_H.setZero(); K_H.template block(0, 0) = Q_inv.template block(0, 0) * HTH_eff; // dx = Kr + (KH-I) dx // LOG(INFO) << "K_r: " << K_r.transpose() // << ", prior: " << ((K_H - Eigen::Matrix::Identity()) * dx_current).transpose(); dx_current = K_r + (K_H - Eigen::Matrix::Identity()) * dx_current; // check nan for (int j = 0; j < state_dim_; ++j) { if (std::isnan(dx_current(j, 0))) { return; } } // Vec3d dv = dx_current.middleRows(NavState::kVelIdx, NavState::kBlockDim); // if (dv.norm() > options_.vel_clip_norm_) { // dv = dv / dv.norm() * options_.vel_clip_norm_; // } // dv = dv * options_.dv_ratio_; // dx_current.middleRows(NavState::kVelIdx, NavState::kBlockDim) = dv; // dx_current.middleRows(18, 5).setZero(); // LOG(INFO) << "iter " << iterations_ << ", dx: " << dx_current.transpose(); const double dx_translation = dx_current.head<3>().norm(); const double dx_rotation_deg = dx_current.segment<3>(3).norm() * 180.0 / M_PI; if (dx_translation > options_.max_update_translation_step_ || dx_rotation_deg > options_.max_update_rotation_step_deg_) { LOG(ERROR) << "Reject ESKF iter update, dtrans: " << dx_translation << ", drot_deg: " << dx_rotation_deg << ", dvel: " << dx_current.segment(NavState::kVelIdx).norm(); x_ = start_x; P_ = P_propagated; return; } if (!use_aa_) { x_ = x_.boxplus(dx_current); } else { // 转到起点的线性空间 x_ = x_.boxplus(dx_current); if (i == -1) { aa_.init(dx_current); // 初始化AA } else { // 利用AA计算dx from start auto dx_all = x_.boxminus(start_x); auto new_dx_all = aa_.compute(dx_all); x_ = start_x.boxplus(new_dx_all); } } last_x = x_; // update last res last_lidar_res = custom_obs_model_.lidar_residual_mean_; custom_obs_model_.converge_ = true; for (int j = 0; j < state_dim_; j++) { if (std::fabs(dx_current[j]) > limit_[j]) { custom_obs_model_.converge_ = false; break; } } if (custom_obs_model_.converge_) { converged_times++; } if (!converged_times && i == maximum_iter_ - 2) { custom_obs_model_.converge_ = true; } if (converged_times > 0 || i == maximum_iter_ - 1) { /// 结束条件：已经收敛 /// 更新P阵, using (45) L_ = P_; Mat3d res_temp_SO3; Vec3d seg_SO3; for (auto it : x_.SO3_states_) { int idx = it.idx_; for (int j = 0; j < 3; j++) { seg_SO3(j) = dx_current(j + idx); } res_temp_SO3 = math::A_matrix(seg_SO3).transpose(); for (int j = 0; j < state_dim_; j++) { L_.block<3, 1>(idx, j) = res_temp_SO3 * (P_.block<3, 1>(idx, j)); } for (int j = 0; j < pose_obs_dim_; j++) { K_H.block<3, 1>(idx, j) = res_temp_SO3 * (K_H.block<3, 1>(idx, j)); } for (int j = 0; j < state_dim_; j++) { L_.block<1, 3>(j, idx) = (L_.block<1, 3>(j, idx)) * res_temp_SO3.transpose(); P_.block<1, 3>(j, idx) = (P_.block<1, 3>(j, idx)) * res_temp_SO3.transpose(); } } P_ = L_ - K_H.block(0, 0) * P_.template block(0, 0); if (nullity > 0) { // LOG_EVERY_N(INFO, 50) << "ESKF observation degeneracy rank " << (pose_obs_dim_ - nullity) << "/" // << pose_obs_dim_; P_.block(0, 0) *= options_.degeneracy_cov_inflation_; } break; } } SymmetrizeAndFloorCovariance(P_, options_.min_cov_diag_); } } // namespace lightning ================================================ FILE: src/core/lio/eskf.hpp ================================================ // // Created by xiang on 2022/2/15. // #ifndef FUSION_ESKF_HPP #define FUSION_ESKF_HPP #include "common/eigen_types.h" #include "common/nav_state.h" #include "core/lio/anderson_acceleration.h" namespace lightning { /** * LIO 中的ESKF重写 * * MTK * 为了实现状态量的自由组合，搞了一套宏函数来实现自定状态变量各维度的索引，然而实际当中状态变量基本是固定的，并不希望这样子用。。 * 而且宏函数无法调试，人类也看不到展开之后的宏长啥样 * 重写的版本只使用固定的NavState，内部逻辑也在NavState里面定义，不做无谓的拓展了 （并没有谁去拓展那玩意） * 固定状态：pos / rot / vel / bg */ class ESKF { public: static constexpr int process_noise_dim_ = 12; // 过程噪声维度 static constexpr int pose_obs_dim_ = 6; // 激光观测只约束位姿 static constexpr int state_dim_ = NavState::dim; // 状态维度 using StateVecType = NavState::VectState; // 状态向量类型 using CovType = Eigen::Matrix; // 协方差矩阵 using ProcessNoiseType = Eigen::Matrix; /// ESKF 观测类型 enum class ObsType { LIDAR, // 开源版本只有Lidar WHEEL_SPEED, // 单独的轮速观测 WHEEL_SPEED_AND_LIDAR, // 轮速+Lidar ACC_AS_GRAVITY, // 重力作为加计观测量 GPS, // GPS/RTk 六自由度位姿 BIAS, }; public: explicit ESKF(const NavState& x = NavState(), const CovType& P = CovType::Identity(), bool use_aa = true) : x_(x), P_(P), use_aa_(use_aa) {} ~ESKF() {} /// 自定义观测模型中的相关信息 /// 观测由观测模型+观测函数组成，模型层面存储具体的结果，观测函数负责实现各矩阵的计算 /// TODO 观测模型已知时，维度可以事先指定（但是雷达匹配点数是不定的，所以这边无法指定矩阵行数） struct CustomObservationModel { bool valid_ = true; bool converge_ = true; Eigen::Matrix R_; // R 阵，观测噪声 /// NOTE 我们还是传H^T H 比较好，光传一个H会因为残差维度不对导致没法融合各类残差 /// 这个只需累加即可 Eigen::Matrix HTH_; Eigen::Matrix HTr_; double lidar_residual_mean_ = 0; double lidar_residual_max_ = 0; }; /// 用户定义的观测模型函数，根据状态计算观测量和雅可比矩阵 using CustomObsFunction = std::function; struct Options { CustomObsFunction lidar_obs_func_; // 雷达观测函数 CustomObsFunction wheelspeed_obs_func_; // 轮速观测函数 CustomObsFunction acc_as_gravity_obs_func_; // 加计观测函数 CustomObsFunction gps_obs_func_; CustomObsFunction bias_obs_func_; int max_iterations_ = 4; StateVecType epsi_; // 收敛条件 bool use_aa_ = false; // use anderson accleration /// 速度clip double vel_clip_norm_ = 1.0; double dv_ratio_ = 0.5; double predict_cov_inflation_ = 1.01; double min_cov_diag_ = 1e-9; double degeneracy_threshold_ratio_ = 1e-3; double degeneracy_cov_inflation_ = 1.02; double max_update_translation_step_ = 0.5; double max_update_rotation_step_deg_ = 5.0; double max_update_velocity_step_ = 2.0; }; /// 初始化 void Init(Options options) { lidar_obs_func_ = options.lidar_obs_func_; wheelspeed_obs_func_ = options.wheelspeed_obs_func_; acc_as_gravity_obs_func_ = options.acc_as_gravity_obs_func_; gps_obs_func_ = options.gps_obs_func_; bias_obs_func_ = options.bias_obs_func_; maximum_iter_ = options.max_iterations_; limit_ = options.epsi_; use_aa_ = options.use_aa_; options_ = options; } /// IMU预测 void Predict(const double& dt, const ProcessNoiseType& Q, const Vec3d& gyro, const Vec3d& acce); /// 自定义模型更新 void Update(ObsType obs, const double& R); // accessors const NavState& GetX() const { return x_; } const CovType& GetP() const { return P_; } const double& GetStamp() const { return stamp_; } void ChangeX(const NavState& state) { x_ = state; } void ChangeP(const CovType& P) { P_ = P; } void ChangeStamp(const double& stamp) { stamp_ = stamp; } void SetUseAA(bool use_aa) { use_aa_ = use_aa; } void SetTime(double timestamp) { x_.timestamp_ = timestamp; } /// 迭代次数 int GetIterations() const { return iterations_; } /// 最终平均观测误差 double GetFinalRes() const { return final_res_; } private: double stamp_ = 0.0; NavState x_; CovType P_ = CovType::Identity(); CovType F_x1_ = CovType::Identity(); CovType L_ = CovType ::Identity(); CustomObservationModel custom_obs_model_; CustomObsFunction lidar_obs_func_, wheelspeed_obs_func_, acc_as_gravity_obs_func_, gps_obs_func_, bias_obs_func_; int maximum_iter_ = 0; // 最大迭代次数 StateVecType limit_; int iterations_ = 0; double final_res_ = 0.0; /// anderson acceleration? bool use_aa_ = false; AndersonAcceleration aa_; Options options_; }; } // namespace lightning #endif // FUSION_ESKF_HPP ================================================ FILE: src/core/lio/imu_filter.h ================================================ // // Created by user on 2026/3/18. // #ifndef LIGHTNING_IMU_FILTER_H #define LIGHTNING_IMU_FILTER_H #include "common/eigen_types.h" #include "common/imu.h" #include namespace lightning { class IMUFilter { private: // 配置参数 struct Config { int median_window_size = 5; // 中值滤波窗口大小 int moving_avg_window = 3; // 移动平均窗口 double rate_limit = 3.0; // 角速度变化率限制 (rad/s²) double spike_threshold = 3.0; // 毛刺检测阈值 (标准差倍数) bool enable_adaptive = true; // 启用自适应滤波 } config_; // 数据缓冲区 std::deque buffer_; std::deque gyro_x_history_; std::deque gyro_y_history_; std::deque gyro_z_history_; // 上一帧滤波后的值 IMU prev_filtered_; // 统计信息 double gyro_mean_[3] = {0}; double gyro_std_[3] = {0}; int sample_count_ = 0; public: IMUFilter() { prev_filtered_.timestamp = -1; } // 设置滤波参数 void SetMedianWindowSize(int size) { if (size >= 3 && size % 2 == 1) { config_.median_window_size = size; } } void SetRateLimit(double limit) { config_.rate_limit = std::abs(limit); } void SetSpikeThreshold(double threshold) { config_.spike_threshold = threshold; } // 主滤波函数 IMU Filter(const IMU &raw_data) { IMU filtered = raw_data; // 1. 更新缓冲区 updateBuffer(raw_data); // 2. 多级滤波处理 filtered.angular_velocity.x() = processAxis(raw_data.angular_velocity.x(), gyro_x_history_, 0); filtered.angular_velocity.y() = processAxis(raw_data.angular_velocity.y(), gyro_y_history_, 1); filtered.angular_velocity.z() = processAxis(raw_data.angular_velocity.z(), gyro_z_history_, 2); // 3. 速率限制（防止突变） if (prev_filtered_.timestamp > 0) { double dt = raw_data.timestamp - prev_filtered_.timestamp; if (dt > 0 && dt < 0.1) { // 合理的dt范围 filtered.angular_velocity.x() = rateLimit(filtered.angular_velocity.x(), prev_filtered_.angular_velocity.x(), dt); filtered.angular_velocity.y() = rateLimit(filtered.angular_velocity.y(), prev_filtered_.angular_velocity.y(), dt); filtered.angular_velocity.z() = rateLimit(filtered.angular_velocity.z(), prev_filtered_.angular_velocity.z(), dt); } } // 4. 更新统计信息（用于自适应阈值） updateStatistics(filtered); prev_filtered_ = filtered; return filtered; } private: // 处理单个轴的数据 double processAxis(double raw_value, std::deque &history, int axis_idx) { double filtered = raw_value; // 1. 毛刺检测与替换 if (history.size() >= config_.median_window_size && sample_count_ > 100) { filtered = detectAndRemoveSpike(raw_value, history, axis_idx); } // 2. 中值滤波 filtered = medianFilter(filtered, history); // 3. 移动平均平滑 filtered = movingAverage(filtered, history); return filtered; } // 更新缓冲区 void updateBuffer(const IMU &data) { // 更新各轴历史数据 gyro_x_history_.push_back(data.angular_velocity.x()); gyro_y_history_.push_back(data.angular_velocity.y()); gyro_z_history_.push_back(data.angular_velocity.z()); // 限制历史数据长度 int max_history = std::max({config_.median_window_size, config_.moving_avg_window, 10}); while (gyro_x_history_.size() > max_history) { gyro_x_history_.pop_front(); gyro_y_history_.pop_front(); gyro_z_history_.pop_front(); } } // 毛刺检测与替换 double detectAndRemoveSpike(double value, std::deque &history, int axis_idx) { if (history.size() < config_.median_window_size) { return value; } // 计算当前窗口的中位数 std::vector window(history.end() - config_.median_window_size, history.end()); std::nth_element(window.begin(), window.begin() + window.size() / 2, window.end()); double median = window[window.size() / 2]; // 计算与中位数的偏差 double diff = std::abs(value - median); // 自适应阈值 double threshold = config_.spike_threshold * gyro_std_[axis_idx]; if (config_.enable_adaptive && gyro_std_[axis_idx] > 0) { threshold = std::max(threshold, config_.spike_threshold * 0.5); } // 如果是毛刺，用中位数替换 if (diff > threshold) { LOG(INFO) << "find imu spike: " << diff << ", " << threshold; return median; } return value; } // 中值滤波 double medianFilter(double value, std::deque &history) { if (history.size() < config_.median_window_size) { return value; } // 取最近N个值进行中值滤波 std::vector window(history.end() - config_.median_window_size, history.end()); std::nth_element(window.begin(), window.begin() + window.size() / 2, window.end()); return window[window.size() / 2]; } // 移动平均 double movingAverage(double value, std::deque &history) { if (history.size() < config_.moving_avg_window) { return value; } double sum = 0; auto it = history.end() - config_.moving_avg_window; for (; it != history.end(); ++it) { sum += *it; } return sum / config_.moving_avg_window; } // 速率限制 double rateLimit(double current, double previous, double dt) { double max_change = config_.rate_limit * dt; double diff = current - previous; if (std::abs(diff) > max_change) { return previous + (diff > 0 ? max_change : -max_change); } return current; } // 更新统计信息 void updateStatistics(const IMU &data) { const double alpha = 0.01; // 指数移动平均系数 if (sample_count_ == 0) { gyro_mean_[0] = data.angular_velocity[0]; gyro_mean_[1] = data.angular_velocity[1]; gyro_mean_[2] = data.angular_velocity[2]; gyro_std_[0] = 0.1; // 初始值 gyro_std_[1] = 0.1; gyro_std_[2] = 0.1; } else { // 更新均值 double prev_mean[3] = {gyro_mean_[0], gyro_mean_[1], gyro_mean_[2]}; gyro_mean_[0] = (1 - alpha) * gyro_mean_[0] + alpha * data.angular_velocity[0]; gyro_mean_[1] = (1 - alpha) * gyro_mean_[1] + alpha * data.angular_velocity[1]; gyro_mean_[2] = (1 - alpha) * gyro_mean_[2] + alpha * data.angular_velocity[2]; // 更新标准差 gyro_std_[0] = (1 - alpha) * gyro_std_[0] + alpha * std::abs(data.angular_velocity[0] - gyro_mean_[0]); gyro_std_[1] = (1 - alpha) * gyro_std_[1] + alpha * std::abs(data.angular_velocity[1] - gyro_mean_[1]); gyro_std_[2] = (1 - alpha) * gyro_std_[2] + alpha * std::abs(data.angular_velocity[2] - gyro_mean_[2]); } sample_count_++; } }; } // namespace lightning #endif // LIGHTNING_IMU_FILTER_H ================================================ FILE: src/core/lio/imu_processing.hpp ================================================ #pragma once #ifndef FASTER_LIO_IMU_PROCESSING_H #define FASTER_LIO_IMU_PROCESSING_H #include #include #include #include #include #include #include #include "common/eigen_types.h" #include "common/measure_group.h" #include "common/point_def.h" #include "core/lio/eskf.hpp" #include "core/lio/imu_filter.h" #include "core/lio/pose6d.h" #include "utils/timer.h" namespace lightning { /// IMU处理类 class ImuProcess { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuProcess(); ~ImuProcess(); void Reset(); void SetExtrinsic(const Vec3d &transl, const Mat3d &rot); void SetGyrCov(const Vec3d &scaler); void SetAccCov(const Vec3d &scaler); void SetGyrBiasCov(const Vec3d &b_g); void SetAccBiasCov(const Vec3d &b_a); void Process(const MeasureGroup &meas, ESKF &kf_state, CloudPtr &scan); bool IsIMUInited() const { return imu_need_init_ == false; } void SetUseIMUFilter(bool b) { use_imu_filter_ = b; } double GetMeanAccNorm() const { return mean_acc_.norm(); } Eigen::Matrix Q_; Vec3d cov_acc_; Vec3d cov_gyr_; Vec3d cov_acc_scale_; Vec3d cov_gyr_scale_; Vec3d cov_bias_gyr_; Vec3d cov_bias_acc_; private: void IMUInit(const MeasureGroup &meas, ESKF &kf_state, int &N); void UndistortPcl(const MeasureGroup &meas, ESKF &kf_state, CloudPtr &pcl_out); static inline constexpr int max_init_count_ = 20; PointCloudType::Ptr cur_pcl_un_ = nullptr; lightning::IMUPtr last_imu_ = nullptr; std::deque imu_queue_; std::vector imu_pose_; Mat3d R_lidar_imu_ = Mat3d ::Identity(); Vec3d t_lidar_mu_ = Vec3d ::Zero(); Vec3d mean_acc_ = Vec3d::Zero(); Vec3d mean_gyr_ = Vec3d::Zero(); Vec3d angvel_last_ = Vec3d ::Zero(); Vec3d acc_s_last_ = Vec3d ::Zero(); double acc_scale_factor_ = 1.0; double last_lidar_end_time_ = 0; int init_iter_num_ = 1; bool b_first_frame_ = true; bool imu_need_init_ = true; bool use_imu_filter_ = true; IMUFilter filter_; }; inline ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true) { init_iter_num_ = 1; Q_.setZero(); Q_.diagonal() << 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-5, 1e-5, 1e-5, 0.0, 0.0, 0.0; cov_acc_ = Vec3d(0.1, 0.1, 0.1); cov_gyr_ = Vec3d(0.1, 0.1, 0.1); cov_bias_gyr_ = Vec3d(0.0001, 0.0001, 0.0001); cov_bias_acc_ = Vec3d(0.0001, 0.0001, 0.0001); mean_acc_ = Vec3d(0, 0, -1.0); mean_gyr_ = Vec3d(0, 0, 0); last_imu_.reset(new lightning::IMU()); } inline ImuProcess::~ImuProcess() {} inline void ImuProcess::Reset() { mean_acc_ = Vec3d(0, 0, -1.0); mean_gyr_ = Vec3d(0, 0, 0); angvel_last_.setZero(); acc_scale_factor_ = 1.0; imu_need_init_ = true; init_iter_num_ = 1; imu_queue_.clear(); imu_pose_.clear(); last_imu_.reset(new lightning::IMU()); cur_pcl_un_.reset(new PointCloudType()); } inline void ImuProcess::SetExtrinsic(const Vec3d &transl, const Mat3d &rot) { t_lidar_mu_ = transl; R_lidar_imu_ = rot; } inline void ImuProcess::SetGyrCov(const Vec3d &scaler) { cov_gyr_scale_ = scaler; } inline void ImuProcess::SetAccCov(const Vec3d &scaler) { cov_acc_scale_ = scaler; } inline void ImuProcess::SetGyrBiasCov(const Vec3d &b_g) { cov_bias_gyr_ = b_g; } inline void ImuProcess::SetAccBiasCov(const Vec3d &b_a) { cov_bias_acc_ = b_a; } inline void ImuProcess::IMUInit(const MeasureGroup &meas, ESKF &kf_state, int &N) { /** 1. initializing the gravity_, gyro bias, acc and gyro covariance ** 2. normalize the acceleration measurenments to unit gravity_ **/ Vec3d cur_acc, cur_gyr; if (b_first_frame_) { Reset(); N = 1; b_first_frame_ = false; const auto &imu_acc = meas.imu_.front()->linear_acceleration; const auto &gyr_acc = meas.imu_.front()->angular_velocity; mean_acc_ = imu_acc; mean_gyr_ = gyr_acc; } for (const auto &imu : meas.imu_) { const auto &imu_acc = imu->linear_acceleration; const auto &gyr_acc = imu->angular_velocity; cur_acc = imu_acc; cur_gyr = gyr_acc; mean_acc_ += (cur_acc - mean_acc_) / N; mean_gyr_ += (cur_gyr - mean_gyr_) / N; cov_acc_ = cov_acc_ * (N - 1.0) / N + (cur_acc - mean_acc_).cwiseProduct(cur_acc - mean_acc_) * (N - 1.0) / (N * N); cov_gyr_ = cov_gyr_ * (N - 1.0) / N + (cur_gyr - mean_gyr_).cwiseProduct(cur_gyr - mean_gyr_) * (N - 1.0) / (N * N); N++; } auto init_state = kf_state.GetX(); init_state.timestamp_ = meas.imu_.back()->timestamp; init_state.grav_ = -mean_acc_ / mean_acc_.norm() * G_m_s2; init_state.bg_ = mean_gyr_; kf_state.ChangeX(init_state); auto init_P = kf_state.GetP(); init_P.setIdentity(); init_P.block(NavState::kBgIdx, NavState::kBgIdx) = 0.0001 * Mat3d::Identity(); kf_state.ChangeP(init_P); // LOG(INFO) << "P diag: " << init_P.diagonal().transpose(); last_imu_ = meas.imu_.back(); } inline void ImuProcess::UndistortPcl(const MeasureGroup &meas, ESKF &kf_state, CloudPtr &pcl_out) { /*** add the imu_ of the last frame-tail to the of current frame-head ***/ auto v_imu = meas.imu_; v_imu.push_front(last_imu_); const double &imu_end_time = v_imu.back()->timestamp; const double &pcl_beg_time = meas.lidar_begin_time_; const double &pcl_end_time = meas.lidar_end_time_; /*** Initialize IMU pose ***/ auto imu_state = kf_state.GetX(); imu_pose_.clear(); imu_pose_.emplace_back(0.0, acc_s_last_, angvel_last_, imu_state.vel_, imu_state.pos_, imu_state.rot_.matrix()); /*** forward propagation at each imu_ point ***/ Vec3d angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu; Mat3d R_imu; double dt = 0; Vec3d acc = Vec3d::Zero(); Vec3d gyro = Vec3d::Zero(); if (use_imu_filter_) { for (auto &imu : v_imu) { auto imu_f = filter_.Filter(*imu); *imu = imu_f; } } for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++) { auto &&head = *(it_imu); auto &&tail = *(it_imu + 1); if (tail->timestamp < last_lidar_end_time_) { continue; } angvel_avr = .5 * (head->angular_velocity + tail->angular_velocity); acc_avr = .5 * (head->linear_acceleration + tail->linear_acceleration); acc_avr = acc_avr * acc_scale_factor_; if (head->timestamp < last_lidar_end_time_) { dt = tail->timestamp - last_lidar_end_time_; } else { dt = tail->timestamp - head->timestamp; } acc = acc_avr; gyro = angvel_avr; if (dt > 0.1) { LOG(ERROR) << "get abnormal dt: " << dt; kf_state.SetTime((*it_imu)->timestamp); break; } Q_.block<3, 3>(0, 0).diagonal() = cov_gyr_; Q_.block<3, 3>(3, 3).diagonal() = cov_acc_; Q_.block<3, 3>(6, 6).diagonal() = cov_bias_gyr_; kf_state.Predict(dt, Q_, gyro, acc); // LOG(INFO) << "gyro: " << gyro.transpose() << ", dt: " << dt; // LOG(INFO) << "acc: " << acc.transpose() << " grav: " << kf_state.GetX().grav_.norm() // << ", vel: " << kf_state.GetX().vel_.transpose() << ", dt: " << dt; /* save the poses at each IMU measurements */ imu_state = kf_state.GetX(); angvel_last_ = angvel_avr - imu_state.bg_; acc_s_last_ = imu_state.rot_ * acc_avr; for (int i = 0; i < 3; i++) { acc_s_last_[i] += imu_state.grav_[i]; } double &&offs_t = tail->timestamp - pcl_beg_time; imu_pose_.emplace_back( Pose6D(offs_t, acc_s_last_, angvel_last_, imu_state.vel_, imu_state.pos_, imu_state.rot_.matrix())); } /*** calculated the pos and attitude prediction at the frame-end ***/ double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; dt = note * (pcl_end_time - imu_end_time); kf_state.Predict(dt, Q_, gyro, acc); imu_state = kf_state.GetX(); last_imu_ = meas.imu_.back(); last_lidar_end_time_ = pcl_end_time; /*** sort point clouds by offset time ***/ pcl_out = meas.scan_; std::sort(pcl_out->points.begin(), pcl_out->points.end(), [](const PointType &p1, const PointType &p2) { return p1.time < p2.time; }); /*** undistort each lidar point (backward propagation) ***/ if (pcl_out->empty()) { return; } auto it_pcl = pcl_out->points.end() - 1; for (auto it_kp = imu_pose_.end() - 1; it_kp != imu_pose_.begin(); it_kp--) { auto head = it_kp - 1; auto tail = it_kp; R_imu = (head->rot); vel_imu = (head->vel); pos_imu = (head->pos); acc_imu = (tail->acc); angvel_avr = (tail->gyr); for (; it_pcl->time / double(1000) > head->offset_time && it_pcl != pcl_out->points.begin(); it_pcl--) { dt = it_pcl->time / double(1000) - head->offset_time; /// dt 有时候存在非法数据 if (dt < 0 || dt > lo::lidar_time_interval) { // LOG(WARNING) << "find abnormal dt in cloud: " << dt; continue; } /* Transform to the 'end' frame, using only the rotation * Note: Compensation direction is INVERSE of Frame's moving direction * So if we want to compensate a point at timestamp-i to the frame-e * p_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ Mat3d R_i(R_imu * math::exp(angvel_avr, dt).matrix()); Vec3d P_i(it_pcl->x, it_pcl->y, it_pcl->z); Vec3d T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos_); Vec3d p_compensate = R_lidar_imu_.transpose() * (imu_state.rot_.inverse() * (R_i * (R_lidar_imu_ * P_i + t_lidar_mu_) + T_ei) - t_lidar_mu_); // not accurate! // save Undistorted points and their rotation it_pcl->x = p_compensate(0); it_pcl->y = p_compensate(1); it_pcl->z = p_compensate(2); // if (it_pcl == pcl_out->points.begin()) { // break; // } } } } inline void ImuProcess::Process(const MeasureGroup &meas, ESKF &kf_state, CloudPtr &scan) { if (meas.imu_.empty()) { return; } if (imu_need_init_) { /// The very first lidar frame IMUInit(meas, kf_state, init_iter_num_); imu_need_init_ = true; last_imu_ = meas.imu_.back(); auto imu_state = kf_state.GetX(); if (init_iter_num_ > max_init_count_) { imu_need_init_ = false; cov_acc_ = cov_acc_scale_; cov_gyr_ = cov_gyr_scale_; const double mean_acc_norm = mean_acc_.norm(); if (mean_acc_norm > 0.5 && mean_acc_norm < 1.5) { acc_scale_factor_ = G_m_s2; } else if (mean_acc_norm > 7.0 && mean_acc_norm < 12.0) { acc_scale_factor_ = 1.0; } else { acc_scale_factor_ = 1.0; LOG(WARNING) << "imu init mean acc norm is abnormal for unit inference: " << mean_acc_norm << ", keep accelerometer scale unchanged"; } LOG(INFO) << "imu init done, bg: " << imu_state.bg_.transpose() << ", grav: " << imu_state.grav_.transpose() << ", acc scale: " << acc_scale_factor_ << ", mean: " << mean_acc_.transpose() << ", " << mean_gyr_.transpose(); } else { LOG(INFO) << "waiting for imu init ... " << init_iter_num_; } return; } Timer::Evaluate([&, this]() { UndistortPcl(meas, kf_state, scan); }, "Undistort Pcl"); } } // namespace lightning #endif ================================================ FILE: src/core/lio/laser_mapping.cc ================================================ #include #include #include #include "common/options.h" #include "core/lightning_math.hpp" #include "laser_mapping.h" #include #include #include #include "ui/pangolin_window.h" #include "wrapper/ros_utils.h" namespace lightning { bool LaserMapping::Init(const std::string &config_yaml) { LOG(INFO) << "init laser mapping from " << config_yaml; if (!LoadParamsFromYAML(config_yaml)) { return false; } // localmap init (after LoadParams) ivox_ = std::make_shared(ivox_options_); // esekf init ESKF::Options eskf_options; eskf_options.max_iterations_ = fasterlio::NUM_MAX_ITERATIONS; eskf_options.epsi_ = 1e-3 * Eigen::Matrix::Ones(); eskf_options.lidar_obs_func_ = [this](NavState &s, ESKF::CustomObservationModel &obs) { ObsModel(s, obs); }; eskf_options.use_aa_ = use_aa_; kf_.Init(eskf_options); return true; } bool LaserMapping::LoadParamsFromYAML(const std::string &yaml_file) { // get params from yaml int lidar_type, ivox_nearby_type; double gyr_cov, acc_cov, b_gyr_cov, b_acc_cov; double filter_size_scan; auto yaml = YAML::LoadFile(yaml_file); try { fasterlio::NUM_MAX_ITERATIONS = yaml["fasterlio"]["max_iteration"].as(); fasterlio::ESTI_PLANE_THRESHOLD = yaml["fasterlio"]["esti_plane_threshold"].as(); filter_size_scan = yaml["fasterlio"]["filter_size_scan"].as(); filter_size_map_min_ = yaml["fasterlio"]["filter_size_map"].as(); keep_first_imu_estimation_ = yaml["fasterlio"]["keep_first_imu_estimation"].as(); gyr_cov = yaml["fasterlio"]["gyr_cov"].as(); acc_cov = yaml["fasterlio"]["acc_cov"].as(); b_gyr_cov = yaml["fasterlio"]["b_gyr_cov"].as(); b_acc_cov = yaml["fasterlio"]["b_acc_cov"].as(); preprocess_->Blind() = yaml["fasterlio"]["blind"].as(); preprocess_->TimeScale() = yaml["fasterlio"]["time_scale"].as(); lidar_type = yaml["fasterlio"]["lidar_type"].as(); preprocess_->NumScans() = yaml["fasterlio"]["scan_line"].as(); preprocess_->PointFilterNum() = yaml["fasterlio"]["point_filter_num"].as(); extrinT_ = yaml["fasterlio"]["extrinsic_T"].as>(); extrinR_ = yaml["fasterlio"]["extrinsic_R"].as>(); ivox_options_.resolution_ = yaml["fasterlio"]["ivox_grid_resolution"].as(); ivox_nearby_type = yaml["fasterlio"]["ivox_nearby_type"].as(); use_aa_ = yaml["fasterlio"]["use_aa"].as(); skip_lidar_num_ = yaml["fasterlio"]["skip_lidar_num"].as(); enable_skip_lidar_ = skip_lidar_num_ > 0; float height_max = yaml["roi"]["height_max"].as(); float height_min = yaml["roi"]["height_min"].as(); preprocess_->SetHeightROI(height_max, height_min); options_.kf_dis_th_ = yaml["fasterlio"]["kf_dis_th"].as(); options_.kf_angle_th_ = yaml["fasterlio"]["kf_angle_th"].as() * M_PI / 180.0; options_.enable_icp_part_ = yaml["fasterlio"]["enable_icp_part"].as(); options_.min_pts = yaml["fasterlio"]["min_pts"].as(); options_.plane_icp_weight_ = yaml["fasterlio"]["plane_icp_weight"].as(); bool use_imu_filter = yaml["fasterlio"]["imu_filter"].as(); p_imu_->SetUseIMUFilter(use_imu_filter); options_.proj_kfs_ = yaml["fasterlio"]["proj_kfs"].as(); } catch (...) { LOG(ERROR) << "bad conversion"; return false; } LOG(INFO) << "lidar_type " << lidar_type; if (lidar_type == 1) { preprocess_->SetLidarType(LidarType::AVIA); LOG(INFO) << "Using AVIA Lidar"; } else if (lidar_type == 2) { preprocess_->SetLidarType(LidarType::VELO32); LOG(INFO) << "Using Velodyne 32 Lidar"; } else if (lidar_type == 3) { preprocess_->SetLidarType(LidarType::OUST64); LOG(INFO) << "Using OUST 64 Lidar"; } else if (lidar_type == 4) { preprocess_->SetLidarType(LidarType::ROBOSENSE); LOG(INFO) << "Using RoboSense Lidar"; } else { LOG(WARNING) << "unknown lidar_type"; return false; } if (ivox_nearby_type == 0) { ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER; } else if (ivox_nearby_type == 6) { ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY6; } else if (ivox_nearby_type == 18) { ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18; } else if (ivox_nearby_type == 26) { ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY26; } else { LOG(WARNING) << "unknown ivox_nearby_type, use NEARBY18"; ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18; } voxel_scan_.setLeafSize(filter_size_scan, filter_size_scan, filter_size_scan); offset_t_lidar_fixed_ = math::VecFromArray(extrinT_); offset_R_lidar_fixed_ = math::MatFromArray(extrinR_); p_imu_->SetExtrinsic(offset_t_lidar_fixed_, offset_R_lidar_fixed_); p_imu_->SetGyrCov(Vec3d(gyr_cov, gyr_cov, gyr_cov)); p_imu_->SetAccCov(Vec3d(acc_cov, acc_cov, acc_cov)); p_imu_->SetGyrBiasCov(Vec3d(b_gyr_cov, b_gyr_cov, b_gyr_cov)); p_imu_->SetAccBiasCov(Vec3d(b_acc_cov, b_acc_cov, b_acc_cov)); return true; } LaserMapping::LaserMapping(Options options) : options_(options) { preprocess_.reset(new PointCloudPreprocess()); p_imu_.reset(new ImuProcess()); } void LaserMapping::ProcessIMU(const lightning::IMUPtr &imu) { publish_count_++; double timestamp = imu->timestamp; UL lock(mtx_buffer_); if (timestamp < last_timestamp_imu_) { LOG(WARNING) << "imu loop back, clear buffer"; imu_buffer_.clear(); } if (p_imu_->IsIMUInited()) { /// 更新最新imu状态 kf_imu_.Predict(timestamp - last_timestamp_imu_, p_imu_->Q_, imu->angular_velocity, imu->linear_acceleration); // LOG(INFO) << "newest wrt lidar: " << timestamp - kf_.GetX().timestamp_; /// 更新ui if (ui_) { ui_->UpdateNavState(kf_imu_.GetX()); } } last_timestamp_imu_ = timestamp; imu_buffer_.emplace_back(imu); } bool LaserMapping::Run() { if (!SyncPackages()) { LOG(WARNING) << "sync package failed"; return false; } /// IMU process, kf prediction, undistortion p_imu_->Process(measures_, kf_, scan_undistort_); if (scan_undistort_->empty() || (scan_undistort_ == nullptr)) { LOG(WARNING) << "No point, skip this scan!"; return false; } /// the first scan if (flg_first_scan_) { LOG(INFO) << "first scan pts: " << scan_undistort_->size(); state_point_ = kf_.GetX(); scan_down_world_->resize(scan_undistort_->size()); for (int i = 0; i < scan_undistort_->size(); i++) { PointBodyToWorld(scan_undistort_->points[i], scan_down_world_->points[i]); } ivox_->AddPoints(scan_down_world_->points); first_lidar_time_ = measures_.lidar_end_time_; state_point_.timestamp_ = lidar_end_time_; flg_first_scan_ = false; return true; } if (enable_skip_lidar_) { skip_lidar_cnt_++; skip_lidar_cnt_ = skip_lidar_cnt_ % skip_lidar_num_; if (skip_lidar_cnt_ != 0) { /// 更新UI中的内容 if (ui_) { ui_->UpdateNavState(kf_.GetX()); ui_->UpdateScan(scan_undistort_, kf_.GetX().GetPose()); } return false; } } LOG(INFO) << "============================="; LOG(INFO) << "LIO get cloud at beg: " << std::setprecision(14) << measures_.lidar_begin_time_ << ", end: " << measures_.lidar_end_time_; if (last_lidar_time_ > 0 && (measures_.lidar_begin_time_ - last_lidar_time_) > 0.5) { LOG(ERROR) << "检测到雷达断流，时长：" << (measures_.lidar_begin_time_ - last_lidar_time_); } last_lidar_time_ = measures_.lidar_begin_time_; flg_EKF_inited_ = (measures_.lidar_begin_time_ - first_lidar_time_) >= fasterlio::INIT_TIME; /// downsample voxel_scan_.setInputCloud(scan_undistort_); voxel_scan_.filter(*scan_down_body_); // if (options_.proj_kfs_) { // ProjectKFs(); // } int cur_pts = scan_down_body_->size(); if (cur_pts < (scan_undistort_->size() * 0.1) || cur_pts < options_.min_pts) { /// 降采样太狠了,有效点数不够，用0.1分辨率代替 // LOG(INFO) << "too few points, using 0.1 resol"; auto v = voxel_scan_; v.setLeafSize(0.1, 0.1, 0.1); v.setInputCloud(scan_undistort_); v.filter(*scan_down_body_); // LOG(INFO) << "Now pts: " << scan_down_body_->size() << ", before: " << cur_pts; cur_pts = scan_down_body_->size(); } if (cur_pts < 5) { LOG(WARNING) << "Too few points, skip this scan!" << scan_undistort_->size() << ", " << scan_down_body_->size(); return false; } scan_down_world_->resize(cur_pts); nearest_points_.resize(cur_pts); // 成员变量预分配 residuals_.resize(cur_pts, 0); point_selected_surf_.resize(cur_pts, 1); point_selected_icp_.resize(cur_pts, 1); plane_coef_.resize(cur_pts, Vec4f::Zero()); auto pred_state = kf_.GetX(); // pred_state.pos_ = state_point_.pos_; // 假定位置不动行不行,防止速度漂移 // kf_.ChangeX(pred_state); kf_.Update(ESKF::ObsType::LIDAR, 1.0); state_point_ = kf_.GetX(); state_point_.timestamp_ = measures_.lidar_end_time_; const double delta_translation = (pred_state.pos_ - state_point_.pos_).norm(); const double delta_rotation_deg = (pred_state.rot_.inverse() * state_point_.rot_).log().norm() * 180.0 / M_PI; const double delta_velocity = (pred_state.vel_ - state_point_.vel_).norm(); const double current_speed = state_point_.vel_.norm(); LOG(INFO) << "[ mapping ]: In num: " << scan_undistort_->points.size() << " down " << cur_pts << " Map grid num: " << ivox_->NumValidGrids() << " effect num : " << effect_feat_surf_ << ", " << effect_feat_icp_; LOG(INFO) << "delta trans: " << (pred_state.pos_ - state_point_.pos_).transpose() << ", ang: " << delta_rotation_deg; // LOG(INFO) << "P diag: " << kf_.GetP().diagonal().transpose(); // Vec3d v_from_last = (state_point_.pos_ - last_state.pos_) / (state_point_.timestamp_ - last_state.timestamp_); // LOG(INFO) << "v from last: " << v_from_last.transpose(); // if (delta_velocity > 1.0 || current_speed > 4.0) { // LOG(ERROR) << "detected very large vel change, last: " << last_state.vel_.transpose() // << ", pred: " << pred_state.vel_.transpose() << ", cur:" << state_point_.vel_.transpose(); // LOG(ERROR) << "please check"; // } /// keyframes if (last_kf_ == nullptr) { MakeKF(); } else { SE3 last_pose = last_kf_->GetLIOPose(); SE3 cur_pose = state_point_.GetPose(); if ((last_pose.translation() - cur_pose.translation()).norm() > options_.kf_dis_th_ || (last_pose.so3().inverse() * cur_pose.so3()).log().norm() > options_.kf_angle_th_) { MakeKF(); } else if (!options_.is_in_slam_mode_ && (state_point_.timestamp_ - last_kf_->GetState().timestamp_) > 2.0) { MakeKF(); } else if ((last_pose.so3().inverse() * cur_pose.so3()).log().norm() > 1.0 * M_PI / 180.0) { // MapIncremental(); } } /// 更新kf_for_imu kf_imu_ = kf_; if (!measures_.imu_.empty()) { double t = measures_.imu_.back()->timestamp; for (auto &imu : imu_buffer_) { double dt = imu->timestamp - t; kf_imu_.Predict(dt, p_imu_->Q_, imu->angular_velocity, imu->linear_acceleration); t = imu->timestamp; } } if (ui_) { ui_->UpdateScan(scan_down_body_, state_point_.GetPose()); } LOG(INFO) << "LIO state: " << state_point_.pos_.transpose() << ", yaw " << state_point_.rot_.angleZ() * 180 / M_PI << ", vel: " << state_point_.vel_.transpose() << ", grav: " << state_point_.grav_.transpose() << ", grav norm: " << state_point_.grav_.norm(); return true; } void LaserMapping::ProjectKFs(CloudPtr cloud, int size_limit) { auto state = kf_.GetX(); SE3 pose_cur(state.rot_, state.pos_); pose_cur = pose_cur.inverse(); for (auto kf : proj_kfs_) { // LOG(INFO) << "projecting kf: " << kf->GetID(); // if (last_kf_) { // auto kf = last_kf_; SE3 pose = pose_cur * kf->GetLIOPose(); int cnt = 0; for (auto &pt : kf->GetCloud()->points) { Vec3d p = pose * ToVec3d(pt); PointType pcl_pt; pcl_pt.x = p.x(); pcl_pt.y = p.y(); pcl_pt.z = p.z(); pcl_pt.intensity = pt.intensity; cloud->push_back(pcl_pt); cnt++; if (cnt > size_limit) { break; } } // } } } void LaserMapping::MakeKF() { Keyframe::Ptr kf = std::make_shared(kf_id_++, scan_undistort_, state_point_); if (last_kf_) { /// opt pose 用之前的递推 SE3 delta = last_kf_->GetLIOPose().inverse() * kf->GetLIOPose(); kf->SetOptPose(last_kf_->GetOptPose() * delta); } else { kf->SetOptPose(kf->GetLIOPose()); } kf->SetState(state_point_); LOG(INFO) << "LIO: create kf " << kf->GetID() << ", state: " << state_point_.pos_.transpose() << ", kf opt pose: " << kf->GetOptPose().translation().transpose() << ", lio pose: " << kf->GetLIOPose().translation().transpose() << ", time: " << std::setprecision(14) << state_point_.timestamp_; if (options_.is_in_slam_mode_) { all_keyframes_.emplace_back(kf); } last_kf_ = kf; // 有keyframes时更新local map Timer::Evaluate([&, this]() { MapIncremental(); }, " Incremental Mapping"); /// 更新project kfs if (proj_kfs_.size() >= options_.max_proj_kfs_) { auto last = proj_kfs_.back(); SE3 delta = last->GetLIOPose().inverse() * kf->GetLIOPose(); if (delta.translation().norm() < 3 || delta.so3().log().norm() < 20 / 180 * M_PI) { // proj_kfs_.pop_back(); } else { proj_kfs_.pop_front(); proj_kfs_.emplace_back(kf); } } else { proj_kfs_.emplace_back(kf); } // for (auto &kf : proj_kfs_) { // LOG(INFO) << "proj kf: " << kf->GetID(); // } } void LaserMapping::ProcessPointCloud2(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { UL lock(mtx_buffer_); Timer::Evaluate( [&, this]() { scan_count_++; double timestamp = ToSec(msg->header.stamp); if (timestamp < last_timestamp_lidar_) { LOG(ERROR) << "lidar loop back, dt: " << timestamp - last_timestamp_lidar_; return; } LOG(INFO) << "get cloud at " << std::setprecision(14) << timestamp << ", latest imu: " << last_timestamp_imu_; CloudPtr cloud(new PointCloudType()); preprocess_->Process(msg, cloud); lidar_buffer_.push_back(cloud); time_buffer_.push_back(timestamp); last_timestamp_lidar_ = timestamp; }, "Preprocess (Standard)"); } void LaserMapping::ProcessPointCloud2(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg) { UL lock(mtx_buffer_); Timer::Evaluate( [&, this]() { scan_count_++; double timestamp = ToSec(msg->header.stamp); if (timestamp < last_timestamp_lidar_) { LOG(ERROR) << "lidar loop back, clear buffer"; lidar_buffer_.clear(); } // LOG(INFO) << "get cloud at " << std::setprecision(14) << timestamp // << ", latest imu: " << last_timestamp_imu_; CloudPtr cloud(new PointCloudType()); preprocess_->Process(msg, cloud); lidar_buffer_.push_back(cloud); time_buffer_.push_back(timestamp); last_timestamp_lidar_ = timestamp; }, "Preprocess (Standard)"); } void LaserMapping::ProcessPointCloud2(CloudPtr cloud) { UL lock(mtx_buffer_); Timer::Evaluate( [&, this]() { scan_count_++; double timestamp = math::ToSec(cloud->header.stamp); if (timestamp < last_timestamp_lidar_) { LOG(ERROR) << "lidar loop back, clear buffer"; lidar_buffer_.clear(); } lidar_buffer_.push_back(cloud); time_buffer_.push_back(timestamp); last_timestamp_lidar_ = timestamp; }, "Preprocess (Standard)"); } bool LaserMapping::SyncPackages() { if (lidar_buffer_.empty() || imu_buffer_.empty()) { LOG(INFO) << "lidar or imu is empty"; return false; } /*** push a lidar scan ***/ if (!lidar_pushed_) { measures_.scan_ = lidar_buffer_.front(); measures_.lidar_begin_time_ = time_buffer_.front(); if (measures_.scan_->points.size() <= 1) { LOG(WARNING) << "Too few input point cloud!"; lidar_end_time_ = measures_.lidar_begin_time_ + lidar_mean_scantime_; } else if (measures_.scan_->points.back().time / double(1000) < 0.5 * lidar_mean_scantime_) { lidar_end_time_ = measures_.lidar_begin_time_ + lidar_mean_scantime_; } else { scan_num_++; lidar_end_time_ = measures_.lidar_begin_time_ + measures_.scan_->points.back().time / double(1000); lidar_mean_scantime_ += (measures_.scan_->points.back().time / double(1000) - lidar_mean_scantime_) / scan_num_; if ((lidar_end_time_ - measures_.lidar_begin_time_) > 5 * lo::lidar_time_interval) { /// timestamp 有异常 lidar_end_time_ = measures_.lidar_begin_time_ + lo::lidar_time_interval; lidar_mean_scantime_ = lo::lidar_time_interval; } } lo::lidar_time_interval = lidar_mean_scantime_; // LOG(INFO) << "recompute lidar end time: " << std::setprecision(14) << lidar_end_time_; measures_.lidar_end_time_ = lidar_end_time_; lidar_pushed_ = true; } if (last_timestamp_imu_ < lidar_end_time_) { LOG(INFO) << "sync failed: " << std::setprecision(14) << last_timestamp_imu_ << ", " << lidar_end_time_; return false; } /*** push imu_ data, and pop from imu_ buffer ***/ double imu_time = imu_buffer_.front()->timestamp; measures_.imu_.clear(); while ((!imu_buffer_.empty()) && (imu_time < lidar_end_time_)) { imu_time = imu_buffer_.front()->timestamp; if (imu_time > lidar_end_time_) { break; } measures_.imu_.push_back(imu_buffer_.front()); imu_buffer_.pop_front(); } lidar_buffer_.pop_front(); time_buffer_.pop_front(); lidar_pushed_ = false; // LOG(INFO) << "sync: " << std::setprecision(14) << measures_.lidar_begin_time_ << ", " << // measures_.lidar_end_time_; return true; } void LaserMapping::MapIncremental() { PointVector points_to_add; PointVector point_no_need_downsample; size_t cur_pts = scan_down_body_->size(); points_to_add.reserve(cur_pts); point_no_need_downsample.reserve(cur_pts); std::vector index(cur_pts); for (size_t i = 0; i < cur_pts; ++i) { index[i] = i; } std::for_each(index.begin(), index.end(), [&](const size_t &i) { /* transform to world frame */ PointBodyToWorld(scan_down_body_->points[i], scan_down_world_->points[i]); /* decide if need add to map */ PointType &point_world = scan_down_world_->points[i]; if (!nearest_points_[i].empty() && flg_EKF_inited_) { const PointVector &points_near = nearest_points_[i]; Eigen::Vector3f center = ((point_world.getVector3fMap() / filter_size_map_min_).array().floor() + 0.5) * filter_size_map_min_; Eigen::Vector3f dis_2_center = points_near[0].getVector3fMap() - center; if (fabs(dis_2_center.x()) > 0.5 * filter_size_map_min_ && fabs(dis_2_center.y()) > 0.5 * filter_size_map_min_ && fabs(dis_2_center.z()) > 0.5 * filter_size_map_min_) { point_no_need_downsample.emplace_back(point_world); return; } bool need_add = true; float dist = math::calc_dist(point_world.getVector3fMap(), center); if (points_near.size() >= fasterlio::NUM_MATCH_POINTS) { for (int readd_i = 0; readd_i < fasterlio::NUM_MATCH_POINTS; readd_i++) { if (math::calc_dist(points_near[readd_i].getVector3fMap(), center) < dist + 1e-6) { need_add = false; break; } } } if (need_add) { points_to_add.emplace_back(point_world); // FIXME 这并发可能有点问题 } } else { points_to_add.emplace_back(point_world); } }); Timer::Evaluate( [&, this]() { ivox_->AddPoints(points_to_add); ivox_->AddPoints(point_no_need_downsample); }, " IVox Add Points"); } /** * Lidar point cloud registration * will be called by the eskf custom observation model * compute point-to-plane residual here * @param s kf state * @param ekfom_data H matrix */ void LaserMapping::ObsModel(NavState &s, ESKF::CustomObservationModel &obs) { int cnt_pts = scan_down_body_->size(); std::vector index(cnt_pts); for (size_t i = 0; i < index.size(); ++i) { index[i] = i; } // LOG(INFO) << "obs from state: " << s.pos_.transpose() << ", " << s.rot_.unit_quaternion().coeffs().transpose(); Timer::Evaluate( [&, this]() { Mat3f R_wl = (s.rot_.matrix() * offset_R_lidar_fixed_).cast(); Vec3f t_wl = (s.rot_ * offset_t_lidar_fixed_ + s.pos_).cast(); std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const size_t &i) { PointType &point_body = scan_down_body_->points[i]; PointType &point_world = scan_down_world_->points[i]; /* transform to world frame */ Vec3f p_body = point_body.getVector3fMap(); point_world.getVector3fMap() = R_wl * p_body + t_wl; point_world.intensity = point_body.intensity; auto &points_near = nearest_points_[i]; points_near.clear(); /** Find the closest surfaces in the map **/ ivox_->GetClosestPoint(point_world, points_near, fasterlio::NUM_MATCH_POINTS); point_selected_surf_[i] = points_near.size() >= fasterlio::MIN_NUM_MATCH_POINTS; point_selected_icp_[i] = point_selected_surf_[i]; /// 能找到3个点以上，则估计平面 if (point_selected_surf_[i]) { point_selected_surf_[i] = math::esti_plane(plane_coef_[i], points_near, fasterlio::ESTI_PLANE_THRESHOLD); } /// 计算平面阈值 if (point_selected_surf_[i]) { auto temp = point_world.getVector4fMap(); temp[3] = 1.0; float pd2 = plane_coef_[i].dot(temp); if (p_body.norm() > 81 * pd2 * pd2) { point_selected_surf_[i] = true; residuals_[i] = pd2; } else { point_selected_surf_[i] = false; } } }); }, " ObsModel (Lidar Match)"); effect_feat_surf_ = 0; effect_feat_icp_ = 0; corr_pts_.resize(cnt_pts); corr_norm_.resize(cnt_pts); for (int i = 0; i < cnt_pts; i++) { if (point_selected_surf_[i]) { corr_norm_[effect_feat_surf_] = plane_coef_[i]; corr_pts_[effect_feat_surf_] = scan_down_body_->points[i].getVector4fMap(); corr_pts_[effect_feat_surf_][3] = residuals_[i]; effect_feat_surf_++; } if (point_selected_icp_[i]) { effect_feat_icp_++; } } corr_pts_.resize(effect_feat_surf_); corr_norm_.resize(effect_feat_surf_); if (effect_feat_surf_ < 20) { obs.valid_ = false; LOG(WARNING) << "No enough effective surface points: " << effect_feat_surf_ << ", icp: " << effect_feat_icp_ << ", required: " << 20; return; } index.resize(effect_feat_surf_); const Mat3f off_R = offset_R_lidar_fixed_.cast(); const Vec3f off_t = offset_t_lidar_fixed_.cast(); const Mat3f Rt = s.rot_.matrix().transpose().cast(); /// 点面ICP部分 obs.HTH_.setZero(); obs.HTr_.setZero(); std::vector JTJ(effect_feat_surf_); std::vector JTr(effect_feat_surf_); std::vector res_sq(index.size()); std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const size_t &i) { Vec3f point_this_be = corr_pts_[i].head<3>(); Vec3f point_this = off_R * point_this_be + off_t; Mat3f point_crossmat = math::SKEW_SYM_MATRIX(point_this); /*** get the normal vector of closest surface/corner ***/ Vec3f norm_vec = corr_norm_[i].head<3>(); /*** calculate the Measurement Jacobian matrix H ***/ Vec3f C(Rt * norm_vec); Vec3f A(point_crossmat * C); Eigen::Matrix J; J.setZero(); J << norm_vec[0], norm_vec[1], norm_vec[2], A[0], A[1], A[2]; float res = -corr_pts_[i][3]; // double w = huber_weight(res); double w = 1.0; JTJ[i] = (J.transpose() * J).eval() * w; JTr[i] = J.transpose() * res * w; res_sq[i] = res * res; }); for (int i = 0; i < index.size(); ++i) { obs.HTH_ += JTJ[i] * options_.plane_icp_weight_; obs.HTr_ += JTr[i] * options_.plane_icp_weight_; } if (!res_sq.empty()) { std::sort(res_sq.begin(), res_sq.end()); obs.lidar_residual_mean_ = res_sq[res_sq.size() / 2]; obs.lidar_residual_max_ = res_sq[res_sq.size() - 1]; // LOG(INFO) << "residual mean: " << obs.lidar_residual_mean_ << ", max: " << obs.lidar_residual_max_ // << ", 85%: " << res_sq[res_sq.size() * 0.85]; } /// 点到点ICP部分 if (options_.enable_icp_part_) { JTJ.resize(cnt_pts); JTr.resize(cnt_pts); std::vector index(cnt_pts); for (size_t i = 0; i < index.size(); ++i) { index[i] = i; } std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const size_t &i) { if (point_selected_icp_[i] == false) { return; } /// TODO: 外参 Vec3d q = scan_down_body_->points[i].getVector3fMap().cast(); Vec3d qs = scan_down_world_->points[i].getVector3fMap().cast(); Eigen::Matrix J; J.setZero(); /// translation 部分 J.block<3, 3>(0, 0) = Mat3d::Identity(); /// rotation 部分 J.block<3, 3>(0, 3) = -(s.rot_.matrix() * offset_R_lidar_fixed_) * SO3::hat(q); Vec3d e = qs - nearest_points_[i][0].getVector3fMap().cast(); if (e.norm() > 0.5) { point_selected_icp_[i] = false; return; } JTJ[i] = J.transpose() * J; JTr[i] = -J.transpose() * e; }); for (int i = 0; i < cnt_pts; ++i) { if (point_selected_icp_[i] == false) { continue; } obs.HTH_ += JTJ[i] * options_.icp_weight_; obs.HTr_ += JTr[i] * options_.icp_weight_; } } } /////////////////////////// private method ///////////////////////////////////////////////////////////////////// CloudPtr LaserMapping::GetGlobalMap(bool use_lio_pose, bool use_voxel, float res) { CloudPtr global_map(new PointCloudType); pcl::VoxelGrid voxel; voxel.setLeafSize(res, res, res); for (auto &kf : all_keyframes_) { CloudPtr cloud = kf->GetCloud(); CloudPtr cloud_filter(new PointCloudType); if (use_voxel) { voxel.setInputCloud(cloud); voxel.filter(*cloud_filter); } else { cloud_filter = cloud; } CloudPtr cloud_trans(new PointCloudType); if (use_lio_pose) { pcl::transformPointCloud(*cloud_filter, *cloud_trans, kf->GetLIOPose().matrix()); } else { pcl::transformPointCloud(*cloud_filter, *cloud_trans, kf->GetOptPose().matrix()); } *global_map += *cloud_trans; LOG(INFO) << "kf " << kf->GetID() << ", pose: " << kf->GetOptPose().translation().transpose(); } CloudPtr global_map_filtered(new PointCloudType); if (use_voxel) { voxel.setInputCloud(global_map); voxel.filter(*global_map_filtered); } else { global_map_filtered = global_map; } global_map_filtered->is_dense = false; global_map_filtered->height = 1; global_map_filtered->width = global_map_filtered->size(); LOG(INFO) << "global map: " << global_map_filtered->size(); return global_map_filtered; } void LaserMapping::SaveMap() { /// 保存地图 auto global_map = GetGlobalMap(true); pcl::io::savePCDFileBinaryCompressed("./data/lio.pcd", *global_map); LOG(INFO) << "lio map is saved to ./data/lio.pcd"; } CloudPtr LaserMapping::GetRecentCloud() { if (lidar_buffer_.empty()) { return nullptr; } return lidar_buffer_.front(); } CloudPtr LaserMapping::GetProjCloud() { auto cloud = scan_undistort_; ProjectKFs(cloud); return cloud; } } // namespace lightning ================================================ FILE: src/core/lio/laser_mapping.h ================================================ #ifndef FASTER_LIO_LASER_MAPPING_H #define FASTER_LIO_LASER_MAPPING_H #include #include #include #include #include "common/eigen_types.h" #include "common/imu.h" #include "common/keyframe.h" #include "common/options.h" #include "core/ivox3d/ivox3d.h" #include "core/lio/eskf.hpp" #include "core/lio/imu_processing.hpp" #include "pointcloud_preprocess.h" #include "livox_ros_driver2/msg/custom_msg.hpp" namespace lightning { namespace ui { class PangolinWindow; } /** * laser mapping * 目前有个问题：点云在缓存之后，实际处理的并不是最新的那个点云（通常是buffer里的前一个），这是因为bag里的点云用的开始时间戳，导致 * 点云的结束时间要比IMU多0.1s左右。为了同步最近的IMU，就只能处理缓冲队列里的那个点云，而不是最新的点云 */ class LaserMapping { public: struct Options { Options() {} bool is_in_slam_mode_ = true; // 是否在slam模式下 bool enable_icp_part_ = true; // 是否添加ICP部分 double plane_icp_weight_ = 1.0; // 点面ICP部分的权重 double icp_weight_ = 100; // ICP部分的权重 int min_pts = 300; // 配准所需的点数 /// 关键帧阈值 double kf_dis_th_ = 2.0; double kf_angle_th_ = 15 * M_PI / 180.0; bool proj_kfs_ = false; int max_proj_kfs_ = 5; }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW using IVoxType = IVox<3, IVoxNodeType::DEFAULT, PointType>; LaserMapping(Options options = Options()); ~LaserMapping() { scan_down_body_ = nullptr; scan_undistort_ = nullptr; scan_down_world_ = nullptr; LOG(INFO) << "laser mapping deconstruct"; } /// init without ros bool Init(const std::string &config_yaml); bool Run(); // callbacks of lidar and imu /// 处理ROS2的点云 void ProcessPointCloud2(const sensor_msgs::msg::PointCloud2::SharedPtr &msg); /// 处理livox的点云 void ProcessPointCloud2(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg); /// 如果已经做了预处理，也可以直接处理点云 void ProcessPointCloud2(CloudPtr cloud); void ProcessIMU(const lightning::IMUPtr &msg_in); /// 保存前端的地图 void SaveMap(); void SetUI(std::shared_ptr ui) { ui_ = ui; } /// 获取关键帧 Keyframe::Ptr GetKeyframe() const { return last_kf_; } /// 获取激光的状态 NavState GetState() const { return state_point_; } /// 获取IMU状态 NavState GetIMUState() const { if (p_imu_->IsIMUInited()) { return kf_imu_.GetX(); } else { NavState s; s.pose_is_ok_ = false; return s; } } CloudPtr GetScanUndist() const { return scan_undistort_; } CloudPtr GetProjCloud(); /// 获取最新的点云 CloudPtr GetRecentCloud(); std::vector GetAllKeyframes() { return all_keyframes_; } /** * 计算全局地图 * @param use_lio_pose * @return */ CloudPtr GetGlobalMap(bool use_lio_pose, bool use_voxel = true, float res = 0.1); private: // sync lidar with imu bool SyncPackages(); void ObsModel(NavState &s, ESKF::CustomObservationModel &obs); inline void PointBodyToWorld(const PointType &pi, PointType &po) { Vec3d p_global(state_point_.rot_ * (offset_R_lidar_fixed_ * pi.getVector3fMap().cast() + offset_t_lidar_fixed_) + state_point_.pos_); po.x = p_global(0); po.y = p_global(1); po.z = p_global(2); po.intensity = pi.intensity; } void MapIncremental(); bool LoadParamsFromYAML(const std::string &yaml); /// 创建关键帧 void MakeKF(); /// 将附近的关键帧投影至cloud中 void ProjectKFs(CloudPtr cloud, int size_limit = 1000); private: Options options_; /// modules IVoxType::Options ivox_options_; std::shared_ptr ivox_ = nullptr; // localmap in ivox std::shared_ptr preprocess_ = nullptr; // point cloud preprocess std::shared_ptr p_imu_ = nullptr; // imu process /// local map related double filter_size_map_min_ = 0; /// params std::vector extrinT_{3, 0.0}; // lidar-imu translation std::vector extrinR_{9, 0.0}; // lidar-imu rotation Mat3d offset_R_lidar_fixed_ = Mat3d::Identity(); Vec3d offset_t_lidar_fixed_ = Vec3d::Zero(); std::string map_file_path_; std::vector all_keyframes_; Keyframe::Ptr last_kf_ = nullptr; int kf_id_ = 0; /// point clouds data CloudPtr scan_undistort_{new PointCloudType()}; // scan after undistortion CloudPtr scan_down_body_{new PointCloudType()}; // downsampled scan in body CloudPtr scan_down_world_{new PointCloudType()}; // downsampled scan in world pcl::VoxelGrid voxel_scan_; // voxel filter for current scan /// 点面相关 std::vector nearest_points_; // nearest points of current scan std::vector corr_pts_; // inlier pts std::vector corr_norm_; // inlier plane norms std::vector residuals_; // point-to-plane residuals std::vector point_selected_surf_; // selected points std::vector plane_coef_; // plane coeffs /// 点到点相关 std::vector point_selected_icp_; // 点到点的selected points std::mutex mtx_buffer_; std::deque time_buffer_; std::deque lidar_buffer_; std::deque imu_buffer_; /// options bool keep_first_imu_estimation_ = false; // 在没有建立地图前，是否要使用前几帧的IMU状态 double timediff_lidar_wrt_imu_ = 0.0; double last_timestamp_lidar_ = 0; double lidar_end_time_ = 0; double last_timestamp_imu_ = -1.0; double first_lidar_time_ = 0.0; bool lidar_pushed_ = false; bool enable_skip_lidar_ = true; // 雷达是否需要跳帧 int skip_lidar_num_ = 5; // 每隔多少帧跳一个雷达 int skip_lidar_cnt_ = 0; /// statistics and flags /// int scan_count_ = 0; int publish_count_ = 0; bool flg_first_scan_ = true; bool flg_EKF_inited_ = false; double lidar_mean_scantime_ = 0.0; int scan_num_ = 0; int effect_feat_surf_ = 0, frame_num_ = 0, effect_feat_icp_ = 0; double last_lidar_time_ = 0; ///////////////////////// EKF inputs and output /////////////////////////////////////////////////////// MeasureGroup measures_; // sync IMU and lidar scan ESKF kf_; // 点云时刻的IMU状态 ESKF kf_imu_; // imu 最新时刻的eskf状态 NavState state_point_; // ekf current state bool use_aa_ = false; // use anderson acceleration? std::list proj_kfs_; // 投影到当前帧的关键帧 std::shared_ptr ui_ = nullptr; }; } // namespace lightning #endif // FASTER_LIO_LASER_MAPPING_H ================================================ FILE: src/core/lio/pointcloud_preprocess.cc ================================================ #include "pointcloud_preprocess.h" #include #include namespace lightning { void PointCloudPreprocess::Set(LidarType lid_type, double bld, int pfilt_num) { lidar_type_ = lid_type; blind_ = bld; point_filter_num_ = pfilt_num; } void PointCloudPreprocess::Process(const sensor_msgs::msg::PointCloud2 ::SharedPtr &msg, PointCloudType::Ptr &pcl_out) { switch (lidar_type_) { case LidarType::OUST64: Oust64Handler(msg); break; case LidarType::VELO32: VelodyneHandler(msg); break; case LidarType::ROBOSENSE: RoboSenseHandler(msg); break; default: LOG(ERROR) << "Error LiDAR Type"; break; } *pcl_out = cloud_out_; } void PointCloudPreprocess::Process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &msg, PointCloudType::Ptr &pcl_out) { cloud_out_.clear(); cloud_full_.clear(); int plsize = msg->point_num; cloud_out_.reserve(plsize); cloud_full_.resize(plsize); std::vector is_valid_pt(plsize, 0); std::vector index(plsize - 1); for (uint i = 0; i < plsize - 1; ++i) { index[i] = i + 1; // 从1开始 } std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const uint &i) { // if ((msg->points[i].line < num_scans_) && // ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { if (i % point_filter_num_ != 0) { return; } cloud_full_[i].x = msg->points[i].x; cloud_full_[i].y = msg->points[i].y; cloud_full_[i].z = msg->points[i].z; cloud_full_[i].intensity = msg->points[i].reflectivity; // use curvature as time of each laser points, curvature unit: ms cloud_full_[i].time = msg->points[i].offset_time / double(1000000); if (cloud_full_[i].z < height_min_ || cloud_full_[i].z > height_max_) { return; } if ((abs(cloud_full_[i].x - cloud_full_[i - 1].x) > 1e-7) || (abs(cloud_full_[i].y - cloud_full_[i - 1].y) > 1e-7) || (abs(cloud_full_[i].z - cloud_full_[i - 1].z) > 1e-7) && (cloud_full_[i].x * cloud_full_[i].x + cloud_full_[i].y * cloud_full_[i].y + cloud_full_[i].z * cloud_full_[i].z > (blind_ * blind_))) { is_valid_pt[i] = 1; } // } }); for (uint i = 1; i < plsize; i++) { if (is_valid_pt[i]) { cloud_out_.points.push_back(cloud_full_[i]); } } cloud_out_.width = cloud_out_.size(); cloud_out_.height = 1; cloud_out_.is_dense = false; *pcl_out = cloud_out_; } void PointCloudPreprocess::Oust64Handler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { cloud_out_.clear(); cloud_full_.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.size(); cloud_out_.reserve(plsize); for (int i = 0; i < pl_orig.points.size(); i++) { if (i % point_filter_num_ != 0) { continue; } double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < (blind_ * blind_)) { continue; } if (pl_orig.points[i].z < height_min_ || pl_orig.points[i].z > height_max_) { continue; } PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.time = pl_orig.points[i].t / 1e6; cloud_out_.points.push_back(added_pt); } cloud_out_.width = cloud_out_.size(); cloud_out_.height = 1; cloud_out_.is_dense = false; } void PointCloudPreprocess::RoboSenseHandler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { cloud_out_.clear(); cloud_full_.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.size(); cloud_out_.reserve(plsize); double head_time = msg->header.stamp.sec + msg->header.stamp.nanosec / 1e9; /// RoboSense的时间戳是double, 均为linux时间且单位为秒，这里减去header time并乘以1000得到毫秒为单位的时间戳 for (int i = 0; i < pl_orig.points.size(); i++) { if (i % point_filter_num_ != 0) { continue; } double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < (blind_ * blind_)) { continue; } if (pl_orig.points[i].z < height_min_ || pl_orig.points[i].z > height_max_) { continue; } PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.time = (pl_orig.points[i].timestamp - head_time) * 1e3; // / 1e6; // curvature unit: ms cloud_out_.points.push_back(added_pt); } cloud_out_.width = cloud_out_.size(); cloud_out_.height = 1; cloud_out_.is_dense = false; } void PointCloudPreprocess::VelodyneHandler(const sensor_msgs::msg::PointCloud2::SharedPtr &msg) { cloud_out_.clear(); cloud_full_.clear(); pcl::PointCloud pl_orig; pcl::fromROSMsg(*msg, pl_orig); int plsize = pl_orig.points.size(); cloud_out_.reserve(plsize); /*** These variables only works when no point timestamps given ***/ double omega_l = 3.61; // scan angular velocity std::vector is_first(num_scans_, true); std::vector yaw_fp(num_scans_, 0.0); // yaw of first scan point std::vector yaw_last(num_scans_, 0.0); // yaw of last scan point std::vector time_last(num_scans_, 0.0); // last offset time /*****************************************************************/ if (pl_orig.points[plsize - 1].time > 0) { given_offset_time_ = true; } else { given_offset_time_ = false; double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; double yaw_end = yaw_first; int layer_first = pl_orig.points[0].ring; for (uint i = plsize - 1; i > 0; i--) { if (pl_orig.points[i].ring == layer_first) { yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; break; } } } for (int i = 0; i < plsize; i++) { PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.time = pl_orig.points[i].time * time_scale_; // curvature unit: ms if (!given_offset_time_) { int layer = pl_orig.points[i].ring; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; if (is_first[layer]) { yaw_fp[layer] = yaw_angle; is_first[layer] = false; added_pt.time = 0.0; yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.time; continue; } // compute offset time if (yaw_angle <= yaw_fp[layer]) { added_pt.time = (yaw_fp[layer] - yaw_angle) / omega_l; } else { added_pt.time = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l; } if (added_pt.time < time_last[layer]) { added_pt.time += 360.0 / omega_l; } yaw_last[layer] = yaw_angle; time_last[layer] = added_pt.time; } if (i % point_filter_num_ == 0) { if (added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > (blind_ * blind_)) { cloud_out_.points.push_back(added_pt); } } } cloud_out_.width = cloud_out_.size(); cloud_out_.height = 1; cloud_out_.is_dense = false; } } // namespace lightning ================================================ FILE: src/core/lio/pointcloud_preprocess.h ================================================ #ifndef FASTER_LIO_POINTCLOUD_PROCESSING_H #define FASTER_LIO_POINTCLOUD_PROCESSING_H #include #include #include #include "common/measure_group.h" #include "common/point_def.h" #include "livox_ros_driver2/msg/custom_msg.hpp" namespace lightning { enum class LidarType { AVIA = 1, VELO32 = 2, OUST64 = 3, ROBOSENSE = 4 }; /** * point cloud preprocess * just unify the point format from livox/velodyne to PCL * * 预处理程序 * 主要是对各种不同的雷达处理时间戳差异 */ class PointCloudPreprocess { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW PointCloudPreprocess() = default; ~PointCloudPreprocess() = default; /// processors void Process(const sensor_msgs::msg::PointCloud2 ::SharedPtr &msg, PointCloudType::Ptr &pcl_out); void Process(const livox_ros_driver2::msg::CustomMsg::SharedPtr &cloud, PointCloudType::Ptr &pcl_out); void Set(LidarType lid_type, double bld, int pfilt_num); // accessors double &Blind() { return blind_; } int &NumScans() { return num_scans_; } int &PointFilterNum() { return point_filter_num_; } float &TimeScale() { return time_scale_; } LidarType GetLidarType() const { return lidar_type_; } void SetLidarType(LidarType lt) { lidar_type_ = lt; } void SetHeightROI(float height_max, float height_min) { height_max_ = height_max; height_min_ = height_min; } private: void Oust64Handler(const sensor_msgs::msg::PointCloud2 ::SharedPtr &msg); void RoboSenseHandler(const sensor_msgs::msg::PointCloud2 ::SharedPtr &msg); void VelodyneHandler(const sensor_msgs::msg::PointCloud2 ::SharedPtr &msg); PointCloudType cloud_full_, cloud_out_; LidarType lidar_type_ = LidarType::AVIA; int point_filter_num_ = 1; int num_scans_ = 6; double blind_ = 0.01; float time_scale_ = 1e-3; bool given_offset_time_ = false; float height_max_ = 1.0; float height_min_ = -1.0; }; } // namespace lightning #endif ================================================ FILE: src/core/lio/pose6d.h ================================================ // // Created by xiang on 25-4-14. // #ifndef FASTER_LIO_POSE6D_H #define FASTER_LIO_POSE6D_H #include "common/eigen_types.h" namespace lightning { struct Pose6D { Pose6D() = default; Pose6D(const double t, const Vec3d &a, const Vec3d &g, const Vec3d &v, const Vec3d &p, const Mat3d &R) { offset_time = t; acc = a; gyr = g; vel = v; pos = p; rot = R; }; double offset_time = 0; Vec3d acc = Vec3d::Zero(); Vec3d gyr = Vec3d::Zero(); Vec3d vel = Vec3d::Zero(); Vec3d pos = Vec3d::Zero(); Mat3d rot = Mat3d::Identity(); }; } // namespace lightning #endif // FASTER_LIO_POSE6D_H ================================================ FILE: src/core/localization/lidar_loc/lidar_loc.cc ================================================ #include #include #include #include #include #include #include #include "pclomp/ndt_omp_impl.hpp" #include "pclomp/voxel_grid_covariance_omp_impl.hpp" #include "core/localization/lidar_loc/lidar_loc.h" #include #include "glog/logging.h" #include "io/file_io.h" #include "io/yaml_io.h" #include "ui/pangolin_window.h" #include "utils/timer.h" namespace lightning::loc { LidarLoc::LidarLoc(LidarLoc::Options options) : options_(options) { pcl_ndt_.reset(new NDTType()); pcl_ndt_->setResolution(1.0); pcl_ndt_->setNeighborhoodSearchMethod(pclomp::DIRECT7); pcl_ndt_->setOulierRatio(0.45); pcl_ndt_->setStepSize(0.1); pcl_ndt_->setTransformationEpsilon(0.01); pcl_ndt_->setMaximumIterations(20); pcl_ndt_->setNumThreads(4); pcl_ndt_rough_.reset(new NDTType()); pcl_ndt_rough_->setResolution(5.0); pcl_ndt_rough_->setNeighborhoodSearchMethod(pclomp::DIRECT7); pcl_ndt_rough_->setStepSize(0.1); pcl_ndt_rough_->setMaximumIterations(4); pcl_ndt_rough_->setNumThreads(4); pcl_icp_.reset(new ICPType()); pcl_icp_->setMaximumIterations(4); pcl_icp_->setTransformationEpsilon(0.01); LOG(INFO) << "match name is NDT_OMP" << ", MaximumIterations is: " << pcl_ndt_->getMaximumIterations(); } LidarLoc::~LidarLoc() { if (update_map_thread_.joinable()) { update_map_quit_ = true; update_map_thread_.join(); } recover_pose_out_.close(); } bool LidarLoc::Init(const std::string& config_path) { YAML_IO yaml(config_path); options_.map_option_.enable_dynamic_polygon_ = yaml.GetValue("maps", "with_dyn_area"); options_.map_option_.max_pts_in_dyn_chunk_ = yaml.GetValue("maps", "max_pts_dyn_chunk"); options_.map_option_.load_map_size_ = yaml.GetValue("maps", "load_map_size"); options_.map_option_.unload_map_size_ = yaml.GetValue("maps", "unload_map_size"); options_.update_kf_dis_ = yaml.GetValue("lidar_loc", "update_kf_dis"); options_.update_lidar_loc_score_ = yaml.GetValue("lidar_loc", "update_lidar_loc_score"); options_.min_init_confidence_ = yaml.GetValue("lidar_loc", "min_init_confidence"); // options_.filter_z_min_ = yaml.GetValue("lidar_loc", "filter_z_min"); // options_.filter_z_max_ = yaml.GetValue("lidar_loc", "filter_z_max"); // options_.filter_intensity_min_ = yaml.GetValue("lidar_loc", "filter_intensity_min"); // options_.filter_intensity_max_ = yaml.GetValue("lidar_loc", "filter_intensity_max"); options_.lidar_loc_odom_th_ = yaml.GetValue("lidar_loc", "lidar_loc_odom_th"); options_.init_with_fp_ = yaml.GetValue("lidar_loc", "init_with_fp"); options_.enable_parking_static_ = yaml.GetValue("lidar_loc", "enable_parking_static"); options_.enable_icp_adjust_ = yaml.GetValue("lidar_loc", "enable_icp_adjust"); options_.with_height_ = yaml.GetValue("loop_closing", "with_height"); options_.try_self_extrap_ = yaml.GetValue("lidar_loc", "try_self_extrap"); lidar_loc::grid_search_angle_step = yaml.GetValue("lidar_loc", "grid_search_angle_step"); lidar_loc::grid_search_angle_range = yaml.GetValue("lidar_loc", "grid_search_angle_range"); LOG(INFO) << "min init confidence: " << options_.min_init_confidence_; std::string map_policy = yaml.GetValue("maps", "dyn_cloud_policy"); if (map_policy == "short") { options_.map_option_.policy_ = TiledMap::DynamicCloudPolicy::SHORT; } else if (map_policy == "long") { options_.map_option_.policy_ = TiledMap::DynamicCloudPolicy::LONG; } else if (map_policy == "persistent") { options_.map_option_.policy_ = TiledMap::DynamicCloudPolicy::PERSISTENT; } options_.map_option_.delete_when_unload_ = yaml.GetValue("maps", "delete_when_unload"); options_.map_option_.load_dyn_cloud_ = yaml.GetValue("maps", "load_dyn_cloud"); options_.map_option_.save_dyn_when_quit_ = yaml.GetValue("maps", "save_dyn_when_quit"); options_.map_option_.save_dyn_when_unload_ = yaml.GetValue("maps", "save_dyn_when_unload"); map_ = std::make_shared(options_.map_option_); map_->LoadMapIndex(); auto fps = map_->GetAllFP(); if (!fps.empty()) { map_->LoadOnPose(fps.front().pose_); /// 更新一次地图，保证有初始数据 UpdateGlobalMap(); } /// load recover pose if exist if (PathExists(options_.recover_pose_path_)) { std::ifstream fin(options_.recover_pose_path_); double data[7] = {0, 0, 0, 0, 0, 0, 1}; for (int i = 0; i < 7; ++i) { fin >> data[i]; } SE3 pose(Quatd(data[6], data[3], data[4], data[5]), Vec3d(data[0], data[1], data[2])); FunctionalPoint fp_recover; fp_recover.name_ = "recover"; fp_recover.pose_ = pose; map_->AddFP(fp_recover); } update_map_thread_ = std::thread([this]() { LidarLoc::UpdateMapThread(); }); return true; } bool LidarLoc::ProcessCloud(CloudPtr cloud_input) { assert(cloud_input != nullptr); if (cloud_input->empty() || cloud_input->size() < 50) { LOG(WARNING) << "loc input is empty or invalid, sz: " << cloud_input->size(); return false; } // CloudPtr cloud(new PointCloudType); // pcl::VoxelGrid voxel; // float sz = 0.1; // voxel.setLeafSize(sz, sz, sz); // voxel.setInputCloud(cloud_input); // voxel.filter(*cloud); current_scan_ = cloud_input; Align(cloud_input); return true; } NavState LidarLoc::GetState() { UL lock_res(result_mutex_); NavState ns; ns.SetPose(current_abs_pose_); ns.timestamp_ = current_timestamp_; ns.confidence_ = current_score_; UL lock(lo_pose_mutex_); if (!lo_pose_queue_.empty()) { auto s = lo_pose_queue_.back(); ns.SetVel(s.GetVel()); } return ns; } bool LidarLoc::ProcessDR(const NavState& state) { // 未初始化成功的数据不接收 if (!state.pose_is_ok_) { return false; } // DR数据check UL lock(dr_pose_mutex_); if (!dr_pose_queue_.empty()) { const double last_stamp = dr_pose_queue_.back().timestamp_; if (state.timestamp_ < last_stamp) { return false; } } dr_pose_queue_.emplace_back(state); while (dr_pose_queue_.size() >= 1000) { dr_pose_queue_.pop_front(); } return true; } bool LidarLoc::ProcessLO(const NavState& state) { /// 理论上相对定位是按时间顺序到达的 UL lock(lo_pose_mutex_); if (!lo_pose_queue_.empty()) { const double last_stamp = lo_pose_queue_.back().timestamp_; if (state.timestamp_ < last_stamp) { LOG(WARNING) << "当前相对定位的结果的时间戳应当比上一个时间戳数值大，实际相减得" << state.timestamp_ - last_stamp; return false; } } lo_pose_queue_.emplace_back(state); while (lo_pose_queue_.size() >= 50) { lo_pose_queue_.pop_front(); } if (state.lidar_odom_reliable_ == false) { lo_reliable_ = false; lo_reliable_cnt_ = 10; } else { if (state.lidar_odom_reliable_ && lo_reliable_cnt_ > 0) { lo_reliable_cnt_--; } if (lo_reliable_cnt_ == 0) { lo_reliable_ = true; } } return true; } bool LidarLoc::YawSearch(SE3& pose, double& confidence, CloudPtr input, CloudPtr output) { SE3 init_pose = pose; auto RPYXYZ = math::SE3ToRollPitchYaw(init_pose); double init_yaw = RPYXYZ.yaw; confidence = 0; bool yaw_search_success = false; int step = lidar_loc::grid_search_angle_step; double radius = lidar_loc::grid_search_angle_range * constant::kDEG2RAD; double angle_search_step = 2 * radius / step; std::vector searched_yaw; std::vector scores(step); std::vector index; std::vector pose_opti(step); for (int i = 0; i < step; ++i) { double search_yaw = init_yaw + i * angle_search_step - radius; searched_yaw.emplace_back(search_yaw); index.emplace_back(i); } LOG(INFO) << "init yaw: " << init_yaw << ", p: " << RPYXYZ.pitch << ", ro: " << RPYXYZ.roll << ", search from " << searched_yaw.front() << " to " << searched_yaw.back(); /// 粗分辨率 std::for_each(index.begin(), index.end(), [&](int i) { double fitness_score = 0; RPYXYZ.yaw = searched_yaw[i]; SE3 pose_esti = math::XYZRPYToSE3(RPYXYZ); Localize(pose_esti, fitness_score, input, output, true); scores[i] = fitness_score; pose_opti[i] = pose_esti; }); // find best match auto best_score_idx = std::max_element(scores.begin(), scores.end()) - scores.begin(); confidence = scores.at(best_score_idx); pose = pose_opti.at(best_score_idx); /// 高分辨率 if (confidence > options_.min_init_confidence_) { Localize(pose, confidence, input, output, false); } if (confidence > options_.min_init_confidence_) { LOG(INFO) << "init success, score: " << confidence << ", th=" << options_.min_init_confidence_; Eigen::Vector3d suc_translation = pose.translation(); Eigen::Matrix3d suc_rotation_matrix = pose.rotationMatrix(); double suc_x = suc_translation.x(); double suc_y = suc_translation.y(); double suc_yaw = atan2(suc_rotation_matrix(1, 0), suc_rotation_matrix(0, 0)); LOG(INFO) << "localization init success, pose: " << suc_x << ", " << suc_y << ", " << suc_yaw << ", conf: " << confidence; yaw_search_success = true; } return yaw_search_success; } bool LidarLoc::InitWithFP(CloudPtr input, const SE3& fp_pose) { assert(input != nullptr && !input->empty()); // 使用功能点的位置进行定位初始化 double fitness_score; SE3 pose_esti = fp_pose; CloudPtr output_cloud(new PointCloudType); // loc_inited_ = YawSearch(pose_esti, fitness_score, input, output_cloud); loc_inited_ = Localize(pose_esti, fitness_score, input, output_cloud); if (loc_inited_) { current_timestamp_ = math::ToSec(input->header.stamp); localization_result_.confidence_ = fitness_score; current_abs_pose_ = pose_esti; localization_result_.pose_ = pose_esti; localization_result_.timestamp_ = current_timestamp_; localization_result_.lidar_loc_valid_ = true; localization_result_.status_ = LocalizationStatus::GOOD; last_abs_pose_set_ = true; last_abs_pose_ = pose_esti; current_score_ = fitness_score; LOG(INFO) << "fitness_score is: " << fitness_score << ", global_pose is: " << fp_pose.translation().transpose(); LOG(INFO) << " [Loc init pose]: " << last_abs_pose_.translation().transpose(); map_height_ = fp_pose.translation()[2]; if (current_lo_pose_set_) { // 设置上一次的相对定位结果 last_lo_pose_ = current_lo_pose_; last_lo_pose_set_ = true; last_dr_pose_ = current_dr_pose_; last_dr_pose_set_ = true; } // 定位成功，则清空失败记录 fp_init_fail_pose_vec_.clear(); } else { // 添加失败历史记录 LOG(INFO) << "init failed, score: " << fitness_score; fp_init_fail_pose_vec_.emplace_back(fp_pose); fp_last_tried_time_ = 1e-6 * static_cast(input->header.stamp); } return loc_inited_; } void LidarLoc::ResetLastPose(const SE3& last_pose) { last_abs_pose_ = last_pose; // TODO：清空动态图层 return; } bool LidarLoc::TryOtherSolution(CloudPtr input, SE3& pose) { double fitness_score; SE3 pose_esti = pose; CloudPtr output_cloud(new PointCloudType); bool loc_success = Localize(pose_esti, fitness_score, input, output_cloud); if (loc_success) { // 激光重置逻辑 float score_th = std::min(1.5 * current_score_, current_score_ + 0.3); if (fitness_score > score_th && fitness_score > 1.0) { // 显著好于现在的估计 LOG(WARNING) << "rtk solution is significantly better: " << fitness_score << " " << current_score_; pose = pose_esti; localization_result_.lidar_loc_smooth_flag_ = false; return true; } else { LOG(INFO) << "not using rtk solution: " << fitness_score << " " << current_score_; return false; } } return false; } bool LidarLoc::UpdateGlobalMap() { NDTType::Ptr ndt(new NDTType()); ndt->setResolution(1.0); ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); ndt->setStepSize(0.1); ndt->setMaximumIterations(4); ndt->setNumThreads(4); map_->SetNewTargetForNDT(ndt); ndt->initCompute(); UL lock(match_mutex_); pcl_ndt_ = ndt; if (!loc_inited_) { NDTType::Ptr ndt_rough(new NDTType()); ndt_rough->setResolution(5.0); ndt_rough->setNeighborhoodSearchMethod(pclomp::DIRECT7); ndt_rough->setStepSize(0.1); ndt_rough->setMaximumIterations(4); ndt_rough->setNumThreads(4); map_->SetNewTargetForNDT(ndt_rough); // ndt_rough->initCompute(); pcl_ndt_rough_ = ndt_rough; } if (options_.enable_icp_adjust_) { ICPType::Ptr icp(new ICPType()); CloudPtr map_cloud(new PointCloudType); pcl::VoxelGrid voxel; auto sz = 0.5; voxel.setLeafSize(sz, sz, sz); voxel.setInputCloud(map_->GetAllMap()); voxel.filter(*map_cloud); icp->setInputTarget(map_cloud); icp->setMaximumIterations(4); icp->setTransformationEpsilon(0.01); pcl_icp_ = icp; } return true; } void LidarLoc::UpdateMapThread() { LOG(INFO) << "UpdateMapThread thread is running"; while (!update_map_quit_) { if (map_->MapUpdated() || map_->DynamicMapUpdated()) { UpdateGlobalMap(); if (ui_) { ui_->UpdatePointCloudGlobal(map_->GetStaticCloud()); ui_->UpdatePointCloudDynamic(map_->GetDynamicCloud()); } map_->CleanMapUpdate(); } usleep(10000); } } void LidarLoc::SetInitialPose(SE3 init_pose) { UL lock(initial_pose_mutex_); loc_inited_ = false; // map_->ClearMap(); initial_pose_set_ = true; initial_pose_ = init_pose; LOG(INFO) << "Set initial pose is: " << initial_pose_.translation().transpose(); } void LidarLoc::Align(const CloudPtr& input) { // 输入必须非空 assert(input != nullptr); // 点云去畸变定到了结束时间，所以该点云的定位也是到结束时间的 double current_time = math::ToSec(input->header.stamp) + lo::lidar_time_interval; current_timestamp_ = current_time; LOG(INFO) << "current time: " << std::fixed << std::setprecision(12) << current_timestamp_; /// 设置当前帧对应的rel_pose if (!AssignLOPose(current_time)) { LOG(WARNING) << "assign LO pose failed"; } if (!AssignDRPose(current_time)) { LOG(WARNING) << "assign DR pose failed"; } /// 1. 车辆静止处理 if (parking_ && loc_inited_) { LOG(INFO) << "车辆静止，不做匹配"; UpdateState(input); current_abs_pose_ = last_abs_pose_; lidar_loc_pose_queue_.emplace_back(current_time, current_abs_pose_); UL lock(result_mutex_); localization_result_.timestamp_ = current_time; localization_result_.pose_ = current_abs_pose_; if (options_.enable_parking_static_) { localization_result_.is_parking_ = true; localization_result_.valid_ = true; } return; } /// 2. 初始化处理 if (!loc_inited_) { UL lock_init(initial_pose_mutex_); LOG(INFO) << "initing lidarloc"; SetInitRltState(); if (initial_pose_set_) { /// 尝试在给定点初始化 if (InitWithFP(input, initial_pose_)) { LOG(INFO) << "init with external pose: " << initial_pose_.translation().transpose(); initial_pose_set_ = false; return; } } if (options_.init_with_fp_) { /// 从功能点初始化 /// 如果之前尝试过，那么需要间隔一段时间再进行搜索 if (!fp_init_fail_pose_vec_.empty() && current_dr_pose_set_) { SE3 last_tried_pose = fp_init_fail_pose_vec_.back(); bool should_try = (current_time - fp_last_tried_time_) > 2.0 || (current_dr_pose_.translation() - last_tried_pose.translation()).norm() > 0.3 || (current_dr_pose_.so3().inverse() * last_tried_pose.so3()).log().norm() > 10 * M_PI / 180.0; if (!should_try) { LOG(INFO) << "skip trying init, please move to another place."; return; } } else { LOG(INFO) << "fp tried pose: " << fp_init_fail_pose_vec_.size() << ", dr pose set: " << current_dr_pose_set_; } auto all_fps = map_->GetAllFP(); bool fp_init_success = false; for (const auto& fp : all_fps) { map_->LoadOnPose(fp.pose_); if (InitWithFP(input, fp.pose_)) { LOG(INFO) << "init with fp: " << fp.name_; fp_init_success = true; break; } } if (!fp_init_success) { LOG(INFO) << "FP init failed."; if (current_dr_pose_set_) { LOG(INFO) << "record fp failed time: " << std::setprecision(12) << current_time << ", pose: " << current_dr_pose_.translation().transpose(); fp_last_tried_time_ = current_time; fp_init_fail_pose_vec_.emplace_back(current_dr_pose_); } } else { fp_last_tried_time_ = 0; fp_init_fail_pose_vec_.clear(); } } /// 初始化未成功时，不往下走流程 return; } /// 4. 设置当前帧对应的 pose guess /// NOTE: LO设置预测的位置和LidarLoc自身递推设置预测的方法并不完全一致，自身外推容易受噪声影响 SE3 guess_from_lo = last_abs_pose_; if (last_lo_pose_set_ && current_lo_pose_set_) { // 如果有里程计，则用两个时刻的相对定位来递推，估计一个当前pose的初值 const SE3 delta = last_lo_pose_.inverse() * current_lo_pose_; guess_from_lo = last_abs_pose_ * delta; LOG(INFO) << "current lo pose: " << current_lo_pose_.translation().transpose(); LOG(INFO) << "last lo pose: " << last_lo_pose_.translation().transpose(); LOG(INFO) << "lo motion: " << delta.translation().transpose(); LOG(INFO) << "last abs pose: " << last_abs_pose_.translation().transpose(); // guess_from_lo.translation()[2] = 0; LOG(INFO) << "loc using lo guess: " << guess_from_lo.translation().transpose(); } SE3 guess_from_self = guess_from_lo; if (lidar_loc_pose_queue_.size() >= 2) { SE3 pred; TimedPose match; if (math::PoseInterp( current_time, lidar_loc_pose_queue_, [](const TimedPose& p) { return p.timestamp_; }, [](const TimedPose& p) { return p.pose_; }, pred, match, 2.0)) { guess_from_self = pred; } } // SE3 guess_from_dr = guess_from_lo; // if (last_dr_pose_set_ && current_dr_pose_set_) { // const SE3 delta = last_dr_pose_.inverse() * current_dr_pose_; // guess_from_dr = last_abs_pose_ * delta; // // guess_from_dr.translation()[2] = 0; // } // bool try_dr = false; // if (((guess_from_dr.translation() - guess_from_lo.translation()).norm() >= try_other_guess_trans_th_ || // (guess_from_dr.so3().inverse() * guess_from_lo.so3()).log().norm() >= try_other_guess_rot_th_)) { // LOG(INFO) << "trying dr pose: " << guess_from_dr.translation().transpose() << ", " // << (guess_from_dr.so3().inverse() * guess_from_lo.so3()).log().norm() // << ", vel_norm: " << current_vel_b_.norm(); // try_dr = true; // } bool try_self = false; // if (options_.try_self_extrap_) { // if (((guess_from_self.translation() - guess_from_lo.translation()).norm() >= try_other_guess_trans_th_ || // (guess_from_self.so3().inverse() * guess_from_lo.so3()).log().norm() >= try_other_guess_rot_th_) && // ((guess_from_dr.translation() - guess_from_self.translation()).norm() >= try_other_guess_trans_th_ || // (guess_from_dr.so3().inverse() * guess_from_self.so3()).log().norm() >= try_other_guess_rot_th_)) { // LOG(INFO) << "trying self extrap pose: " << guess_from_self.translation().transpose() << ", " // << (guess_from_self.so3().inverse() * guess_from_lo.so3()).log().norm(); // try_self = true; // } // } /// 5. 载入地图, 与地图匹配定位 /// 尝试各种初始估计 CloudPtr output_cloud(new PointCloudType); double fitness_score = 0; SE3 current_pose_esti = guess_from_lo; bool loc_success_lo, loc_success_self, loc_success_dr; loc_success_lo = loc_success_self = loc_success_dr = false; bool loc_success = false; /// 注意load on pose存在滞后，优先load on DR map_->LoadOnPose(guess_from_lo); loc_success_lo = Localize(current_pose_esti, fitness_score, input, output_cloud); // LO 那个肯定会算 double score_lo = fitness_score; SE3 res_of_lo = current_pose_esti; SE3 res_of_dr = current_pose_esti; SE3 res_of_self = current_pose_esti; double score_dr = 0; // 先尝试外部预测，最后用自身 // if (try_dr) { // /// 尝试DR外推的pose // res_of_dr = guess_from_dr; // loc_success_dr = Localize(res_of_dr, score_dr, input, output_cloud); // if (score_dr > (fitness_score - 0.1)) { // current_pose_esti = res_of_dr; // fitness_score = score_dr; // LOG(INFO) << "take dr guess: " << current_pose_esti.translation().transpose() // << " , confidence: " << score_dr << ", v_norm: " << current_vel_b_.norm(); // } // } // 用纯激光定位有点太抖了，加一些权重 Vec6d delta = (guess_from_lo.inverse() * current_pose_esti).log(); SE3 esti_balanced = guess_from_lo * SE3::exp(delta * 0.1); current_pose_esti = esti_balanced; // double score_self = 0; // if (try_self) { // /// 尝试自身外推的pose // LOG(INFO) << "localize with extrap"; // res_of_self = guess_from_self; // loc_success_self = Localize(res_of_self, score_self, input, output_cloud); // // 避免分值接近但长时间采信自身预测，此处更相信外部预测源 // if (score_self > (fitness_score + 0.1)) { // current_pose_esti = res_of_self; // fitness_score = score_self; // LOG(INFO) << "take self guess: " << current_pose_esti.translation().transpose() // << " , confidence: " << score_self; // } // } /// NOTE 如果LO, DR出发点和收敛点不同，但分值相近，说明场景可能处在退化状态，此时使用DR预测的Pose // if (try_dr && (res_of_lo.translation() - res_of_dr.translation()).head<2>().norm() > 0.2 && // fabs(score_lo - score_dr) < 0.2 && score_lo < 1.2) { // LOG(WARNING) << "判定激光定位进入退化状态，现在会使用DR递推pose而不是激光定位位置"; // current_pose_esti = guess_from_dr; // } if (options_.force_2d_) { PoseRPYD RPYXYZ = math::SE3ToRollPitchYaw(current_pose_esti); RPYXYZ.roll = 0; RPYXYZ.pitch = 0; RPYXYZ.z = 0; current_pose_esti = math::XYZRPYToSE3(RPYXYZ); } // if (options_.with_height_) { // current_pose_esti.translation()[2] = map_height_; // LOG(INFO) << "adjust current pose to : " << current_pose_esti.translation().transpose(); // } current_abs_pose_ = current_pose_esti; current_score_ = fitness_score; double delta_rel_abs_pose = 0; bool lidar_loc_odom_valid = true; if (loc_success_lo || loc_success_self || loc_success_dr) { loc_success = true; } else { LOG(INFO) << "loc success is false."; } if (loc_success) { lidar_loc_odom_valid = CheckLidarOdomValid(current_pose_esti, delta_rel_abs_pose); last_timestamp_ = current_timestamp_; // 成功时，更新上一时刻激光定位时间 match_fail_count_ = 0; } else { current_score_ = fitness_score; LOG(WARNING) << "localization failed! score: " << current_score_; /// 若连续3帧匹配失败就设一个大分值 ++match_fail_count_; } /// 确定激光定位是否满足平滑性要求 Vec3d dpred = current_abs_pose_.translation() - guess_from_self.translation(); if (fabs(dpred[0]) < 0.5 && fabs(dpred[1]) < 0.5 && (current_abs_pose_.so3().inverse() * guess_from_self.so3()).log().norm() < 2.0 * M_PI / 180.0) { localization_result_.lidar_loc_smooth_flag_ = true; } else { localization_result_.lidar_loc_smooth_flag_ = false; } localization_result_.lidar_loc_odom_reliable_ = lo_reliable_; localization_result_.is_parking_ = false; // if (ui_) { // ui_->UpdatePredictPose(guess_from_lo); // } /// 7. 输出结果 { UL lock(result_mutex_); localization_result_.timestamp_ = current_timestamp_; localization_result_.confidence_ = fitness_score; if (match_fail_count_ < 100) { localization_result_.lidar_loc_valid_ = true; localization_result_.status_ = LocalizationStatus::GOOD; } else if (match_fail_count_ >= 100 && match_fail_count_ < 300) { localization_result_.lidar_loc_valid_ = false; localization_result_.status_ = LocalizationStatus::FOLLOWING_DR; } else { match_fail_count_ = 300; localization_result_.lidar_loc_valid_ = false; localization_result_.status_ = LocalizationStatus::FAIL; } localization_result_.lidar_loc_odom_delta_ = delta_rel_abs_pose; localization_result_.lidar_loc_odom_error_normal_ = lidar_loc_odom_valid; localization_result_.pose_ = current_pose_esti; } UpdateState(input); /// 8. 更新动态图层 /// 条件：1. 定位成功 2. 与上次更新间隔一定距离 3. RTK与激光定位横纵向误差都小于0.3，或者匹配分值大于1.0 bool score_cond = current_score_ > options_.update_lidar_loc_score_; if (options_.update_dynamic_cloud_ && loc_success && (((current_pose_esti.translation() - last_dyn_upd_pose_.pose_.translation()).norm() > options_.update_kf_dis_) || fabs(current_time - last_dyn_upd_pose_.timestamp_) > options_.update_kf_time_)) { if (score_cond /* || update_cache_dis_ < options_.max_update_cache_dis_ */) { // LOG(INFO) << "passing through z filter, input:" << input->size(); pcl::PassThrough pass; pass.setInputCloud(input); pass.setFilterFieldName("z"); pass.setFilterLimits(0.5, options_.filter_z_max_); CloudPtr input_z_filter(new PointCloudType()); pass.filter(*input_z_filter); if (!input_z_filter->empty()) { CloudPtr cloud_t(new PointCloudType()); pcl::transformPointCloud(*input_z_filter, *cloud_t, current_pose_esti.matrix()); // 以现在的scan来更新地图 map_->UpdateDynamicCloud(cloud_t, true); last_dyn_upd_pose_.timestamp_ = current_time; last_dyn_upd_pose_.pose_ = current_pose_esti; } } } if (lidar_loc_pose_queue_.empty()) { lidar_loc_pose_queue_.emplace_back(current_time, current_abs_pose_); } else if (current_time > lidar_loc_pose_queue_.back().timestamp_) { lidar_loc_pose_queue_.emplace_back(current_time, current_abs_pose_); } while (lidar_loc_pose_queue_.size() > 1000) { lidar_loc_pose_queue_.pop_front(); } ave_scores_.emplace_back(current_score_); while (ave_scores_.size() > 20) { ave_scores_.pop_front(); } /// 9. save for recover pose recover_pose_out_.open(options_.recover_pose_path_); if (recover_pose_out_) { Vec3d t = current_pose_esti.translation(); Quatd q = current_pose_esti.unit_quaternion(); recover_pose_out_ << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w(); recover_pose_out_.close(); } } bool LidarLoc::CheckLidarOdomValid(const SE3& current_pose_esti, double& delta_posi) { delta_posi = ((last_lo_pose_.inverse() * current_lo_pose_).translation() - (last_abs_pose_.inverse() * current_pose_esti).translation()) .head(2) .norm(); bool valid = true; if (delta_posi > options_.lidar_loc_odom_th_ && last_lo_pose_set_) { LOG(INFO) << "delta_rel_abs_pose is: " << delta_posi; LOG(INFO) << "LO相对pose: " << last_lo_pose_.translation().transpose() << " " << current_lo_pose_.translation().transpose() << " " << (last_lo_pose_.inverse() * current_lo_pose_).translation().transpose(); LOG(INFO) << "Lidar Loc计算相对pose: " << last_abs_pose_.translation().transpose() << " " << current_pose_esti.translation().transpose() << " " << (last_abs_pose_.inverse() * current_pose_esti).translation().transpose(); lo_reliable_ = false; lo_reliable_cnt_ = 10; valid = false; } last_abs_pose_ = current_pose_esti; last_lo_pose_ = current_lo_pose_; last_lo_pose_set_ = true; last_dr_pose_ = current_dr_pose_; last_dr_pose_set_ = true; return valid; } bool LidarLoc::Localize(SE3& pose, double& confidence, CloudPtr input, CloudPtr output, bool use_rough_res) { Eigen::Matrix4f trans; bool loc_success = false; Eigen::Matrix4f guess_pose = pose.matrix().cast(); LOG(INFO) << "loc from: " << pose.translation().transpose(); if (pcl_ndt_->getInputTarget() == nullptr) { LOG(INFO) << "lidar loc target is null, skip"; return false; } UL lock(match_mutex_); NDTType::Ptr ndt = nullptr; if (use_rough_res) { ndt = pcl_ndt_rough_; } else { ndt = pcl_ndt_; } ndt->setInputSource(input); ndt->align(*output, guess_pose); trans = ndt->getFinalTransformation(); confidence = ndt->getTransformationProbability(); auto tgt = ndt->getInputTarget(); if (!tgt->empty()) { pcl::io::savePCDFile("./data/tgt.pcd", *tgt); } if (loc_inited_ == false && confidence > options_.min_init_confidence_) { loc_success = true; } else { loc_success = true; } if (options_.enable_icp_adjust_ && loc_inited_) { Eigen::Matrix4f adjust_trans; CloudPtr input_voxel(new PointCloudType); pcl::VoxelGrid voxel_icp; double ls = 0.2; voxel_icp.setLeafSize(ls, ls, ls); voxel_icp.setInputCloud(input); voxel_icp.filter(*input_voxel); pcl_icp_->setInputSource(input_voxel); Timer::Evaluate([&]() { pcl_icp_->align(*output, trans); }, "pcl_icp adjust", true); adjust_trans = pcl_icp_->getFinalTransformation(); Eigen::Matrix3f rotation_diff = trans.block<3, 3>(0, 0).transpose() * adjust_trans.block<3, 3>(0, 0); Eigen::AngleAxisf angle_axis(rotation_diff); float a = angle_axis.angle(); float d = (trans.block<3, 1>(0, 3) - adjust_trans.block<3, 1>(0, 3)).norm(); LOG(INFO) << "icp adjust d: " << d << ", a: " << a; if (pcl_icp_->hasConverged() && std::fabs(d) <= 0.05 && std::fabs(a) <= 0.05) { LOG(INFO) << "icp ajust trans set success"; trans = adjust_trans; } } Eigen::Matrix3d rot = trans.block<3, 3>(0, 0).cast(); Quatd q_3d = Quatd(rot); Vec3d t_3d = trans.block<3, 1>(0, 3).cast(); q_3d.normalize(); pose = SE3(q_3d, t_3d); LOG(INFO) << "confidence: " << confidence << ", t: " << t_3d.transpose() << ", succ: " << loc_success; return loc_success; } bool LidarLoc::CheckStatic(double timestamp) { if (parking_) { // if (current_vel_b_.norm() < common::options::lo::parking_speed) { // LOG(INFO) << "car is in static mode"; static_count_++; if (static_count_ >= lo::parking_count) { static_count_ = 0; return false; } return true; } else { static_count_ = 0; return false; } } void LidarLoc::UpdateState(const CloudPtr& input) { last_timestamp_ = current_timestamp_; } void LidarLoc::SetInitRltState() { UL lock(result_mutex_); localization_result_.confidence_ = 0.0; localization_result_.timestamp_ = current_timestamp_; localization_result_.lidar_loc_valid_ = false; localization_result_.status_ = LocalizationStatus::INITIALIZING; } bool LidarLoc::LocInited() { // UL lock( data_mutex_); return loc_inited_; } bool LidarLoc::AssignLOPose(double timestamp) { UL lock(lo_pose_mutex_); SE3 interp_pose; NavState best_match; // 无法拿到最新的，插值结果都是外推出来的，和真实有时偏差会很大（╯﹏╰） // if (!lo_pose_queue_.empty()) { // LOG(INFO) << "lo interp: " << timestamp << " " << lo_pose_queue_.back().timestamp_ << " " // << lo_pose_queue_.back().pose_.translation().transpose(); // } bool pose_interp_success = math::PoseInterp( timestamp, lo_pose_queue_, [](const NavState& dr) { return dr.timestamp_; }, [](const NavState& dr) { return dr.GetPose(); }, interp_pose, best_match, 5.0); if (pose_interp_success) { current_lo_pose_ = interp_pose; current_lo_pose_set_ = true; current_vel_b_ = best_match.GetRot().inverse() * best_match.GetVel(); current_vel_ = best_match.GetVel(); // if (options_.with_height_) { // current_lo_pose_.translation()[2] = map_height_; // } return true; } else { current_lo_pose_set_ = false; return false; } } bool LidarLoc::AssignDRPose(double timestamp) { UL lock(dr_pose_mutex_); SE3 interp_pose; NavState best_match; bool pose_interp_success = math::PoseInterp( timestamp, dr_pose_queue_, [](const NavState& dr) { return dr.timestamp_; }, [](const NavState& dr) { return dr.GetPose(); }, interp_pose, best_match, 5.0); if (pose_interp_success) { parking_ = best_match.is_parking_; current_dr_pose_ = interp_pose; current_dr_pose_set_ = true; // if (options_.with_height_) { // current_dr_pose_.translation()[2] = map_height_; // } return true; } else { parking_ = false; current_dr_pose_set_ = false; return false; } } void LidarLoc::Finish() { if (map_) { LOG(INFO) << "saving maps"; update_map_quit_ = true; update_map_thread_.join(); /// 永久保存时，再存储地图 if (options_.map_option_.policy_ == TiledMap::DynamicCloudPolicy::PERSISTENT && options_.map_option_.save_dyn_when_quit_ && !has_set_pose_) { map_->SaveToBin(true); LOG(INFO) << "dynamic maps saved"; } } } } // namespace lightning::loc ================================================ FILE: src/core/localization/lidar_loc/lidar_loc.h ================================================ #pragma once #include #include #include #include #include #include #include "common/nav_state.h" #include "common/timed_pose.h" #include "core/localization/localization_result.h" #include "core/maps/tiled_map.h" #include "pclomp/ndt_omp_impl.hpp" namespace lightning::ui { class PangolinWindow; } namespace lightning::loc { /// 激光定位对外接口类 class LidarLoc { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using UL = std::unique_lock; /// 定位方法 enum class LocMethod { NDT_OMP, // OMP版本NDT }; struct Options { Options() {} bool display_realtime_cloud_ = false; // 是否显示实时点云 bool debug_ = false; // 是否使用单测模式 LocMethod match_method_ = LocMethod::NDT_OMP; // 匹配方式 bool try_self_extrap_ = false; // 是否尝试自己的外推pose bool with_height_ = true; // 建图期间是否带有高度约束？ bool force_2d_ = true; // 强制在2D空间 float min_init_confidence_ = 0.1; // 初始化时要求的最小分值 bool init_with_fp_ = true; // 是否使用功能点进行初始化 bool enable_parking_static_ = false; // 是否在静止时输出固定位置 bool enable_icp_adjust_ = false; // 是否使用icp调整ndt匹配结果提高定位精度 /// 点云过滤 // float filter_z_min_ = -1.0; float filter_z_max_ = 30.0; // float filter_intensity_min_ = 10.0; // float filter_intensity_max_ = 100.0; /// 地图配置 TiledMap::Options map_option_ = TiledMap::Options(); // 动态图层更新相关 bool update_dynamic_cloud_ = true; // 是否使用定位后的点云来更新动态图层 double update_kf_dis_ = 5.0; // 每隔多少米更新一次 double update_kf_time_ = 10.0; // 每隔多少时间更新一次 double update_lidar_loc_score_ = 2.2; // 更新时激光定位匹配分值阈值 double lidar_loc_odom_th_ = 0.3; // 激光两帧匹配结果与对应lidarodom结果差的阈值，超过则认为lidarodom异常 double max_update_cache_dis_ = 30.0; // 更新动态图层的缓冲距离 std::string recover_pose_path_ = "./data/recover_pose.txt"; }; explicit LidarLoc(Options options = Options()); virtual ~LidarLoc(); /// 初始化 bool Init(const std::string& config_path); /// 处理 lidar odom 消息 bool ProcessLO(const NavState& state); /// 处理拼接后点云 bool ProcessCloud(CloudPtr cloud_input); /// 处理DR状态 bool ProcessDR(const NavState& state); /// 获取位姿 NavState GetState(); /// 设置当前定位状态标志位 void SetInitRltState(); /** * 尝试在另一个位置进行定位 * @param input * @param pose * @return */ bool TryOtherSolution(CloudPtr input, SE3& pose); /// 使用功能点初始化 bool InitWithFP(CloudPtr input, const SE3& fp_pose); /// 更新全局地图 bool UpdateGlobalMap(); /// @brief 定位是否已经成功初始化 bool LocInited(); /// @brief 激光定位重置接口 void ResetLastPose(const SE3& last_pose); /** * @brief 激光地图匹配 * @param pose 预测位姿 * @param confidence 得分 * @param input 输入点云 * @param output 输出点云 * @param use_rough_res 是否使用粗分辨率 * @return */ bool Localize(SE3& pose, double& confidence, CloudPtr input, CloudPtr output, bool use_rough_res = false); /// 设置UI void SetUI(std::shared_ptr ui) { ui_ = ui; } /// 设置init pose void SetInitialPose(SE3 init_pose); /// 获取定位结果 LocalizationResult GetLocalizationResult() { UL lock(result_mutex_); return localization_result_; } void Finish(); /// 激光定位是否认为LO有效 bool LidarLocThinkLOReliable() { return lo_reliable_; } private: // 内部函数 ========================================================================== /** * 对点云进行配准 * @param input */ void Align(const CloudPtr& input); /** * 寻找当前帧对应的LO相对位姿 * @param timestamp * @return */ bool AssignLOPose(double timestamp); /** * 寻找当前帧对应的DR相对位姿 * @param timestamp * @return */ bool AssignDRPose(double timestamp); /** * 检查车辆是否静止 * @param timestamp * @return */ bool CheckStatic(double timestamp); /** * 更新自身状态 * @param input */ void UpdateState(const CloudPtr& input); /** * 更新地图 */ void UpdateMapThread(); /** * 使用网格搜索best yaw */ bool YawSearch(SE3& pose, double& confidence, CloudPtr input, CloudPtr output); bool CheckLidarOdomValid(const SE3& current_pose_esti, double& delta_posi); // 成员变量 ========================================================================== Options options_; std::mutex match_mutex_; // 锁定pcl_ndt指针 using NDTType = pclomp::NormalDistributionsTransform; NDTType::Ptr pcl_ndt_ = nullptr; NDTType::Ptr pcl_ndt_rough_ = nullptr; // 粗分辨率 using ICPType = pcl::IterativeClosestPoint; ICPType::Ptr pcl_icp_ = nullptr; CloudPtr current_scan_ = nullptr; // 当前扫描 std::shared_ptr ui_ = nullptr; // ui SE3 last_loc_pose_; std::mutex initial_pose_mutex_; // 初始定位锁 bool initial_pose_set_ = false; // 定位是否被手动设置 SE3 initial_pose_; // 手动设置的初始位姿 bool loc_inited_ = false; // 定位是否初始化成功 double current_timestamp_ = 0; // 本次输入的时间戳 double last_timestamp_ = 0; // 上次输入的时间戳 bool last_lo_pose_set_ = false; bool current_lo_pose_set_ = false; SE3 last_lo_pose_; // 上一次相对位置，相对位置来自LO SE3 current_lo_pose_; // 本次的LO相对位置 bool last_dr_pose_set_ = false; bool current_dr_pose_set_ = false; SE3 last_dr_pose_; // 上一次相对位置，相对位置来自DR SE3 current_dr_pose_; // 本次的DR相对位置 bool parking_ = false; double try_other_guess_trans_th_ = 0.3; // 在初始估计相差多少时，尝试其他的解 double try_other_guess_rot_th_ = 0.5 * M_PI / 180.0; // 在初始估计相差多少时，尝试其他的解 // double low_vel_th_ = 1.0; // 低速阈值m/s // double update_cache_dis_ = 0; // 动态图层的更新缓冲距离 std::deque ave_scores_; // 近期匹配的分值情况 Vec3d current_vel_b_ = Vec3d::Zero(); // 本次的车体系下的速度 Vec3d current_vel_ = Vec3d::Zero(); // 本次dr的速度 std::deque lidar_loc_pose_queue_; // lidar odom pose 轨迹 bool lo_reliable_ = true; int lo_reliable_cnt_ = 0; SE3 last_abs_pose_; // 上一次绝对定位，绝对定位来自于地图匹配得到的位姿 TimedPose last_dyn_upd_pose_; // 上次更新动态图层时使用的位姿 SE3 current_abs_pose_; // 本次的绝对定位 bool last_abs_pose_set_ = false; double current_score_ = 1e5; /// 设一个大分值，若定位一开始就匹配失败，则可以直接用GPS重置 int match_fail_count_ = 0; int static_count_ = 0; int rtk_reset_cnt_ = 0; // RTK重置计数 std::mutex result_mutex_; LocalizationResult localization_result_; // 输出结果 // 相对运动观测队列 std::mutex lo_pose_mutex_; std::deque lo_pose_queue_; std::mutex dr_pose_mutex_; std::deque dr_pose_queue_; /// 功能点初始化的记录 std::vector fp_init_fail_pose_vec_; double fp_last_tried_time_ = 0; bool update_map_quit_ = false; std::thread update_map_thread_; // 地图更新 std::shared_ptr map_ = nullptr; // 地图 double map_height_ = 0; bool has_set_pose_ = false; // 外部set_pose标志位，若存在则本次动态图层不落盘 std::ofstream recover_pose_out_; }; } // namespace lightning::loc ================================================ FILE: src/core/localization/lidar_loc/pclomp/ndt_omp.cpp ================================================ #include "ndt_omp.h" #include "ndt_omp_impl.hpp" template class pclomp::NormalDistributionsTransform; template class pclomp::NormalDistributionsTransform; ================================================ FILE: src/core/localization/lidar_loc/pclomp/ndt_omp.h ================================================ /* * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2012, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * 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 OWNER 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. * * $Id$ * */ #ifndef PCL_REGISTRATION_NDT_OMP_H_ #define PCL_REGISTRATION_NDT_OMP_H_ #include #include #include "voxel_grid_covariance_omp.h" namespace pclomp { enum NeighborSearchMethod { DIRECT26, DIRECT7, DIRECT1 }; /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. * \note For more information please see * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. * PhD thesis, Orebro University. Orebro Studies in Technology 36., * More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease * In ACM Transactions on Mathematical Software. and * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100 * \note Math refactored by Todor Stoyanov. * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) */ template class NormalDistributionsTransform : public pcl::Registration { protected: typedef typename pcl::Registration::PointCloudSource PointCloudSource; typedef typename PointCloudSource::Ptr PointCloudSourcePtr; typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; typedef typename pcl::Registration::PointCloudTarget PointCloudTarget; typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; typedef pcl::PointIndices::Ptr PointIndicesPtr; typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr; /** \brief Typename of searchable voxel grid containing mean and covariance. */ typedef pclomp::VoxelGridCovariance TargetGrid; /** \brief Typename of pointer to searchable voxel grid. */ typedef TargetGrid *TargetGridPtr; /** \brief Typename of const pointer to searchable voxel grid. */ typedef const TargetGrid *TargetGridConstPtr; /** \brief Typename of const pointer to searchable voxel grid leaf. */ typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr; public: typedef std::shared_ptr > Ptr; typedef std::shared_ptr > ConstPtr; /** \brief Constructor. * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0 */ NormalDistributionsTransform(); /** \brief Empty destructor */ virtual ~NormalDistributionsTransform() {} void setNumThreads(int n) { num_threads_ = n; } /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to). * \param[in] cloud the input point cloud target */ inline void setInputTarget(const PointCloudTargetConstPtr &cloud) { // pcl::Registration::setInputTarget(cloud); // init(); } /** \brief Set/change the voxel grid resolution. * \param[in] resolution side length of voxels */ inline void setResolution(float resolution) { // Prevents unnessary voxel initiations if (resolution_ != resolution) { resolution_ = resolution; target_cells_.setLeafSize(resolution_, resolution_, resolution_); // if (input_) { // init(); // } } } /** \brief Get voxel grid resolution. * \return side length of voxels */ inline float getResolution() const { return (resolution_); } /** \brief Get the newton line search maximum step length. * \return maximum step length */ inline double getStepSize() const { return (step_size_); } /** \brief Set/change the newton line search maximum step length. * \param[in] step_size maximum step length */ inline void setStepSize(double step_size) { step_size_ = step_size; } /** \brief Get the point cloud outlier ratio. * \return outlier ratio */ inline double getOulierRatio() const { return (outlier_ratio_); } /** \brief Set/change the point cloud outlier ratio. * \param[in] outlier_ratio outlier ratio */ inline void setOulierRatio(double outlier_ratio) { outlier_ratio_ = outlier_ratio; } inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) { search_method = method; } /** \brief Get the registration alignment probability. * \return transformation probability */ inline double getTransformationProbability() const { return (trans_probability_); } /** \brief Get the number of iterations required to calculate alignment. * \return final number of iterations */ inline int getFinalNumIteration() const { return (nr_iterations_); } inline TargetGrid &getTargetGrid() { return (target_cells_); } /** \brief Convert 6 element transformation vector to affine transformation. * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] * \param[out] trans affine transform corresponding to given transfomation vector */ static void convertTransform(const Eigen::Matrix &x, Eigen::Affine3f &trans) { trans = Eigen::Translation(float(x(0)), float(x(1)), float(x(2))) * Eigen::AngleAxis(float(x(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis(float(x(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis(float(x(5)), Eigen::Vector3f::UnitZ()); } /** \brief Convert 6 element transformation vector to transformation matrix. * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector */ static void convertTransform(const Eigen::Matrix &x, Eigen::Matrix4f &trans) { Eigen::Affine3f _affine; convertTransform(x, _affine); trans = _affine.matrix(); } // negative log likelihood function // lower is better double calculateScore(const PointCloudSource &cloud) const; /// added by xiang /** * 增加一个目标点云 * @param target */ void AddTarget(PointCloudTargetPtr target) { target_cells_.AddTarget(target); target_ = target; this->target_cloud_updated_ = true; } bool initCompute() { if (!this->target_) { return false; } // Update the correspondence estimation // if (this->correspondence_estimation_) { // this->correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_); // this->correspondence_estimation_->setSearchMethodSource(tree_reciprocal_, // force_no_recompute_reciprocal_); // } // Check if input was set if (!input_) { return false; } // If no point indices have been given, construct a set of indices for the entire input point cloud if (!indices_) { this->fake_indices_ = true; indices_.reset(new pcl::Indices); } // If we have a set of fake indices, but they do not match the number of points in the cloud, update them if (this->fake_indices_ && indices_->size() != input_->size()) { const auto indices_size = indices_->size(); try { indices_->resize(input_->size()); } catch (const std::bad_alloc &) { PCL_ERROR("[initCompute] Failed to allocate %lu indices.\n", input_->size()); } for (auto i = indices_size; i < indices_->size(); ++i) { (*indices_)[i] = static_cast(i); } } return true; } /** * 计算所有目标云的区块 */ void ComputeTargetGrids() { target_cells_.ComputeTargetGrids(); } protected: using pcl::Registration::reg_name_; using pcl::Registration::getClassName; using pcl::Registration::input_; using pcl::Registration::indices_; using pcl::Registration::target_; using pcl::Registration::nr_iterations_; using pcl::Registration::max_iterations_; using pcl::Registration::previous_transformation_; using pcl::Registration::final_transformation_; using pcl::Registration::transformation_; using pcl::Registration::transformation_epsilon_; using pcl::Registration::converged_; using pcl::Registration::corr_dist_threshold_; using pcl::Registration::inlier_threshold_; using pcl::Registration::update_visualizer_; /** \brief Estimate the transformation and returns the transformed source (input) as output. * \param[out] output the resultant input transfomed point cloud dataset */ virtual void computeTransformation(PointCloudSource &output) { computeTransformation(output, Eigen::Matrix4f::Identity()); } /** \brief Estimate the transformation and returns the transformed source (input) as output. * \param[out] output the resultant input transfomed point cloud dataset * \param[in] guess the initial gross estimation of the transformation */ virtual void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess); /** \brief Initiate covariance voxel structure. */ void inline init() { // target_cells_.setLeafSize(resolution_, resolution_, resolution_); // target_cells_.setInputCloud(target_); // Initiate voxel structure. // target_cells_.filter(true); } /** \brief Compute derivatives of probability function w.r.t. the transformation vector. * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector * \param[in] trans_cloud transformed point cloud * \param[in] p the current transform vector * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. */ double computeDerivatives(Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, PointCloudSource &trans_cloud, Eigen::Matrix &p, bool compute_hessian = true); /** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation * vector. \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. \param[in,out] score_gradient the gradient vector of * the probability function w.r.t. the transformation vector \param[in,out] hessian the hessian matrix of the * probability function w.r.t. the transformation vector \param[in] x_trans transformed point minus mean of occupied * covariance voxel \param[in] c_inv covariance of occupied covariance voxel \param[in] compute_hessian flag to * calculate hessian, unnessissary for step calculation. */ double updateDerivatives(Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, const Eigen::Matrix &point_gradient_, const Eigen::Matrix &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian = true) const; /** \brief Precompute anglular components of derivatives. * \note Equation 6.19 and 6.21 [Magnusson 2009]. * \param[in] p the current transform vector * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. */ void computeAngleDerivatives(Eigen::Matrix &p, bool compute_hessian = true); /** \brief Compute point derivatives. * \note Equation 6.18-21 [Magnusson 2009]. * \param[in] x point from the input cloud * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. */ void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix &point_gradient_, Eigen::Matrix &point_hessian_, bool compute_hessian = true) const; void computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix &point_gradient_, Eigen::Matrix &point_hessian_, bool compute_hessian = true) const; /** \brief Compute hessian of probability function w.r.t. the transformation vector. * \note Equation 6.13 [Magnusson 2009]. * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector * \param[in] trans_cloud transformed point cloud * \param[in] p the current transform vector */ void computeHessian(Eigen::Matrix &hessian, PointCloudSource &trans_cloud, Eigen::Matrix &p); /** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation * vector. \note Equation 6.13 [Magnusson 2009]. \param[in,out] hessian the hessian matrix of the probability * function w.r.t. the transformation vector \param[in] x_trans transformed point minus mean of occupied covariance * voxel \param[in] c_inv covariance of occupied covariance voxel */ void updateHessian(Eigen::Matrix &hessian, const Eigen::Matrix &point_gradient_, const Eigen::Matrix &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const; /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente * method. \note Search Algorithm [More, Thuente 1994] \param[in] x initial transformation vector, \f$ x \f$ in * Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009] \param[in] step_dir * descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in * Algorithm 2 [Magnusson 2009] \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente * (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009] \param[in] step_max maximum step * length, \f$ \alpha_max \f$ in Moore-Thuente (1994) \param[in] step_min minimum step length, \f$ \alpha_min \f$ in * Moore-Thuente (1994) \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 * (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009] \param[in,out] score_gradient gradient of * score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ * in Algorithm 2 [Magnusson 2009] \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ * f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009] \param[in,out] * trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 * [Magnusson 2009] \return final step length */ double computeStepLengthMT(const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, PointCloudSource &trans_cloud); /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994) * \note Updating Algorithm until some value satifies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 * \f$ and Modified Updating Algorithm from then on [More, Thuente 1994]. \param[in,out] a_l first endpoint of * interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) \param[in,out] f_l value at first endpoint, \f$ f_l * \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified * Update Algorithm \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ * \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm \param[in,out] * a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) \param[in,out] f_u value at * second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ * \phi(\alpha_u) \f$ for Modified Update Algorithm \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in * Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified * Update Algorithm \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) \param[in] f_t value at * trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ * \phi(\alpha_t) \f$ for Modified Update Algorithm \param[in] g_t derivative at trial value, \f$ g_t \f$ in * Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified * Update Algorithm \return if interval converges */ bool updateIntervalMT(double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t); /** \brief Select new trial value for More-Thuente method. * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$ * until some value satifies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ * then \f$ \phi(\alpha_k) \f$ is used from then on. * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, * Ya-xiang Yuan (89-100). \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente * (1994) \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994) \param[in] g_l derivative at * first endpoint, \f$ g_l \f$ in Moore-Thuente (1994) \param[in] a_u second endpoint of interval \f$ I \f$, \f$ * \alpha_u \f$ in Moore-Thuente (1994) \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994) * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994) * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994) * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994) * \return new trial value */ double trialValueSelectionMT(double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, double g_t); /** \brief Auxilary function used to determin endpoints of More-Thuente interval. * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994) * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994) * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994) * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994) * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] * \return sufficent decrease value */ inline double auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4) { return (f_a - f_0 - mu * g_0 * a); } /** \brief Auxilary function derivative used to determin endpoints of More-Thuente interval. * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994) * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994) * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] * \return sufficent decrease derivative */ inline double auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) { return (g_a - mu * g_0); } /** \brief The voxel grid generated from target cloud containing point means and covariances. */ TargetGrid target_cells_; // double fitness_epsilon_; /** \brief The side length of voxels. */ float resolution_; /** \brief The maximum step length. */ double step_size_; /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */ double outlier_ratio_; /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 * [Magnusson 2009]. */ double gauss_d1_, gauss_d2_, gauss_d3_; /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson * 2009]. */ double trans_probability_; /** \brief Precomputed Angular Gradient * * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. */ Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_; Eigen::Matrix j_ang; /** \brief Precomputed Angular Hessian * * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009]. */ Eigen::Vector3d h_ang_a2_, h_ang_a3_, h_ang_b2_, h_ang_b3_, h_ang_c2_, h_ang_c3_, h_ang_d1_, h_ang_d2_, h_ang_d3_, h_ang_e1_, h_ang_e2_, h_ang_e3_, h_ang_f1_, h_ang_f2_, h_ang_f3_; Eigen::Matrix h_ang; /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in * Equation 6.18 [Magnusson 2009]. */ // Eigen::Matrix point_gradient_; /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in * Equation 6.20 [Magnusson 2009]. */ // Eigen::Matrix point_hessian_; int num_threads_; bool mask_xyz_; bool mask_xy_; bool mask_rp_; bool mask_rpy_; bool mask_yaw_; bool mask_z_; bool no_guarnteed_decrease_; public: inline void resetMask() { mask_xyz_ = false; mask_xy_ = false; mask_rp_ = false; mask_rpy_ = false; mask_yaw_ = false; mask_z_ = false; } inline void setMaskXYZ(bool new_value) { mask_xyz_ = new_value; } inline void setMaskXY(bool new_value) { mask_xy_ = new_value; } inline void setMaskRP(bool new_value) { mask_rp_ = new_value; } inline void setMaskYaw(bool new_value) { mask_yaw_ = new_value; } inline void setMaskRPY(bool new_value) { mask_rpy_ = new_value; } inline void setMaskZ(bool new_value) { mask_z_ = new_value; } inline void setNoGuarnteedDecrease(bool new_value) { no_guarnteed_decrease_ = new_value; } Eigen::Matrix hessian_matrix_; Eigen::Matrix getHessionMatrix() { return hessian_matrix_; } public: NeighborSearchMethod search_method; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pclomp #endif // PCL_REGISTRATION_NDT_H_ ================================================ FILE: src/core/localization/lidar_loc/pclomp/ndt_omp_impl.hpp ================================================ #include "ndt_omp.h" /* * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * 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 OWNER 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. * * $Id$ * */ #ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_ #define PCL_REGISTRATION_NDT_OMP_IMPL_H_ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template pclomp::NormalDistributionsTransform::NormalDistributionsTransform() : target_cells_(), resolution_(1.0f), step_size_(0.1), outlier_ratio_(0.55), gauss_d1_(), gauss_d2_(), gauss_d3_(), trans_probability_(), j_ang_a_(), j_ang_b_(), j_ang_c_(), j_ang_d_(), j_ang_e_(), j_ang_f_(), j_ang_g_(), j_ang_h_(), h_ang_a2_(), h_ang_a3_(), h_ang_b2_(), h_ang_b3_(), h_ang_c2_(), h_ang_c3_(), h_ang_d1_(), h_ang_d2_(), h_ang_d3_(), h_ang_e1_(), h_ang_e2_(), h_ang_e3_(), h_ang_f1_(), h_ang_f2_(), h_ang_f3_() { this->reg_name_ = "NormalDistributionsTransform"; mask_xyz_ = false; mask_xy_ = false; mask_rp_ = false; mask_rpy_ = false; mask_yaw_ = false; mask_z_ = false; no_guarnteed_decrease_ = false; double gauss_c1, gauss_c2; // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10.0 * (1 - outlier_ratio_); gauss_c2 = outlier_ratio_ / pow(resolution_, 3); gauss_d3_ = -log(gauss_c2); gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3_; gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); transformation_epsilon_ = 0.1; max_iterations_ = 35; search_method = DIRECT7; num_threads_ = omp_get_max_threads(); this->force_no_recompute_ = true; target_cells_.setLeafSize(resolution_, resolution_, resolution_); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pclomp::NormalDistributionsTransform::computeTransformation( PointCloudSource &output, const Eigen::Matrix4f &guess) { nr_iterations_ = 0; converged_ = false; double gauss_c1, gauss_c2; // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] gauss_c1 = 10 * (1 - outlier_ratio_); gauss_c2 = outlier_ratio_ / pow(resolution_, 3); gauss_d3_ = -log(gauss_c2); gauss_d1_ = -log(gauss_c1 + gauss_c2) - gauss_d3_; gauss_d2_ = -2 * log((-log(gauss_c1 * exp(-0.5) + gauss_c2) - gauss_d3_) / gauss_d1_); if (guess != Eigen::Matrix4f::Identity()) { // Initialise final transformation to the guessed one final_transformation_ = guess; // Apply guessed transformation prior to search for neighbours transformPointCloud(output, output, guess); } Eigen::Transform eig_transformation; eig_transformation.matrix() = final_transformation_; // Convert initial guess matrix to 6 element transformation vector Eigen::Matrix p, delta_p, score_gradient; Eigen::Vector3f init_translation = eig_transformation.translation(); Eigen::Vector3f init_rotation = eig_transformation.rotation().eulerAngles(0, 1, 2); p << init_translation(0), init_translation(1), init_translation(2), init_rotation(0), init_rotation(1), init_rotation(2); Eigen::Matrix hessian; double score = 0; double delta_p_norm; // std::cout<<"=======================================\n"; // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length // determination. score = computeDerivatives(score_gradient, hessian, output, p); while (!converged_) { // Store previous transformation previous_transformation_ = transformation_; if (mask_rp_) { hessian.block<2, 6>(3, 0) *= 0.; hessian.block<6, 2>(0, 3) *= 0.; score_gradient.block<2, 1>(3, 0) *= 0.; } if (mask_xyz_) { hessian.block<3, 6>(0, 0) *= 0.; hessian.block<6, 3>(0, 0) *= 0.; score_gradient.block<3, 1>(0, 0) *= 0.; } if (mask_xy_) { hessian.block<2, 6>(0, 0) *= 0.; hessian.block<6, 2>(0, 0) *= 0.; score_gradient.block<2, 1>(0, 0) *= 0.; } if (mask_yaw_) { hessian.block<1, 6>(5, 0) *= 0.; hessian.block<6, 1>(0, 5) *= 0.; score_gradient.block<1, 1>(5, 0) *= 0.; } if (mask_z_) { hessian.block<1, 6>(2, 0) *= 0.; hessian.block<6, 1>(0, 2) *= 0.; score_gradient.block<1, 1>(2, 0) *= 0.; } if (mask_rpy_) { hessian.block<3, 6>(3, 0) *= 0.; hessian.block<6, 3>(0, 3) *= 0.; score_gradient.block<3, 1>(3, 0) *= 0.; } // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] Eigen::JacobiSVD> sv(hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); // Negative for maximization as opposed to minimization delta_p = sv.solve(-score_gradient); // std::cout<<"score_gradient: \n" << score_gradient.transpose() <<"\n"; // std::cout<<"delta_p: \n" << delta_p.transpose() <<"\n"; // Calculate step length with guarnteed sufficient decrease [More, Thuente 1994] delta_p_norm = delta_p.norm(); // std::cout << "step_size_ = " << step_size_ << ", delta_p_norm = " << delta_p_norm << ", score = " << score << // std::endl; std::cout << "x = " << delta_p(0) // << " , y = " << delta_p(1) // << " , z = " << delta_p(2) // << " , roll = " << delta_p(3) // << " , pitch = " << delta_p(4) // << " , yaw = " << delta_p(5) << std::endl; if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) { trans_probability_ = score / static_cast(input_->points.size()); converged_ = delta_p_norm == delta_p_norm; return; } delta_p.normalize(); delta_p_norm = computeStepLengthMT(p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output); delta_p *= delta_p_norm; transformation_ = (Eigen::Translation(static_cast(delta_p(0)), static_cast(delta_p(1)), static_cast(delta_p(2))) * Eigen::AngleAxis(static_cast(delta_p(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis(static_cast(delta_p(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis(static_cast(delta_p(5)), Eigen::Vector3f::UnitZ())) .matrix(); p = p + delta_p; // Update Visualizer (untested) if (update_visualizer_ != 0) { update_visualizer_(output, std::vector(), *target_, std::vector()); } if (nr_iterations_ > max_iterations_ || (nr_iterations_ && (std::fabs(delta_p_norm) < transformation_epsilon_))) { converged_ = true; } nr_iterations_++; } hessian_matrix_ = hessian; // Store transformation probability. The realtive differences within each scan registration are accurate // but the normalization constants need to be modified for it to be globally accurate trans_probability_ = score / static_cast(input_->points.size()); } #ifndef _OPENMP int omp_get_max_threads() { return 1; } int omp_get_thread_num() { return 0; } #endif ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template double pclomp::NormalDistributionsTransform::computeDerivatives( Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, PointCloudSource &trans_cloud, Eigen::Matrix &p, bool compute_hessian) { score_gradient.setZero(); hessian.setZero(); double score = 0; std::vector scores(num_threads_); std::vector, Eigen::aligned_allocator>> score_gradients( num_threads_); std::vector, Eigen::aligned_allocator>> hessians( num_threads_); for (int i = 0; i < num_threads_; i++) { scores[i] = 0; score_gradients[i].setZero(); hessians[i].setZero(); } // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009] computeAngleDerivatives(p); std::vector> neighborhoods(num_threads_); std::vector> distancess(num_threads_); // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) for (int idx = 0; idx < input_->points.size(); idx++) { int thread_n = omp_get_thread_num(); // Original Point and Transformed Point PointSource x_pt, x_trans_pt; // Original Point and Transformed Point (for math) Eigen::Vector3d x, x_trans; // Occupied Voxel TargetGridLeafConstPtr cell; // Inverse Covariance of Occupied Voxel Eigen::Matrix3d c_inv; // Initialize Point Gradient and Hessian Eigen::Matrix point_gradient_; Eigen::Matrix point_hessian_; point_gradient_.setZero(); point_gradient_.block<3, 3>(0, 0).setIdentity(); point_hessian_.setZero(); x_trans_pt = trans_cloud.points[idx]; auto &neighborhood = neighborhoods[thread_n]; auto &distances = distancess[thread_n]; // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. switch (search_method) { // case KDTREE: // target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); // break; case DIRECT26: target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood); break; default: case DIRECT7: target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood); break; case DIRECT1: target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood); break; } double score_pt = 0; Eigen::Matrix score_gradient_pt = Eigen::Matrix::Zero(); Eigen::Matrix hessian_pt = Eigen::Matrix::Zero(); for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++) { cell = *neighborhood_it; x_pt = input_->points[idx]; x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z); x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] x_trans -= cell->getMean(); // Uses precomputed covariance for speed. c_inv = cell->getInverseCov(); // c_inv = Eigen::Matrix3d::Identity(); //anjiss cov // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 // [Magnusson 2009] computePointDerivatives(x, point_gradient_, point_hessian_); // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 // and 6.13, respectively [Magnusson 2009] score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian); } scores[thread_n] += score_pt; score_gradients[thread_n].noalias() += score_gradient_pt; hessians[thread_n].noalias() += hessian_pt; } for (int i = 0; i < num_threads_; i++) { score += scores[i]; score_gradient += score_gradients[i]; hessian += hessians[i]; } return (score); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pclomp::NormalDistributionsTransform::computeAngleDerivatives( Eigen::Matrix &p, bool compute_hessian) { // Simplified math for near 0 angles double cx, cy, cz, sx, sy, sz; if (fabs(p(3)) < 10e-5) { // p(3) = 0; cx = 1.0; sx = 0.0; } else { cx = cos(p(3)); sx = sin(p(3)); } if (fabs(p(4)) < 10e-5) { // p(4) = 0; cy = 1.0; sy = 0.0; } else { cy = cos(p(4)); sy = sin(p(4)); } if (fabs(p(5)) < 10e-5) { // p(5) = 0; cz = 1.0; sz = 0.0; } else { cz = cos(p(5)); sz = sin(p(5)); } // Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009] j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy); j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy); j_ang_c_ << (-sy * cz), sy * sz, cy; j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy; j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy); j_ang_f_ << (-cy * sz), (-cy * cz), 0; j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0; j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0; j_ang.setZero(); j_ang.row(0).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f); j_ang.row(1).noalias() = Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f); j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f); j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f); j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f); j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f); j_ang.row(6).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f); j_ang.row(7).noalias() = Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f); if (compute_hessian) { // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row // index [Magnusson 2009] h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy; h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy); h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy); h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy); h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0; h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0; h_ang_d1_ << (-cy * cz), (cy * sz), (sy); h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy); h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy); h_ang_e1_ << (sy * sz), (sy * cz), 0; h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0; h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0; h_ang_f1_ << (-cy * cz), (cy * sz), 0; h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0; h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0; h_ang.setZero(); h_ang.row(0).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f); // a2 h_ang.row(1).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f); // a3 h_ang.row(2).noalias() = Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f); // b2 h_ang.row(3).noalias() = Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f); // b3 h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f); // c2 h_ang.row(5).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f); // c3 h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f); // d1 h_ang.row(7).noalias() = Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f); // d2 h_ang.row(8).noalias() = Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f); // d3 h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f); // e1 h_ang.row(10).noalias() = Eigen::Vector4f((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f); // e2 h_ang.row(11).noalias() = Eigen::Vector4f((cx * cy * sz), (cx * cy * cz), 0, 0.0f); // e3 h_ang.row(12).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), 0, 0.0f); // f1 h_ang.row(13).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f); // f2 h_ang.row(14).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f); // f3 } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pclomp::NormalDistributionsTransform::computePointDerivatives( Eigen::Vector3d &x, Eigen::Matrix &point_gradient_, Eigen::Matrix &point_hessian_, bool compute_hessian) const { Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f); // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson // 2009] Eigen::Matrix x_j_ang = j_ang * x4; point_gradient_(1, 3) = x_j_ang[0]; point_gradient_(2, 3) = x_j_ang[1]; point_gradient_(0, 4) = x_j_ang[2]; point_gradient_(1, 4) = x_j_ang[3]; point_gradient_(2, 4) = x_j_ang[4]; point_gradient_(0, 5) = x_j_ang[5]; point_gradient_(1, 5) = x_j_ang[6]; point_gradient_(2, 5) = x_j_ang[7]; if (compute_hessian) { Eigen::Matrix x_h_ang = h_ang * x4; // Vectors from Equation 6.21 [Magnusson 2009] Eigen::Vector4f a(0, x_h_ang[0], x_h_ang[1], 0.0f); Eigen::Vector4f b(0, x_h_ang[2], x_h_ang[3], 0.0f); Eigen::Vector4f c(0, x_h_ang[4], x_h_ang[5], 0.0f); Eigen::Vector4f d(x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f); Eigen::Vector4f e(x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f); Eigen::Vector4f f(x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f); // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at // (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] point_hessian_.block<4, 1>((9 / 3) * 4, 3) = a; point_hessian_.block<4, 1>((12 / 3) * 4, 3) = b; point_hessian_.block<4, 1>((15 / 3) * 4, 3) = c; point_hessian_.block<4, 1>((9 / 3) * 4, 4) = b; point_hessian_.block<4, 1>((12 / 3) * 4, 4) = d; point_hessian_.block<4, 1>((15 / 3) * 4, 4) = e; point_hessian_.block<4, 1>((9 / 3) * 4, 5) = c; point_hessian_.block<4, 1>((12 / 3) * 4, 5) = e; point_hessian_.block<4, 1>((15 / 3) * 4, 5) = f; } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pclomp::NormalDistributionsTransform::computePointDerivatives( Eigen::Vector3d &x, Eigen::Matrix &point_gradient_, Eigen::Matrix &point_hessian_, bool compute_hessian) const { // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson // 2009] point_gradient_(1, 3) = x.dot(j_ang_a_); point_gradient_(2, 3) = x.dot(j_ang_b_); point_gradient_(0, 4) = x.dot(j_ang_c_); point_gradient_(1, 4) = x.dot(j_ang_d_); point_gradient_(2, 4) = x.dot(j_ang_e_); point_gradient_(0, 5) = x.dot(j_ang_f_); point_gradient_(1, 5) = x.dot(j_ang_g_); point_gradient_(2, 5) = x.dot(j_ang_h_); if (compute_hessian) { // Vectors from Equation 6.21 [Magnusson 2009] Eigen::Vector3d a, b, c, d, e, f; a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_); b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_); c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_); d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_); e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_); f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_); // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at // (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] point_hessian_.block<3, 1>(9, 3) = a; point_hessian_.block<3, 1>(12, 3) = b; point_hessian_.block<3, 1>(15, 3) = c; point_hessian_.block<3, 1>(9, 4) = b; point_hessian_.block<3, 1>(12, 4) = d; point_hessian_.block<3, 1>(15, 4) = e; point_hessian_.block<3, 1>(9, 5) = c; point_hessian_.block<3, 1>(12, 5) = e; point_hessian_.block<3, 1>(15, 5) = f; } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template double pclomp::NormalDistributionsTransform::updateDerivatives( Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, const Eigen::Matrix &point_gradient4, const Eigen::Matrix &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian) const { Eigen::Matrix x_trans4(x_trans[0], x_trans[1], x_trans[2], 0.0f); Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero(); c_inv4.topLeftCorner(3, 3) = c_inv.cast(); float gauss_d2 = gauss_d2_; // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] float e_x_cov_x = exp(-gauss_d2 * x_trans4.dot(x_trans4 * c_inv4) * 0.5f); // Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009] float score_inc = -gauss_d1_ * e_x_cov_x; e_x_cov_x = gauss_d2 * e_x_cov_x; // Error checking for invalid values. if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return (0); // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] e_x_cov_x *= gauss_d1_; Eigen::Matrix c_inv4_x_point_gradient4 = c_inv4 * point_gradient4; Eigen::Matrix x_trans4_dot_c_inv4_x_point_gradient4 = x_trans4 * c_inv4_x_point_gradient4; score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast(); if (compute_hessian) { Eigen::Matrix x_trans4_x_c_inv4 = x_trans4 * c_inv4; Eigen::Matrix point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = point_gradient4.transpose() * c_inv4_x_point_gradient4; Eigen::Matrix x_trans4_dot_c_inv4_x_ext_point_hessian_4ij; for (int i = 0; i < 6; i++) { // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] // Update gradient, Equation 6.12 [Magnusson 2009] x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = x_trans4_x_c_inv4 * point_hessian_.block<4, 6>(i * 4, 0); for (int j = 0; j < hessian.cols(); j++) { // Update hessian, Equation 6.13 [Magnusson 2009] hessian(i, j) += e_x_cov_x * (-gauss_d2 * x_trans4_dot_c_inv4_x_point_gradient4(i) * x_trans4_dot_c_inv4_x_point_gradient4(j) + x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) + point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i)); } } } return (score_inc); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // template void // pclomp::NormalDistributionsTransform::computeHessian (Eigen::Matrix &hessian, // PointCloudSource &trans_cloud, // Eigen::Matrix &) // { // // Original Point and Transformed Point // PointSource x_pt, x_trans_pt; // // Original Point and Transformed Point (for math) // Eigen::Vector3d x, x_trans; // // Occupied Voxel // TargetGridLeafConstPtr cell; // // Inverse Covariance of Occupied Voxel // Eigen::Matrix3d c_inv; // // // Initialize Point Gradient and Hessian // Eigen::Matrix point_gradient_; // Eigen::Matrix point_hessian_; // point_gradient_.setZero(); // point_gradient_.block<3, 3>(0, 0).setIdentity(); // point_hessian_.setZero(); // // hessian.setZero (); // // // Precompute Angular Derivatives unessisary because only used after regular derivative calculation // // // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] // for (size_t idx = 0; idx < input_->points.size (); idx++) // { // x_trans_pt = trans_cloud.points[idx]; // // // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. // std::vector neighborhood; // std::vector distances; // target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); // // for (typename std::vector::iterator neighborhood_it = neighborhood.begin (); // neighborhood_it != neighborhood.end (); neighborhood_it++) // { // cell = *neighborhood_it; // // { // x_pt = input_->points[idx]; // x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); // // x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); // // // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] // x_trans -= cell->getMean (); // // Uses precomputed covariance for speed. // c_inv = cell->getInverseCov (); // //c_inv = Eigen::Matrix3d::Identity(); //anjiss cov // // // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 // [Magnusson 2009] computePointDerivatives (x, point_gradient_, point_hessian_); // // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively // [Magnusson 2009] updateHessian (hessian, point_gradient_, point_hessian_, x_trans, c_inv); // } // } // } // } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pclomp::NormalDistributionsTransform::updateHessian( Eigen::Matrix &hessian, const Eigen::Matrix &point_gradient_, const Eigen::Matrix &point_hessian_, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const { Eigen::Vector3d cov_dxd_pi; // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] double e_x_cov_x = gauss_d2_ * exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); // Error checking for invalid values. if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) return; // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] e_x_cov_x *= gauss_d1_; for (int i = 0; i < 6; i++) { // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] cov_dxd_pi = c_inv * point_gradient_.col(i); for (int j = 0; j < hessian.cols(); j++) { // Update hessian, Equation 6.13 [Magnusson 2009] hessian(i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot(cov_dxd_pi) * x_trans.dot(c_inv * point_gradient_.col(j)) + x_trans.dot(c_inv * point_hessian_.block<3, 1>(3 * i, j)) + point_gradient_.col(j).dot(cov_dxd_pi)); } } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pclomp::NormalDistributionsTransform::updateIntervalMT( double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t) { // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] if (f_t > f_l) { a_u = a_t; f_u = f_t; g_u = g_t; return (false); } // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] else if (g_t * (a_l - a_t) > 0) { a_l = a_t; f_l = f_t; g_l = g_t; return (false); } // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] else if (g_t * (a_l - a_t) < 0) { a_u = a_l; f_u = f_l; g_u = g_l; a_l = a_t; f_l = f_t; g_l = g_t; return (false); } // Interval Converged else return (true); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template double pclomp::NormalDistributionsTransform::trialValueSelectionMT( double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, double g_t) { // Case 1 in Trial Value Selection [More, Thuente 1994] if (f_t > f_l) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; double w = std::sqrt(z * z - g_t * g_l); // Equation 2.4.56 [Sun, Yuan 2006] double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l // Equation 2.4.2 [Sun, Yuan 2006] double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); if (std::fabs(a_c - a_l) < std::fabs(a_q - a_l)) return (a_c); else return (0.5 * (a_q + a_c)); } // Case 2 in Trial Value Selection [More, Thuente 1994] else if (g_t * g_l < 0) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; double w = std::sqrt(z * z - g_t * g_l); // Equation 2.4.56 [Sun, Yuan 2006] double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; if (std::fabs(a_c - a_t) >= std::fabs(a_s - a_t)) return (a_c); else return (a_s); } // Case 3 in Trial Value Selection [More, Thuente 1994] else if (std::fabs(g_t) <= std::fabs(g_l)) { // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; double w = std::sqrt(z * z - g_t * g_l); double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); // Calculate the minimizer of the quadratic that interpolates g_l and g_t // Equation 2.4.5 [Sun, Yuan 2006] double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; double a_t_next; if (std::fabs(a_c - a_t) < std::fabs(a_s - a_t)) a_t_next = a_c; else a_t_next = a_s; if (a_t > a_l) return (std::min(a_t + 0.66 * (a_u - a_t), a_t_next)); else return (std::max(a_t + 0.66 * (a_u - a_t), a_t_next)); } // Case 4 in Trial Value Selection [More, Thuente 1994] else { // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t // Equation 2.4.52 [Sun, Yuan 2006] double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; double w = std::sqrt(z * z - g_t * g_u); // Equation 2.4.56 [Sun, Yuan 2006] return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template double pclomp::NormalDistributionsTransform::computeStepLengthMT( const Eigen::Matrix &x, Eigen::Matrix &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix &score_gradient, Eigen::Matrix &hessian, PointCloudSource &trans_cloud) { // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] double phi_0 = -score; // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] double d_phi_0 = -(score_gradient.dot(step_dir)); Eigen::Matrix x_t; if (d_phi_0 >= 0) { // Not a decent direction if (d_phi_0 == 0) return 0; else { // Reverse step direction and calculate optimal step. d_phi_0 *= -1; step_dir *= -1; } } // The Search Algorithm for T(mu) [More, Thuente 1994] int max_step_iterations = 10; int step_iterations = 0; // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994] double mu = 1.e-4; // Curvature condition constant, Equation 1.2 [More, Thuete 1994] double nu = 0.9; // Initial endpoints of Interval I, double a_l = 0, a_u = 0; // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994] double f_l = auxilaryFunction_PsiMT(a_l, phi_0, phi_0, d_phi_0, mu); double g_l = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu); double f_u = auxilaryFunction_PsiMT(a_u, phi_0, phi_0, d_phi_0, mu); double g_u = auxilaryFunction_dPsiMT(d_phi_0, d_phi_0, mu); // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max bool interval_converged = (step_max - step_min) > 0, open_interval = true; double a_t = step_init; a_t = std::min(a_t, step_max); a_t = std::max(a_t, step_min); x_t = x + step_dir * a_t; final_transformation_ = (Eigen::Translation(static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))) * Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ())) .matrix(); // New transformed point cloud transformPointCloud(*input_, trans_cloud, final_transformation_); // Updates score, gradient and hessian. Hessian calculation is unessisary but testing showed that most step // calculations use the initial step suggestion and recalculation the reusable portions of the hessian would intail // more computation time. score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, true); // Calculate phi(alpha_t) double phi_t = -score; // Calculate phi'(alpha_t) double d_phi_t = -(score_gradient.dot(step_dir)); // Calculate psi(alpha_t) double psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu); // Calculate psi'(alpha_t) double d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu); // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, // Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) { // Use auxilary function if interval I is not closed if (open_interval) { a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); } else { a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); } a_t = std::min(a_t, step_max); a_t = std::max(a_t, step_min); x_t = x + step_dir * a_t; final_transformation_ = (Eigen::Translation(static_cast(x_t(0)), static_cast(x_t(1)), static_cast(x_t(2))) * Eigen::AngleAxis(static_cast(x_t(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis(static_cast(x_t(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis(static_cast(x_t(5)), Eigen::Vector3f::UnitZ())) .matrix(); // New transformed point cloud // Done on final cloud to prevent wasted computation transformPointCloud(*input_, trans_cloud, final_transformation_); // Updates score, gradient. Values stored to prevent wasted computation. score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false); // Calculate phi(alpha_t+) phi_t = -score; // Calculate phi'(alpha_t+) d_phi_t = -(score_gradient.dot(step_dir)); // Calculate psi(alpha_t+) psi_t = auxilaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu); // Calculate psi'(alpha_t+) d_psi_t = auxilaryFunction_dPsiMT(d_phi_t, d_phi_0, mu); // Check if I is now a closed interval if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) { open_interval = false; // Converts f_l and g_l from psi to phi f_l = f_l + phi_0 - mu * d_phi_0 * a_l; g_l = g_l + mu * d_phi_0; // Converts f_u and g_u from psi to phi f_u = f_u + phi_0 - mu * d_phi_0 * a_u; g_u = g_u + mu * d_phi_0; } if (open_interval) { // Update interval end points using Updating Algorithm [More, Thuente 1994] interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t); } else { // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t); } step_iterations++; } // If inner loop was run then hessian needs to be calculated. // Hessian is unnessisary for step length determination but gradients are required // so derivative and transform data is stored for the next iteration. // if (step_iterations) // computeHessian (hessian, trans_cloud, x_t); return (a_t); } // template // double pclomp::NormalDistributionsTransform::calculateScore(const PointCloudSource & // trans_cloud) const // { // double score = 0; // // for (int idx = 0; idx < trans_cloud.points.size(); idx++) // { // PointSource x_trans_pt = trans_cloud.points[idx]; // // // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. // std::vector neighborhood; // std::vector distances; // target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances); // // for (typename std::vector::iterator neighborhood_it = neighborhood.begin(); // neighborhood_it != neighborhood.end(); neighborhood_it++) // { // TargetGridLeafConstPtr cell = *neighborhood_it; // // Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); // // // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] // x_trans -= cell->getMean(); // // Uses precomputed covariance for speed. // Eigen::Matrix3d c_inv = cell->getInverseCov(); // // // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] // double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2); // // Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009] // double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_; // // score += score_inc / neighborhood.size(); // } // } // return (score) / static_cast (trans_cloud.size()); // } #endif // PCL_REGISTRATION_NDT_IMPL_H_ ================================================ FILE: src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.cpp ================================================ #include "voxel_grid_covariance_omp.h" #include "voxel_grid_covariance_omp_impl.hpp" template class pclomp::VoxelGridCovariance; template class pclomp::VoxelGridCovariance; ================================================ FILE: src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.h ================================================ /* * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. * * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * 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 OWNER 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. * */ #ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_ #define PCL_VOXEL_GRID_COVARIANCE_OMP_H_ #include #include #include #include #include #include "core/lightning_math.hpp" namespace pclomp { /** \brief A searchable voxel strucure containing the mean and covariance of the data. * \note For more information please see * Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. * PhD thesis, Orebro University. Orebro Studies in Technology 36 * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) */ template class VoxelGridCovariance : public pcl::VoxelGrid { protected: using pcl::VoxelGrid::filter_name_; using pcl::VoxelGrid::getClassName; using pcl::VoxelGrid::input_; using pcl::VoxelGrid::indices_; using pcl::VoxelGrid::filter_limit_negative_; using pcl::VoxelGrid::filter_limit_min_; using pcl::VoxelGrid::filter_limit_max_; using pcl::VoxelGrid::filter_field_name_; using pcl::VoxelGrid::downsample_all_data_; using pcl::VoxelGrid::leaf_layout_; using pcl::VoxelGrid::save_leaf_layout_; using pcl::VoxelGrid::leaf_size_; using pcl::VoxelGrid::min_b_; using pcl::VoxelGrid::max_b_; using pcl::VoxelGrid::inverse_leaf_size_; using pcl::VoxelGrid::div_b_; using pcl::VoxelGrid::divb_mul_; typedef typename pcl::traits::fieldList::type FieldList; typedef typename pcl::Filter::PointCloud PointCloud; typedef typename PointCloud::Ptr PointCloudPtr; typedef typename PointCloud::ConstPtr PointCloudConstPtr; public: typedef std::shared_ptr> Ptr; typedef std::shared_ptr> ConstPtr; /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf. * Inverse covariance, eigen vectors and engen values are precomputed. */ struct Leaf { EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** \brief Constructor. * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the * identity matrix */ Leaf() = default; /** \brief Get the voxel covariance. * \return covariance matrix */ Eigen::Matrix3d getCov() const { return (cov_); } /** \brief Get the inverse of the voxel covariance. * \return inverse covariance matrix */ Eigen::Matrix3d getInverseCov() const { return (icov_); } /** \brief Get the voxel centroid. * \return centroid */ Eigen::Vector3d getMean() const { return (mean_); } /** \brief Number of points contained by voxel */ int nr_points = 0; /** \brief 3D voxel centroid */ Eigen::Vector3d mean_ = Eigen::Vector3d::Zero(); /** \brief Nd voxel centroid * \note Differs from \ref mean_ when color data is used */ Eigen::Vector3f centroid = Eigen::Vector3f::Zero(); /** \brief Voxel covariance matrix */ Eigen::Matrix3d cov_ = Eigen::Matrix3d::Identity(); /** \brief Inverse of voxel covariance matrix */ Eigen::Matrix3d icov_ = Eigen::Matrix3d::Zero(); std::vector points_; }; /** \brief Const pointer to VoxelGridCovariance leaf structure */ typedef const Leaf *LeafConstPtr; using HashMap = std::unordered_map>; public: /** \brief Constructor. * Sets \ref leaf_size_ to 0 and \ref searchable_ to false. */ VoxelGridCovariance() : min_points_per_voxel_(6), min_covar_eigvalue_mult_(0.01), leaves_() { downsample_all_data_ = false; save_leaf_layout_ = false; leaf_size_.setZero(); min_b_.setZero(); max_b_.setZero(); filter_name_ = "VoxelGridCovariance"; leaves_.reserve(200000); } /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance * calculation). \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used */ inline void setMinPointPerVoxel(int min_points_per_voxel) { if (min_points_per_voxel > 2) { min_points_per_voxel_ = min_points_per_voxel; } else { PCL_WARN("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName().c_str()); min_points_per_voxel_ = 3; } } /** \brief Get the minimum number of points required for a cell to be used. * \return the minimum number of points for required for a voxel to be used */ inline int getMinPointPerVoxel() { return min_points_per_voxel_; } /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues */ inline void setCovEigValueInflationRatio(double min_covar_eigvalue_mult) { min_covar_eigvalue_mult_ = min_covar_eigvalue_mult; } /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices. * \return the minimum allowable ratio between eigenvalues */ inline double getCovEigValueInflationRatio() { return min_covar_eigvalue_mult_; } inline Eigen::Vector3i Pt2Key(const Eigen::Vector3f &pt) const { return (pt * inverse_leaf_size_[0]).array().round().template cast(); } /** \brief Get the voxels surrounding point p, not including the voxel contating point p. * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice). * \param[in] reference_point the point to get the leaf structure at * \param[out] neighbors * \return number of neighbors found */ int getNeighborhoodAtPoint(const Eigen::MatrixXi &, const PointT &reference_point, std::vector &neighbors) const; int getNeighborhoodAtPoint(const PointT &reference_point, std::vector &neighbors) const; int getNeighborhoodAtPoint7(const PointT &reference_point, std::vector &neighbors) const; int getNeighborhoodAtPoint1(const PointT &reference_point, std::vector &neighbors) const; /** \brief Get the leaf structure map * \return a map contataining all leaves */ inline const HashMap &getLeaves() { return leaves_; } /// added by xiang /// add a target point cloud into voxel grid void AddTarget(PointCloudPtr target); /// compute the cov and inv cov of each grid, paralleled void ComputeTargetGrids(); protected: /** \brief Minimum points contained with in a voxel to allow it to be useable. */ int min_points_per_voxel_; /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */ double min_covar_eigvalue_mult_; /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */ HashMap leaves_; }; } // namespace pclomp #endif // #ifndef PCL_VOXEL_GRID_COVARIANCE_H_ ================================================ FILE: src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp_impl.hpp ================================================ /* * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, Willow Garage, Inc. * * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * 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. * * Neither the name of the copyright holder(s) nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * 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 OWNER 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. * */ #ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ #define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_ #include #include #include #include #include #include "voxel_grid_covariance_omp.h" ////////////////////////////////////////////////////////////////////////////////////////// template int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector& neighbors) const { neighbors.clear(); // Find displacement coordinates auto key = Pt2Key(Eigen::Vector3f(reference_point.x, reference_point.y, reference_point.z)); neighbors.reserve(relative_coordinates.cols()); // Check each neighbor to see if it is occupied and contains sufficient points // Slower than radius search because needs to check 26 indices for (int ni = 0; ni < relative_coordinates.cols(); ni++) { auto displacement = relative_coordinates.col(ni); // Checking if the specified cell is in the grid auto leaf_iter = leaves_.find(key + displacement); if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_) { LeafConstPtr leaf = &(leaf_iter->second); neighbors.push_back(leaf); } } return (static_cast(neighbors.size())); } ////////////////////////////////////////////////////////////////////////////////////////// template int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint(const PointT& reference_point, std::vector& neighbors) const { neighbors.clear(); // Find displacement coordinates Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices(); return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); } ////////////////////////////////////////////////////////////////////////////////////////// template int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector& neighbors) const { neighbors.clear(); Eigen::MatrixXi relative_coordinates(3, 7); relative_coordinates.setZero(); relative_coordinates(0, 1) = 1; relative_coordinates(0, 2) = -1; relative_coordinates(1, 3) = 1; relative_coordinates(1, 4) = -1; relative_coordinates(2, 5) = 1; relative_coordinates(2, 6) = -1; return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors); } ////////////////////////////////////////////////////////////////////////////////////////// template int pclomp::VoxelGridCovariance::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector& neighbors) const { neighbors.clear(); return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3, 1), reference_point, neighbors); } template void pclomp::VoxelGridCovariance::AddTarget(pclomp::VoxelGridCovariance::PointCloudPtr target) { for (size_t cp = 0; cp < target->points.size(); ++cp) { auto key = Pt2Key(target->points[cp].getVector3fMap()); auto iter = leaves_.find(key); if (iter == leaves_.end()) { Leaf l; l.points_.emplace_back(target->points[cp].getVector3fMap()); l.nr_points = 1; leaves_.insert({key, l}); } else { auto& leaf = iter->second; leaf.points_.emplace_back(target->points[cp].getVector3fMap()); leaf.nr_points++; } } } template void pclomp::VoxelGridCovariance::ComputeTargetGrids() { std::for_each(leaves_.begin(), leaves_.end(), [this](auto& it) { Leaf& leaf = it.second; for (auto& pt : leaf.points_) { Eigen::Vector3d pt3d(pt[0], pt[1], pt[2]); // Accumulate point sum for centroid calculation leaf.mean_ += pt3d; // Accumulate x*xT for single pass covariance calculation leaf.cov_ += pt3d * pt3d.transpose(); // Do we need to process all the fields? leaf.centroid += pt; } // Eigen values and vectors calculated to prevent near singluar matrices Eigen::SelfAdjointEigenSolver eigensolver; Eigen::Matrix3d eigen_val; Eigen::Vector3d pt_sum; double min_covar_eigvalue; leaf.centroid /= static_cast(leaf.nr_points); // Point sum used for single pass covariance calculation pt_sum = leaf.mean_; // Normalize mean leaf.mean_ /= leaf.nr_points; if (leaf.nr_points < min_points_per_voxel_) { return; } // Single pass covariance calculation leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose(); leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points; // Normalize Eigen Val such that max no more than 100x min. eigensolver.compute(leaf.cov_); eigen_val = eigensolver.eigenvalues().asDiagonal(); auto evecs = eigensolver.eigenvectors(); if (eigen_val(0, 0) < 0 || eigen_val(1, 1) < 0 || eigen_val(2, 2) <= 0) { leaf.nr_points = -1; return; } // Avoids matrices near singularities (eq 6.11)[Magnusson 2009] min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val(2, 2); if (eigen_val(0, 0) < min_covar_eigvalue) { eigen_val(0, 0) = min_covar_eigvalue; if (eigen_val(1, 1) < min_covar_eigvalue) { eigen_val(1, 1) = min_covar_eigvalue; } leaf.cov_ = evecs * eigen_val * evecs.inverse(); } leaf.icov_ = leaf.cov_.inverse(); if (leaf.icov_.maxCoeff() == std::numeric_limits::infinity() || leaf.icov_.minCoeff() == -std::numeric_limits::infinity()) { leaf.nr_points = -1; } }); } #define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance; #endif // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_ ================================================ FILE: src/core/localization/localization.cpp ================================================ #include #include #include "core/localization/lidar_loc/lidar_loc.h" #include "core/localization/localization.h" #include #include "core/localization/pose_graph/pgo.h" #include "io/yaml_io.h" #include "ui/pangolin_window.h" namespace lightning::loc { // ！ 构造函数 Localization::Localization(Options options) { options_ = options; } // ！初始化函数 bool Localization::Init(const std::string& yaml_path, const std::string& global_map_path) { UL lock(global_mutex_); if (lidar_loc_ != nullptr) { // 若已经启动，则变为初始化 Finish(); } YAML_IO yaml(yaml_path); options_.with_ui_ = yaml.GetValue("system", "with_ui"); /// lidar odom前端 LaserMapping::Options opt_lio; opt_lio.is_in_slam_mode_ = false; lio_ = std::make_shared(opt_lio); if (!lio_->Init(yaml_path)) { LOG(ERROR) << "failed to init lio"; return false; } /// 激光定位 LidarLoc::Options lidar_loc_options; lidar_loc_options.update_dynamic_cloud_ = yaml.GetValue("lidar_loc", "update_dynamic_cloud"); lidar_loc_options.force_2d_ = yaml.GetValue("lidar_loc", "force_2d"); lidar_loc_options.map_option_.enable_dynamic_polygon_ = false; lidar_loc_options.map_option_.map_path_ = global_map_path; lidar_loc_ = std::make_shared(lidar_loc_options); if (options_.with_ui_) { ui_ = std::make_shared(); ui_->SetCurrentScanSize(1); ui_->Init(); lidar_loc_->SetUI(ui_); // lio_->SetUI(ui_); } lidar_loc_->Init(yaml_path); /// pose graph pgo_ = std::make_shared(); pgo_->SetDebug(false); /// 各模块的异步调用 options_.enable_lidar_loc_skip_ = yaml.GetValue("system", "enable_lidar_loc_skip"); options_.enable_lidar_loc_rviz_ = yaml.GetValue("system", "enable_lidar_loc_rviz"); options_.lidar_loc_skip_num_ = yaml.GetValue("system", "lidar_loc_skip_num"); options_.enable_lidar_odom_skip_ = yaml.GetValue("system", "enable_lidar_odom_skip"); options_.lidar_odom_skip_num_ = yaml.GetValue("system", "lidar_odom_skip_num"); options_.loc_on_kf_ = yaml.GetValue("lidar_loc", "loc_on_kf"); lidar_odom_proc_cloud_.SetMaxSize(1); lidar_loc_proc_cloud_.SetMaxSize(1); lidar_odom_proc_cloud_.SetName("激光里程计"); lidar_loc_proc_cloud_.SetName("激光定位"); // 允许跳帧 lidar_loc_proc_cloud_.SetSkipParam(options_.enable_lidar_loc_skip_, options_.lidar_loc_skip_num_); lidar_odom_proc_cloud_.SetSkipParam(options_.enable_lidar_odom_skip_, options_.lidar_odom_skip_num_); lidar_odom_proc_cloud_.SetProcFunc([this](CloudPtr cloud) { LidarOdomProcCloud(cloud); }); lidar_loc_proc_cloud_.SetProcFunc([this](CloudPtr cloud) { LidarLocProcCloud(cloud); }); if (options_.online_mode_) { lidar_odom_proc_cloud_.Start(); lidar_loc_proc_cloud_.Start(); } /// TODO: 发布 pgo_->SetHighFrequencyGlobalOutputHandleFunction([this](const LocalizationResult& res) { // if (loc_result_.timestamp_ > 0) { // double loc_fps = 1.0 / (res.timestamp_ - loc_result_.timestamp_); // // LOG_EVERY_N(INFO, 10) << "loc fps: " << loc_fps; // } loc_result_ = res; if (tf_callback_ && loc_result_.valid_) { tf_callback_(loc_result_.ToGeoMsg()); } if (ui_) { ui_->UpdateNavState(loc_result_.ToNavState()); ui_->UpdateRecentPose(loc_result_.pose_); } }); /// 预处理器 preprocess_.reset(new PointCloudPreprocess()); preprocess_->Blind() = yaml.GetValue("fasterlio", "blind"); preprocess_->TimeScale() = yaml.GetValue("fasterlio", "time_scale"); int lidar_type = yaml.GetValue("fasterlio", "lidar_type"); preprocess_->NumScans() = yaml.GetValue("fasterlio", "scan_line"); preprocess_->PointFilterNum() = yaml.GetValue("fasterlio", "point_filter_num"); float height_max = yaml.GetValue("roi", "height_max"); float height_min = yaml.GetValue("roi", "height_min"); preprocess_->SetHeightROI(height_max, height_min); LOG(INFO) << "lidar_type " << lidar_type; if (lidar_type == 1) { preprocess_->SetLidarType(LidarType::AVIA); LOG(INFO) << "Using AVIA Lidar"; } else if (lidar_type == 2) { preprocess_->SetLidarType(LidarType::VELO32); LOG(INFO) << "Using Velodyne 32 Lidar"; } else if (lidar_type == 3) { preprocess_->SetLidarType(LidarType::OUST64); LOG(INFO) << "Using OUST 64 Lidar"; } else if (lidar_type == 4) { preprocess_->SetLidarType(LidarType::ROBOSENSE); LOG(INFO) << "Using OUST 64 Lidar"; } else { LOG(WARNING) << "unknown lidar_type"; } return true; } void Localization::ProcessLidarMsg(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) { UL lock(global_mutex_); if (lidar_loc_ == nullptr || lio_ == nullptr || pgo_ == nullptr) { return; } // 串行模式 CloudPtr laser_cloud(new PointCloudType); preprocess_->Process(cloud, laser_cloud); laser_cloud->header.stamp = cloud->header.stamp.sec * 1e9 + cloud->header.stamp.nanosec; if (options_.online_mode_) { lidar_odom_proc_cloud_.AddMessage(laser_cloud); } else { LidarOdomProcCloud(laser_cloud); } } void Localization::ProcessLivoxLidarMsg(const livox_ros_driver2::msg::CustomMsg::SharedPtr cloud) { UL lock(global_mutex_); if (lidar_loc_ == nullptr || lio_ == nullptr || pgo_ == nullptr) { return; } // 串行模式 CloudPtr laser_cloud(new PointCloudType); preprocess_->Process(cloud, laser_cloud); laser_cloud->header.stamp = cloud->header.stamp.sec * 1e9 + cloud->header.stamp.nanosec; if (options_.online_mode_) { lidar_odom_proc_cloud_.AddMessage(laser_cloud); } else { LidarOdomProcCloud(laser_cloud); } } void Localization::LidarOdomProcCloud(CloudPtr cloud) { if (lio_ == nullptr) { return; } /// NOTE: 在NCLT这种数据集中，lio内部是有缓存的，它拿到的点云不一定是最新时刻的点云 lio_->ProcessPointCloud2(cloud); if (!lio_->Run()) { return; } auto lo_state = lio_->GetState(); lidar_loc_->ProcessLO(lo_state); pgo_->ProcessLidarOdom(lo_state); // LOG(INFO) << "LO pose: " << std::setprecision(12) << lo_state.timestamp_ << " " // << lo_state.GetPose().translation().transpose(); /// 获得lio的关键帧 auto scan = lio_->GetProjCloud(); if (options_.loc_on_kf_) { auto kf = lio_->GetKeyframe(); if (kf == lio_kf_) { /// 关键帧未更新，那就只更新IMU状态 // auto dr_state = lio_->GetState(); // lidar_loc_->ProcessDR(dr_state); // pgo_->ProcessDR(dr_state); return; } // if (ui_) { // ui_->UpdateKF(kf); // } lio_kf_ = kf; // auto scan = lio_->GetScanUndist(); if (options_.online_mode_) { lidar_loc_proc_cloud_.AddMessage(scan); } else { LidarLocProcCloud(scan); } } else { // auto scan = cloud; // 这个cloud应该差一个外参 if (options_.online_mode_) { lidar_loc_proc_cloud_.AddMessage(scan); } else { LidarLocProcCloud(scan); } } } void Localization::LidarLocProcCloud(CloudPtr scan_undist) { lidar_loc_->ProcessCloud(scan_undist); auto res = lidar_loc_->GetLocalizationResult(); pgo_->ProcessLidarLoc(res); if (ui_) { // Twi with Til, here pose means Twl, thus Til=I ui_->UpdateScan(scan_undist, res.pose_); } if (loc_state_callback_) { auto loc_state = std::make_shared(); loc_state->data = static_cast(res.status_); LOG(INFO) << "loc_state: " << loc_state->data; loc_state_callback_(*loc_state); } // cv::Mat img(100, 100, CV_8UC3, cv::Scalar(255, 255, 255)); // cv::imshow("img", img); // cv::waitKey(0); } void Localization::ProcessIMUMsg(IMUPtr imu) { UL lock(global_mutex_); if (lidar_loc_ == nullptr || lio_ == nullptr || pgo_ == nullptr) { return; } double this_imu_time = imu->timestamp; if (last_imu_time_ > 0 && this_imu_time < last_imu_time_) { LOG(WARNING) << "IMU 时间异常：" << this_imu_time << ", last: " << last_imu_time_; } last_imu_time_ = this_imu_time; /// 里程计处理IMU lio_->ProcessIMU(imu); /// 这里需要 IMU predict，否则没法process DR了 auto dr_state = lio_->GetIMUState(); if (!dr_state.pose_is_ok_) { return; } // /// 停车判定 // constexpr auto kThVbrbStill = 0.05; // 0.08; // constexpr auto kThOmegaStill = 0.05; // if (dr_state.GetVel().norm() < kThVbrbStill && imu->angular_velocity.norm() < kThOmegaStill) { // dr_state.is_parking_ = true; // dr_state.SetVel(Vec3d::Zero()); // } /// 如果没有odm, 用lio替代DR // LOG(INFO) << "dr state: " << std::setprecision(12) << dr_state.timestamp_ << " " // << dr_state.GetPose().translation().transpose() // << ", q=" << dr_state.GetPose().unit_quaternion().coeffs().transpose(); lidar_loc_->ProcessDR(dr_state); pgo_->ProcessDR(dr_state); } // void Localization::ProcessOdomMsg(const nav_msgs::msg::Odometry::SharedPtr odom_msg) { // UL lock(global_mutex_); // // if (lidar_loc_ == nullptr || lio_ == nullptr || pgo_ == nullptr) { // return; // } // double this_odom_time = ToSec(odom_msg->header.stamp); // if (last_odom_time_ > 0 && this_odom_time < last_odom_time_) { // LOG(WARNING) << "Odom Time Abnormal:" << this_odom_time << ", last: " << last_odom_time_; // } // last_odom_time_ = this_odom_time; // // lio_->ProcessOdometry(odom_msg); // // if (!lio_->GetbOdomHF()) { // return; // } // // auto dr_state = lio_->GetStateHF(mapping::FasterLioMapping::kHFStateOdomFiltered); // // constexpr auto kThVbrbStill = 0.03; // 0.08; // constexpr auto kThOmegaStill = 0.03; // if (dr_state.Getvwi().norm() < kThVbrbStill && dr_state.Getwii().norm() < kThOmegaStill) { // dr_state.is_parking_ = true; // dr_state.Setvwi(Vec3d::Zero()); // dr_state.Setwii(Vec3d::Zero()); // } // // lidar_loc_->ProcessDR(dr_state); // pgo_->ProcessDR(dr_state); // } void Localization::Finish() { lidar_loc_->Finish(); if (ui_) { ui_->Quit(); } lidar_loc_proc_cloud_.Quit(); lidar_odom_proc_cloud_.Quit(); } void Localization::SetExternalPose(const Eigen::Quaterniond& q, const Eigen::Vector3d& t) { UL lock(global_mutex_); /// 设置外部重定位的pose if (lidar_loc_) { lidar_loc_->SetInitialPose(SE3(q, t)); } } void Localization::SetTFCallback(Localization::TFCallback&& callback) { tf_callback_ = callback; } } // namespace lightning::loc ================================================ FILE: src/core/localization/localization.h ================================================ #pragma once #include "geometry_msgs/msg/transform_stamped.hpp" #include "std_msgs/msg/int32.hpp" #include "common/imu.h" #include "core/lio/laser_mapping.h" #include "core/localization/localization_result.h" #include "core/system/async_message_process.h" /// 预声明 namespace lightning { namespace ui { class PangolinWindow; } namespace loc { class LidarLoc; class PGO; /** * 实时定位接口实现 */ class Localization { public: struct Options { Options() {} bool online_mode_ = false; // 在线模式还是离线模式 bool with_ui_ = false; // 是否带ui /// 参数 SE3 T_body_lidar_; bool enable_lidar_odom_skip_ = false; // 是否允许激光里程计跳帧 int lidar_odom_skip_num_ = 1; // 如果允许跳帧，跳多少帧 bool enable_lidar_loc_skip_ = true; // 是否允许激光定位跳帧 bool enable_lidar_loc_rviz_ = false; // 是否允许调试用rviz int lidar_loc_skip_num_ = 4; // 如果允许跳帧，跳多少帧 bool loc_on_kf_ = false; }; Localization(Options options = Options()); ~Localization() = default; /** * 初始化，读配置参数 * @param yaml_path * @param global_map_path * @param init_reloc_pose */ bool Init(const std::string& yaml_path, const std::string& global_map_path); /// 处理lidar消息 void ProcessLidarMsg(const sensor_msgs::msg::PointCloud2::SharedPtr laser_msg); void ProcessLivoxLidarMsg(const livox_ros_driver2::msg::CustomMsg::SharedPtr laser_msg); /// 处理IMU消息 void ProcessIMUMsg(IMUPtr imu); // void ProcessOdomMsg(const nav_msgs::msg::Odometry::SharedPtr odom_msg) override; /// 由外部设置pose，适用于手动重定位 void SetExternalPose(const Eigen::Quaterniond& q, const Eigen::Vector3d& t); /// TODO: 其他初始化逻辑 /// TODO: 处理odom消息 /// 结束，保存临时地图 void Finish(); /// 异步处理函数 void LidarOdomProcCloud(CloudPtr); void LidarLocProcCloud(CloudPtr); using TFCallback = std::function; using LocStateCallback = std::function; using PointcloudBodyCallback = std::function; using PointcloudWorldCallback = std::function; void SetTFCallback(TFCallback&& callback); // void SetPathCallback(std::function&& callback); // void SetPointcloudWorldCallback(std::function&& callback); // void SetPointcloudBodyCallback(std::function&& callback); // void SetLocStateCallback(std::function&& callback); // void SetHealthDiagNormalCallback(interface::health_diag_normal_callback&& callback); private: /// 模块 ======================================================================================================== std::mutex global_mutex_; // 防止处理过程中被重复init Options options_; /// 预处理 std::shared_ptr preprocess_ = nullptr; // point cloud preprocess /// 前端 std::shared_ptr lio_ = nullptr; Keyframe::Ptr lio_kf_ = nullptr; // ui std::shared_ptr ui_ = nullptr; // pose graph std::shared_ptr pgo_ = nullptr; // lidar localization std::shared_ptr lidar_loc_; /// TODO async 处理 sys::AsyncMessageProcess lidar_odom_proc_cloud_; // lidar odom 处理点云 sys::AsyncMessageProcess lidar_loc_proc_cloud_; // lidar loc 处理点云 /// 结果数据 ===================================================================================================== LocalizationResult loc_result_; /// 框架相关 TFCallback tf_callback_; LocStateCallback loc_state_callback_; PointcloudBodyCallback pointcloud_body_callback_; PointcloudWorldCallback pointcloud_world_callback_; /// 输入检查 double last_imu_time_ = 0; double last_odom_time_ = 0; double last_cloud_time_ = 0; }; } // namespace loc } // namespace lightning ================================================ FILE: src/core/localization/localization_result.cc ================================================ // // Created by xiang on 24-4-11. // #include "localization_result.h" #include "core/lightning_math.hpp" namespace lightning::loc { geometry_msgs::msg::TransformStamped LocalizationResult::ToGeoMsg() const { geometry_msgs::msg::TransformStamped msg; msg.header.frame_id = "map"; msg.header.stamp = math::FromSec(timestamp_); msg.child_frame_id = "base_link"; msg.transform.translation.x = pose_.translation().x(); msg.transform.translation.y = pose_.translation().y(); msg.transform.translation.z = pose_.translation().z(); msg.transform.rotation.x = pose_.so3().unit_quaternion().x(); msg.transform.rotation.y = pose_.so3().unit_quaternion().y(); msg.transform.rotation.z = pose_.so3().unit_quaternion().z(); msg.transform.rotation.w = pose_.so3().unit_quaternion().w(); return msg; } NavState LocalizationResult::ToNavState() const { NavState ret; ret.timestamp_ = timestamp_; ret.confidence_ = confidence_; ret.pos_ = pose_.translation(); ret.rot_ = pose_.so3(); ret.pose_is_ok_ = status_ == LocalizationStatus::GOOD; ret.vel_ = (pose_.so3() * vel_b_); return ret; } } // namespace lightning::loc ================================================ FILE: src/core/localization/localization_result.h ================================================ // // Created by xiang on 2021/11/17. // #pragma once #include #include "common/eigen_types.h" #include "common/nav_state.h" namespace lightning::loc { /// 定位结果，包含所有的信息 /// 点云定位和最终结果都可以按照这个来输出 /// 定位状态 enum class LocalizationStatus { IDLE, // 0 无定位，也没有准备初始化 INITIALIZING, // 1 初始化过程中，位姿无效 GOOD, // 2 正常定位 FOLLOWING_DR, // 3 异常定位，输出DR递推 FAIL, // 4 定位失败 }; /// 定位结果 struct LocalizationResult { /// 对外部分 double timestamp_ = 0; // 时间戳 SE3 pose_; // 定位的位置和姿态（lidarLoc和PGO共用此变量） bool valid_ = false; // 标志此定位是否有效 LocalizationStatus status_ = LocalizationStatus::IDLE; // 定位状态位 /// 对内部分 bool lidar_loc_valid_ = false; // 该时刻lidarLoc是否有效 bool lidar_loc_inlier_ = false; // 该时刻lidarLoc在PGO中是否是内点 double confidence_ = 1.0; // lidarLoc的confidence(若有效) double lidar_loc_error_vert_ = 0; // lidarLoc相对于PGO定位的误差（纵向） double lidar_loc_error_hori_ = 0; // lidarLoc相对于PGO定位的误差（横向） double lidar_loc_delta_t_ = 0; // 相对于上一帧lidarLoc消息的时延 double lidar_loc_odom_delta_ = 0; // lidarLoc与lidarOdom对应时间两帧相差的距离 bool lidar_loc_smooth_flag_ = false; // Lidar loc是否满足平滑性要求（自身定位结果与外推结果类似） bool lidar_loc_odom_error_normal_ = true; // lidarLoc与lidarOdom对应时间两帧相差的距离超过一定阈值则为false bool lidar_loc_odom_reliable_ = true; // LO 认为自己的定位可不可信 double lidar_odom_error_vert_ = 0; // lidarOdom(通过滑窗首帧转换到map系下)相对于PGO定位的误差（纵向） double lidar_odom_error_hori_ = 0; // lidarOdom(通过滑窗首帧转换到map系下)相对于PGO定位的误差（横向） bool rel_pose_set_ = false; // 相对定位是否被设置（通常是lidarOdom） SE3 rel_pose_; // 相对定位的位置和姿态 Vec3d vel_b_ = Vec3d::Zero(); // 相对定位的速度 double lidar_odom_delta_t_ = 0; // 相对于上一帧lidarOdom消息的时延 double dr_delta_t_ = 0; // 相对于上一帧DR消息的时延 double is_parking_ = false; geometry_msgs::msg::TransformStamped ToGeoMsg() const; // 转到geometry msg NavState ToNavState() const; // 转到navstate }; } // namespace lightning::loc ================================================ FILE: src/core/localization/pose_graph/pgo.cc ================================================ #include "pgo.h" #include "pgo_impl.h" #include #include #include "common/options.h" #include "core/lightning_math.hpp" namespace lightning::loc { PGO::PGO() : impl_(new PGOImpl), pose_extrapolator_(new PoseExtrapolator) { smoother_ = std::make_shared(pgo::pgo_smooth_factor); LOG(INFO) << "smoother factor: " << pgo::pgo_smooth_factor; } PGO::~PGO() = default; void PGO::SetGlobalOutputHandleFunction(PGO::GlobalOutputHandleFunction handle) { impl_->output_func_ = std::move(handle); } void PGO::SetHighFrequencyGlobalOutputHandleFunction(PGO::GlobalOutputHandleFunction handle) { high_freq_output_func_ = std::move(handle); } void PGO::PubResult() { // 在有需要时，高频外推然后向外发送数据 if (high_freq_output_func_ && impl_->result_.valid_) { // 检查激光定位分值情况 if (last_lidar_loc_time_ > 0 && impl_->result_.timestamp_ > last_lidar_loc_time_) { if (impl_->result_.confidence_ < lidar_loc_score_thd_) { localization_unusual_count_++; LOG(INFO) << "当前lidar_loc分值较低: " << impl_->result_.confidence_ << " , 当前次数: " << localization_unusual_count_; } else { localization_unusual_count_ = 0; } } last_lidar_loc_time_ = impl_->result_.timestamp_; if (localization_unusual_count_ > localization_unusual_thd_ && last_lidar_loc_time_ > 0) { localization_unusual_tag_ = true; // LOG(INFO) << "连续多次匹配分值过低, 需检查定位精度"; } else { localization_unusual_tag_ = false; } auto result = impl_->result_; ExtrapolateLocResult(result); double dt = result.timestamp_ - impl_->result_.timestamp_; bool extrap_success = true; /// 利用激光定位的外推结果校验DR外推 if (impl_->lidar_loc_pose_queue_.size() > 5 && impl_->result_.lidar_loc_smooth_flag_ && (result.timestamp_ - impl_->lidar_loc_pose_queue_.back().timestamp_) < 0.3) { SE3 recent_lidar_loc_pose = impl_->lidar_loc_pose_queue_.back().pose_; /// 要求激光pose队列有效，且激光定位自身满足平滑性要求 SE3 lidar_loc_extrap_pose; // 外推之后的激光定位位姿 TimedPose best_match; bool interp_succ = math::PoseInterp( result.timestamp_, impl_->lidar_loc_pose_queue_, [](const TimedPose& tp) { return tp.timestamp_; }, [](const TimedPose& tp) { return tp.pose_; }, lidar_loc_extrap_pose, best_match, 1.0); if (interp_succ) { /// 检查激光定位外推结果与DR外推的差异 /// 条件1. 从激光定位指向外推结果的矢量方向 不应该与激光定位自身外推方向相差太多 // Vec3d v1 = result.pose_.translation() - recent_lidar_loc_pose.translation(); // Vec3d v2 = lidar_loc_extrap_pose.translation() - recent_lidar_loc_pose.translation(); // if (v1.norm() > 0.1 && v2.norm() > 0.1) { // v1.normalize(); // v2.normalize(); // const double theta_th = std::cos(15 * M_PI / 180.0); // 角度阈值 // if (v1.dot(v2) <= theta_th) { // /// 外推检查不通过 // LOG(WARNING) << "外推检查不通过：" << result.pose_.translation().transpose() << ", " // << lidar_loc_extrap_pose.translation().transpose() << ", dv: " << v1.dot(v2); // extrap_success = false; // result.pose_ = lidar_loc_extrap_pose; // } // } // SE3 delta = result.pose_.inverse() * lidar_loc_extrap_pose; // if (fabs(delta.translation()[0]) > 0.5 || fabs(delta.translation()[1]) > 0.5 || // delta.so3().log().norm() > 2.0 * M_PI / 180.0) { // /// 外推检查不通过 // LOG(WARNING) << "外推检查不通过：" << result.pose_.translation().transpose() << ", " // << lidar_loc_extrap_pose.translation().transpose() // << ", ang: " << delta.so3().log().norm(); // extrap_success = false; // result.pose_ = lidar_loc_extrap_pose; // } } } if (!impl_->dr_pose_queue_.empty()) { smoother_->PushDRPose(impl_->dr_pose_queue_.back().GetPose()); } impl_->result_.timestamp_ = result.timestamp_; impl_->result_.pose_ = result.pose_; SE3 extra_pose = result.pose_; smoother_->PushPose(result.pose_); result.pose_ = smoother_->GetPose(); // 输出force 2d // common::PoseRPY RPYXYZ = common::math::SE3ToRollPitchYaw(smoother_->GetPose()); // RPYXYZ.roll = 0; // RPYXYZ.pitch = 0; // RPYXYZ.z = 0; // result.pose_ = common::math::XYZRPYToSE3(RPYXYZ); high_freq_output_func_(result); impl_->output_pose_queue_.emplace_back(result.timestamp_, result.pose_); while (impl_->output_pose_queue_.size() > 1000) { impl_->output_pose_queue_.pop_front(); } // LOG_EVERY_N(INFO, 10) << std::fixed << "extrap: " << extra_pose.translation().transpose() // << ", smoother: " << result.pose_.translation().transpose() << ", dt: " << dt << ", t: // " << std::setprecision(12) << result.timestamp_; if (!extrap_success) { /// 外推失效，那么认为DR queue外推失效，清空DR队列 impl_->dr_pose_queue_.clear(); /// 同时清空final_output_pose，因为长时间依靠final output可能会导致发散 /// 所以当外推失效时，下个时刻就是激光定位+LO的pose。如果LO也失效，那就直接是激光定位的Pose /// final_output_pose_.clear(); } return; } } bool PGO::ProcessDR(const NavState& dr_result) { UL lock(impl_->data_mutex_); /// 假定DR定位是按时间顺序到达的 double delta_timestamp = 0; if (!impl_->dr_pose_queue_.empty()) { const double last_stamp = impl_->dr_pose_queue_.back().timestamp_; delta_timestamp = dr_result.timestamp_ - last_stamp; if (dr_result.timestamp_ < last_stamp) { LOG(WARNING) << "当前DR定位的结果的时间戳应当比上一个时间戳数值大，实际相减得" << dr_result.timestamp_ - last_stamp; return false; } } impl_->dr_pose_queue_.emplace_back(dr_result); if (impl_->dr_pose_queue_.size() > 1) { auto curr_it = impl_->dr_pose_queue_.rbegin(); auto last_it = curr_it; ++last_it; // curr_it->delta_t_ = curr_it->timestamp_ - last_it->timestamp_; } while (impl_->dr_pose_queue_.size() >= pgo::PGO_MAX_SIZE_OF_RELATIVE_POSE_QUEUE) { impl_->dr_pose_queue_.pop_front(); } if (!impl_->dr_pose_queue_.empty() && !is_parking_) { PubResult(); } else if (is_parking_ && high_freq_output_func_) { parking_result_.timestamp_ = dr_result.timestamp_; high_freq_output_func_(parking_result_); } return true; } bool PGO::ProcessLidarOdom(const NavState& lio_result) { UL lock(impl_->data_mutex_); /// 假定LidarOdom定位是按时间顺序到达的 if (!impl_->lidar_odom_pose_queue_.empty()) { const double last_stamp = impl_->lidar_odom_pose_queue_.back().timestamp_; if (lio_result.timestamp_ < last_stamp) { LOG(WARNING) << "当前LidarOdom定位时间戳回退，实际相减得" << lio_result.timestamp_ - last_stamp; } } // 保存lidarOdom帧 impl_->lidar_odom_pose_queue_.emplace_back(lio_result); if (impl_->lidar_odom_pose_queue_.size() > 1) { auto curr_it = impl_->lidar_odom_pose_queue_.rbegin(); auto last_it = curr_it; ++last_it; // curr_it->delta_t_ = curr_it->timestamp_ - last_it->timestamp_; } while (impl_->lidar_odom_pose_queue_.size() >= pgo::PGO_MAX_SIZE_OF_RELATIVE_POSE_QUEUE) { impl_->lidar_odom_pose_queue_.pop_front(); } if (!lio_result.lidar_odom_reliable_) { impl_->lidar_odom_conflict_with_dr_ = true; impl_->lidar_odom_conflict_with_dr_cnt_ = 200; } else { if (impl_->lidar_odom_conflict_with_dr_cnt_ > 0) { impl_->lidar_odom_conflict_with_dr_cnt_--; } else { impl_->lidar_odom_conflict_with_dr_ = false; } } /// 如果LO的时间比DR更新，则发布LO的递推结果 if (!impl_->dr_pose_queue_.empty() && lio_result.timestamp_ >= impl_->dr_pose_queue_.back().timestamp_ && !is_parking_) { PubResult(); } else if (is_parking_ && high_freq_output_func_) { parking_result_.timestamp_ = lio_result.timestamp_; high_freq_output_func_(parking_result_); } return true; } bool PGO::ProcessLidarLoc(const LocalizationResult& loc_result) { UL lock(impl_->data_mutex_); is_parking_ = loc_result.is_parking_; if (is_parking_ && high_freq_output_func_) { parking_result_ = loc_result; high_freq_output_func_(loc_result); return true; } // 如果相对位姿(DR和LidarOdom有一个即可)还没来，也退出 if (RelativePoseQueueEmpty()) { LOG(WARNING) << "PGO received LidarLoc, but is waiting for LO or DR ... "; return false; } if (!loc_result.lidar_loc_valid_) { return false; } // 不允许时间回退 static double last_lidar_loc_timestamp = -1; double lidar_loc_delta_t = loc_result.timestamp_ - last_lidar_loc_timestamp; if (last_lidar_loc_timestamp > 0) { if (lidar_loc_delta_t < 0) { LOG(ERROR) << "lidar loc 时间回退: " << lidar_loc_delta_t; return false; } else { last_lidar_loc_timestamp = loc_result.timestamp_; } } else { last_lidar_loc_timestamp = loc_result.timestamp_; } // 增加一个PGO Frame并触发一次PGO auto new_frame = std::make_shared(); new_frame->timestamp_ = loc_result.timestamp_; new_frame->opti_pose_ = loc_result.pose_; new_frame->last_opti_pose_ = loc_result.pose_; new_frame->lidar_loc_set_ = true; new_frame->lidar_loc_valid_ = loc_result.lidar_loc_valid_; new_frame->lidar_loc_pose_ = loc_result.pose_; new_frame->lidar_loc_delta_t_ = lidar_loc_delta_t; // 捕获lidarLoc给出的置信度 new_frame->confidence_ = loc_result.confidence_; if (loc_result.confidence_ > 0.6) { new_frame->lidar_loc_normalized_weight_ = Vec6d::Ones(); } else { const double clap_weight = std::max(loc_result.confidence_, 0.5); new_frame->lidar_loc_normalized_weight_ = clap_weight * Vec6d::Ones(); } impl_->result_.lidar_loc_odom_error_normal_ = loc_result.lidar_loc_odom_error_normal_; impl_->result_.lidar_loc_smooth_flag_ = loc_result.lidar_loc_smooth_flag_; if (!loc_result.lidar_loc_odom_error_normal_) { // LOG(WARNING) << "PGO接收到LO失效"; impl_->lidar_odom_valid_ = false; impl_->lidar_odom_valid_cnt_ = 10; } else { /// 如果Odom有效，也需要累计一段时间 if (impl_->lidar_odom_valid_cnt_ > 0) { impl_->lidar_odom_valid_cnt_--; } else { // LOG(INFO) << "PGO认为LO生效"; impl_->lidar_odom_valid_ = true; } } LOG(INFO) << std::setprecision(14) << std::fixed << "PGO received LidarLoc [" << new_frame->lidar_loc_pose_.translation().transpose() << "], t=" << new_frame->timestamp_; return ProcessPGOFrame(new_frame); } bool PGO::ProcessPGOFrame(std::shared_ptr frame) { impl_->AddPGOFrame(frame); impl_->lidar_loc_pose_queue_.emplace_back(impl_->result_.timestamp_, impl_->result_.pose_); while (impl_->lidar_loc_pose_queue_.size() > 50) { impl_->lidar_loc_pose_queue_.pop_front(); } if (!impl_->dr_pose_queue_.empty() && frame->timestamp_ >= impl_->dr_pose_queue_.back().timestamp_) { PubResult(); } return true; } std::shared_ptr PGO::GetCurrentPGOFrame() const { return impl_->current_frame_; } bool PGO::Reset() { UL lock(impl_->data_mutex_); smoother_->Reset(); return impl_->Reset(); } void PGO::SetDebug(bool debug) { impl_->SetDebug(debug); } void PGO::LogWindowState() { UL lock(impl_->data_mutex_); // 显示最近5个PGOFrame的信息 auto& window = impl_->frames_; int idx1 = (window.size() - 5) >= 0 ? (window.size() - 5) : -1; int idx2 = (window.size() - 4) >= 0 ? (window.size() - 4) : -1; int idx3 = (window.size() - 3) >= 0 ? (window.size() - 3) : -1; int idx4 = (window.size() - 2) >= 0 ? (window.size() - 2) : -1; int idx5 = (window.size() - 1) >= 0 ? (window.size() - 1) : -1; boost::format fmt("--- %c --- %c --- %c --- %c --- %c ---"); std::string lidar_info = idx1 >= 0 ? "info" : "empty"; LOG(INFO) << "Show PGO window state: \n" << " ************************************************** \n" << " ** index : --- 1st --- 2nd --- 3rd --- 4th --- 5th \n" << " ** lidar loc:" << " ** rtk loc:" << " ** rel loc:" << " ** " << " ** // + is valid/good, ? is invalid/bad, W is wrong, G is good \n" << " ************************************************** "; // LOG(INFO) << "PGO received lidar loc [" // << new_frame->lidar_loc_pose_.translation().transpose() << "], let's go for one shot! ||||||||||||||||| \n" // << " ********* PGO status ********* \n" // << " ** current window size : " << impl_->frames_.size() << "\n" // << " ** gps queue size : " << impl_->gps_queue_.size() << "\n" // << " ** LO queue size : " << impl_->lidar_odom_pose_queue_.size() << "\n" // << " ** DR queue size : " << impl_->dr_pose_queue_.size() << "\n" // << " ********* ********** ********* \n"; } bool PGO::ExtrapolateLocResult(LocalizationResult& output_result) { if (!output_result.valid_) { return false; } auto& dr_pose_queue = impl_->dr_pose_queue_; auto& lo_pose_queue = impl_->lidar_odom_pose_queue_; SE3 interp_pose; NavState best_match; /// 确定外推的最远时间，由DR，GPS，LO中取最大者（因为这几个都可能断流） double latest_time = impl_->result_.timestamp_; if (!dr_pose_queue.empty() && dr_pose_queue.back().timestamp_ > latest_time) { latest_time = dr_pose_queue.back().timestamp_; } if (!lo_pose_queue.empty() && lo_pose_queue.back().timestamp_ > latest_time) { latest_time = lo_pose_queue.back().timestamp_; } // if (!impl_->gps_queue_.empty() && impl_->gps_queue_.back().timestamp_ > latest_time) { // latest_time = impl_->gps_queue_.back().timestamp_; // } // 其他数据源时间检测imu时间 if (!dr_pose_queue.empty() && latest_time - dr_pose_queue.back().timestamp_ > imu_interruption_time_thd_) { imu_interruption_tag_ = true; LOG(ERROR) << "长时间未获取到DR数据, IMU存在断流, 断流时间: " << latest_time - dr_pose_queue.back().timestamp_; } else { imu_interruption_tag_ = false; } // 用LO外推到最新时刻 // if (impl_->lidar_odom_valid_ && !lo_pose_queue.empty() && // lo_pose_queue.back().timestamp_ > output_result.timestamp_) { // bool lo_interp_success = roki::common::math::PoseInterp( // output_result.timestamp_, lo_pose_queue, // [](const NavState& nav_state) { return nav_state.timestamp_; }, // [](const NavState& nav_state) { return nav_state.pose_; }, interp_pose, best_match); // if (lo_interp_success) { // SE3 pose_incre = interp_pose.inverse() * lo_pose_queue.back().pose_; // output_result.pose_ = output_result.pose_ * pose_incre; // const double time_incre = lo_pose_queue.back().timestamp_ - output_result.timestamp_; // output_result.timestamp_ = lo_pose_queue.back().timestamp_; // // 如果外推时间比较久(比如超过3s)，跟踪状态改为跟踪相对位姿 // if (time_incre > 3.0) { // output_result.status_ = common::GlobalPoseStatus::FOLLOWING_LiDAR_ODOM; // } // } // } // if (impl_->lidar_odom_conflict_with_dr_ && impl_->lidar_odom_valid_) { // /// 用LO外推到DR时刻 // SE3 interp_pose1, interp_pose2; // bool interp1 = common::math::PoseInterp( // latest_time, lo_pose_queue, [](const NavState& nav_state) { return nav_state.timestamp_; }, // [](const NavState& nav_state) { return nav_state.pose_; }, interp_pose1, best_match); // bool interp2 = common::math::PoseInterp( // output_result.timestamp_, lo_pose_queue, // [](const NavState& nav_state) { return nav_state.timestamp_; }, // [](const NavState& nav_state) { return nav_state.pose_; }, interp_pose2, best_match); // if (interp1 && interp2) { // SE3 pose_incre = interp_pose2.inverse() * interp_pose1; // output_result.pose_ = output_result.pose_ * pose_incre; // output_result.timestamp_ = latest_time; // } // } // 其次用DR做外推 if (!dr_pose_queue.empty() && dr_pose_queue.back().timestamp_ > output_result.timestamp_) { double dr_extrap_time = dr_pose_queue.back().timestamp_ - output_result.timestamp_; // DR 递推的时间 /// NOTE 当LO失效，lidar loc有较大延时，可能需要DR递推较久的时间 double dr_pose_inc_th = 5.0 * dr_extrap_time; // 允许DR外推的距离 if (!impl_->lidar_odom_valid_) { dr_pose_inc_th *= 2; // lidar odom invalid，放宽 } if (dr_pose_inc_th < 2.0) { dr_pose_inc_th = 2.0; } bool dr_interp_success = math::PoseInterp( output_result.timestamp_, dr_pose_queue, [](const NavState& nav_state) { return nav_state.timestamp_; }, [](const NavState& nav_state) { return nav_state.GetPose(); }, interp_pose, best_match); if (dr_interp_success) { SE3 pose_incre = interp_pose.inverse() * dr_pose_queue.back().GetPose(); /// 限制此处的pose incre大小 // if (pose_incre.translation().norm() > dr_pose_inc_th) { // LOG(WARNING) << "pose increment is too large: " << pose_incre.translation().norm() // << ", skip extrapolate, dt=" << dr_extrap_time << ", th: " << dr_pose_inc_th; // return true; // } output_result.pose_ = output_result.pose_ * pose_incre; const double time_incre = dr_pose_queue.back().timestamp_ - output_result.timestamp_; output_result.timestamp_ = dr_pose_queue.back().timestamp_; } } if (output_result.timestamp_ < (latest_time - 0.05)) { /// 如果中间失效导致最新定位无法更新，那么用激光定位将其外推至最新位置 SE3 interp_pose_final; TimedPose best_match_final; if (math::PoseInterp( latest_time, impl_->lidar_loc_pose_queue_, [](const TimedPose& pose) { return pose.timestamp_; }, [](const TimedPose& pose) { return pose.pose_; }, interp_pose_final, best_match_final, 5.0)) { output_result.pose_ = interp_pose_final; output_result.timestamp_ = latest_time; } } return true; } } // namespace lightning::loc ================================================ FILE: src/core/localization/pose_graph/pgo.h ================================================ #pragma once #include "common/eigen_types.h" #include "common/nav_state.h" #include "core/localization/localization_result.h" #include "pgo_impl.h" #include "pose_extrapolator.h" #include "smoother.h" namespace lightning::loc { /** * Pose Graph Optimization * 基于 Pose Graph 的多源信息融合。 * * 输入： * -- DR，需要降低对此的依赖（轮速失效case较多），兼容没有DR * -- LiDAR Odom，频率可能不高，比如5Hz；lidarOdom应该提供自己的hessian矩阵的PCA分析，提供退化情况参考 * -- LiDAR Loc，频率可能也不高，可动态配置 * * 输出： * -- 融合定位结果，与IMU同频率发布 * -- 判断各个信息源的outlier，并剔除之 * -- 盲走一段时间误差控制在容许范围内 * * 调用： * -- 外部可以调用所有接口塞数据和获取结果，是多线程安全的； * -- 但内部只有一个线程，不保证一定不阻塞，外部应当handle这种情况。 */ class PGO { public: PGO(); ~PGO(); /// 向外输出全局定位结果 using GlobalOutputHandleFunction = std::function; void SetGlobalOutputHandleFunction(GlobalOutputHandleFunction handle); void SetHighFrequencyGlobalOutputHandleFunction(GlobalOutputHandleFunction handle); /// 处理lidarOdom信息 bool ProcessDR(const NavState& dr_result); /// 处理lidarOdom信息 bool ProcessLidarOdom(const NavState& lio_result); /// 接收激光定位信息（触发PGO优化） bool ProcessLidarLoc(const LocalizationResult& loc_result); /// 处理外部组装好的一个PGO frame，将触发优化（仅在单测时外部直接调用） bool ProcessPGOFrame(std::shared_ptr frame); std::shared_ptr GetCurrentPGOFrame() const; // 高频查询定位，使用DR做递推，多线程安全 bool GetCurrentLocalization() const; /// 重置PGO bool Reset(); /// 向外发布位姿 void PubResult(); /// debug stuffs void SetDebug(bool debug = true); void LogWindowState(); public: bool localization_unusual_tag_ = false; // 定位失效标志位 bool imu_interruption_tag_ = false; // imu断流标志位 private: inline bool RelativePoseQueueEmpty() { return (impl_->dr_pose_queue_.empty() && impl_->lidar_odom_pose_queue_.empty()); } /** * 为当前定位结果尽可能实时地外推位姿，考虑LO和DR两个信息源 * 在当前激光定位的基础之上，先用LO进行外推，再用DR进行外推 * * 同时和自身外推做校验，防止DR的异常 * * @param @in/out output_result 当前融合定位结果（激光定位时间） * @return */ bool ExtrapolateLocResult(LocalizationResult& output_result); std::unique_ptr impl_; // 在本层放一个extrapolator，被三个地方访问：DR，lidarOdom，LidarLoc，RTK std::unique_ptr pose_extrapolator_; std::function high_freq_output_func_; LocalizationResult high_freq_result_; LocalizationResult parking_result_; std::shared_ptr smoother_; float imu_interruption_time_thd_ = 1.0; // imu数据断流时间阈值 s float lidar_loc_score_thd_ = 0.5; // 激光定位分值阈值 int localization_unusual_thd_ = 10; // 激光定位失效次数阈值 int localization_unusual_count_ = 0; // 定位异常次数 double last_lidar_loc_time_ = 0.; // 上次激光定位时间戳 bool is_parking_ = false; }; } // namespace lightning::loc ================================================ FILE: src/core/localization/pose_graph/pgo_impl.cc ================================================ #include "pgo_impl.h" #include "core/opti_algo/algo_select.h" #include "core/robust_kernel/robust_kernel_all.h" #include "core/lightning_math.hpp" #include #include #include #include namespace lightning::loc { /** * 几个踩过的坑 * - g2o fix某个vertex后，unary * edge的计算可能被跳过，导致拿chi2的时候可能拿到一个非法值，很神奇；所以最好是拿chi2前都重新计算一遍 */ namespace { /// SE3 转g2o的SE3Quat /// 把约束边信息转化为字符串 template std::string print_info(const std::vector& edges, double th = 0) { std::vector chi2; for (auto& edge : edges) { if (edge->Level() == 0) { chi2.push_back(edge->Chi2()); } } std::sort(chi2.begin(), chi2.end()); double ave_chi2 = std::accumulate(chi2.begin(), chi2.end(), 0.0) / chi2.size(); boost::format fmt("数量: %d, 均值: %f, 中位数: %f, 0.1分位: %f, 0.9分位: %f, 最大值: %f, 阈值: %f\n"); if (!chi2.empty()) { std::string str = (fmt % chi2.size() % ave_chi2 % chi2[chi2.size() / 2] % chi2[int(chi2.size() * 0.1)] % chi2[int(chi2.size() * 0.9)] % chi2.back() % th) .str(); return str; } return std::string(""); } } // namespace /// alias for pgo option PGOImpl::PGOImpl(Options options) { options_ = options; /// set noise params auto set6dnoise = [](Vec6d& noise_item, const double& pos_noise, const double& ang_noise) { noise_item.head<3>() = Vec3d::Ones() * pos_noise; noise_item.tail<3>() = Vec3d::Ones() * ang_noise; }; set6dnoise(lidar_loc_noise_, options_.lidar_loc_pos_noise, options_.lidar_loc_ang_noise); set6dnoise(lidar_odom_rel_noise_, options_.lidar_odom_pos_noise, options_.lidar_odom_ang_noise); set6dnoise(dr_rel_noise_, options_.dr_pos_noise, options_.dr_ang_noise); // Setup solver miao::OptimizerConfig config(miao::AlgorithmType::LEVENBERG_MARQUARDT, miao::LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN, false); config.incremental_mode_ = true; config.max_vertex_size_ = options_.PGO_MAX_FRAMES; optimizer_ = miao::SetupOptimizer<6, 3>(config); } bool PGOImpl::Reset() { LOG(WARNING) << "PGO is reset"; CleanProblem(); frames_.clear(); frames_by_id_.clear(); current_frame_ = nullptr; last_frame_ = nullptr; return true; } void PGOImpl::AddPGOFrame(std::shared_ptr pgo_frame) { assert(pgo_frame != nullptr); if (last_frame_ != nullptr) { const double adjacent_dalta_t = pgo_frame->timestamp_ - last_frame_->timestamp_; if (adjacent_dalta_t < 0.) { LOG(WARNING) << "PGO received pgoframe, however timestamp rollback for " << adjacent_dalta_t << "senonds!"; return; } pgo_frame->lidar_loc_delta_t_ = adjacent_dalta_t; } if (debug_) { LOG(INFO) << "inserting pgo frame, it's timestamp is " << std::setprecision(18) << pgo_frame->timestamp_; } // 这里尝试设置相对位姿观测，如果上游（通常是激光定位）给了就跳过； // 如果 LidarOdom 和 DR 都设置失败，结束本函数。 bool interp_lio_success = AssignLidarOdomPoseIfNeeded(pgo_frame); bool interp_dr_success = AssignDRPoseIfNeeded(pgo_frame); if (!interp_lio_success && !interp_dr_success) { LOG(ERROR) << "PGO received pgo frame, but assign relative pose failed!"; return; } is_in_map_ = pgo_frame->lidar_loc_set_ && pgo_frame->lidar_loc_valid_; if (!is_in_map_) { // LOG(ERROR) << "PGO received PGOFrame with lidar_loc_set_(" << pgo_frame->lidar_loc_set_ << "), lidar_loc_valid_(" << pgo_frame->lidar_loc_valid_ << "); Reject It!"; return; } pgo_frame->frame_id_ = accumulated_frame_id_++; current_frame_ = pgo_frame; frames_by_id_.emplace(pgo_frame->frame_id_, pgo_frame); // 由于到达时间可能不一致，最好是插到正确的位置（仅限多线程，单线程没这问题） // 2023-02-16：在唯一触发源唯一的情况下，无需考虑到达时间不一致问题 frames_.emplace_back(pgo_frame); /// 触发一次优化 RunOptimization(); // // 根据优化更新一些状态量 // UpdatePoseGraphState(); // 输出结果信息 CollectOptimizationStatistics(); // 清空 // CleanProblem(); /// 需要时，删除一部分 SlideWindowAdaptively(); last_frame_ = current_frame_; } bool PGOImpl::AssignLidarOdomPoseIfNeeded(std::shared_ptr frame) { assert(frame != nullptr); SE3 interp_pose; Vec3d interp_vel_b; NavState best_match; bool lo_interp_done = false; // 无论如何，我们都要寻找最近的lidarOdom帧，用于判断PGOFrame中lidarOdom的置信度状态 lo_interp_done = math::PoseInterp( frame->timestamp_, lidar_odom_pose_queue_, [](const NavState& nav_state) { return nav_state.timestamp_; }, [](const NavState& nav_state) { return nav_state.GetPose(); }, interp_pose, best_match); if (lo_interp_done) { UpdateLidarOdomStatusInFrame(best_match, frame); // frame->lidar_odom_delta_t_ = best_match.delta_t_; } // 如果PGOFrame中还没有设置LO插值位姿，设置它。 if (!frame->lidar_odom_set_) { if (lo_interp_done) { interp_vel_b = best_match.GetRot().matrix().transpose() * best_match.GetVel(); frame->lidar_odom_set_ = true; frame->lidar_odom_valid_ = true; frame->lidar_odom_pose_ = interp_pose; frame->lidar_odom_vel_ = interp_vel_b; static int counts = 0; ++counts; return true; } else { // lidarodom 不见得一定比 lidarloc 快。 LOG(WARNING) << "PGOFrame (frame_id " << frame->frame_id_ << ") Interpolate on lidarOdom Failed!"; LOG(WARNING) << "PGOFrame time: " << std::fixed << std::setprecision(18) << frame->timestamp_ << ", latest lidarOdom time: " << lidar_odom_pose_queue_.back().timestamp_; return false; } } else { return true; } // 正常不会到这里 return false; } bool PGOImpl::AssignDRPoseIfNeeded(std::shared_ptr frame) { assert(frame != nullptr); SE3 interp_pose; Vec3d interp_vel_b; NavState best_match; bool dr_interp_done = false; // 无论如何，我们都要寻找最近的DR帧，用于判断PGO收到的DR消息的时延 dr_interp_done = math::PoseInterp( frame->timestamp_, dr_pose_queue_, [](const NavState& nav_state) { return nav_state.timestamp_; }, [](const NavState& nav_state) { return nav_state.GetPose(); }, interp_pose, best_match); if (!frame->dr_valid_) { if (dr_interp_done) { interp_vel_b = best_match.GetRot().matrix().transpose() * best_match.GetVel(); frame->dr_set_ = true; frame->dr_valid_ = true; frame->dr_pose_ = interp_pose; frame->dr_vel_b_ = interp_vel_b; static int counts = 0; ++counts; // LOG(INFO) << "PGO interp DR success - " << counts << " times."; return true; } else { // DR理论上应该比 lidarloc 快。 LOG(WARNING) << "PGOFrame (frame_id" << frame->frame_id_ << ") Interpolate on DR Failed!"; return false; } } else { return true; } // 正常不会到这里 return false; } void PGOImpl::UpdateLidarOdomStatusInFrame(NavState& lio_result, std::shared_ptr frame) { // 把LidarOdom信息更新到PGOFrame frame->lidar_odom_normalized_weight_ = lio_result.confidence_ * Vec6d::Ones(); Eigen::Array3d trans_confidence = {1.0, 1.0, 1.0}; Eigen::Array3d rot_confidence = {1.0, 1.0, 1.0}; if ((trans_confidence < kLidarOdomTransDegenThres).any()) { frame->lidar_odom_trans_degenerated = true; } if ((rot_confidence < kLidarOdomRotDegenThres).any()) { frame->lidar_odom_rot_degenerated = true; } } void PGOImpl::RunOptimization() { // if (frames_.size() < kMinNumRequiredForOptimization) { // LOG(INFO) << "Skip optimization because frame size is " << frames_.size(); // return; // } if (debug_) { LOG(INFO) << "Run pose graph optimization: "; } // build g2o problem BuildProblem(); // solve problems optimizer_->InitializeOptimization(); optimizer_->SetVerbose(options_.verbose_); optimizer_->Optimize(5); // 确定inlier和outliers // RemoveOutliers(); // solve again // optimizer_->InitializeOptimization(); // optimizer_->Optimize(5); // get results for (const auto& frame : frames_) { auto v = std::dynamic_pointer_cast(optimizer_->GetVertex(frame->frame_id_)); if (v == nullptr) { continue; } ++frame->opti_times_; frame->last_opti_pose_ = frame->opti_pose_; frame->opti_pose_ = SE3(v->Estimate().matrix()); } } void PGOImpl::BuildProblem() { // Add Vertex AddVertex(); // Add Factors if (is_in_map_) { AddLidarLocFactors(); } AddLidarOdomFactors(); AddPriorFactors(); } void PGOImpl::CleanProblem() { optimizer_->Clear(); vertices_.clear(); lidar_loc_edges_.clear(); lidar_odom_edges_.clear(); dr_edges_.clear(); prior_edges_.clear(); } void PGOImpl::AddVertex() { auto v = std::make_shared(); v->SetId(current_frame_->frame_id_); v->SetEstimate(current_frame_->opti_pose_); optimizer_->AddVertex(v); /// NOTE: 在incremental模式下，optimizer可能会修改顶点的ID（需要代替过去的某个顶点）， /// 这导致顶点ID与frame_id对应不上。这里我们将frame_id改成顶点ID current_frame_->frame_id_ = v->GetId(); vertices_.emplace_back(v); } void PGOImpl::AddLidarLocFactors() { // // 不支持未设置的或者无效的lidarLoc约束。 if (!current_frame_->lidar_loc_set_) { return; } /// 不管lidarLoc是否为valid，factor都会加，只是可能会判为outlier SE3 loc_obs_pose = current_frame_->lidar_loc_pose_; Mat6d loc_obs_cov = Mat6d::Zero(); loc_obs_cov(0, 0) = lidar_loc_noise_[0] * lidar_loc_noise_[0]; loc_obs_cov(1, 1) = lidar_loc_noise_[1] * lidar_loc_noise_[1]; loc_obs_cov(2, 2) = lidar_loc_noise_[2] * lidar_loc_noise_[2]; loc_obs_cov(3, 3) = lidar_loc_noise_[3] * lidar_loc_noise_[3]; loc_obs_cov(4, 4) = lidar_loc_noise_[4] * lidar_loc_noise_[4]; loc_obs_cov(5, 5) = lidar_loc_noise_[5] * lidar_loc_noise_[5]; Mat6d loc_obs_info = loc_obs_cov.inverse(); Vec6d loc_obs_weight = current_frame_->lidar_loc_normalized_weight_; loc_obs_info(0, 0) *= loc_obs_weight[0]; loc_obs_info(1, 1) *= loc_obs_weight[1]; loc_obs_info(2, 2) *= loc_obs_weight[2]; loc_obs_info(3, 3) *= loc_obs_weight[3]; loc_obs_info(4, 4) *= loc_obs_weight[4]; loc_obs_info(5, 5) *= loc_obs_weight[5]; auto e = std::make_shared(); e->SetVertex(0, optimizer_->GetVertex(current_frame_->frame_id_)); e->SetMeasurement(loc_obs_pose); e->SetInformation(loc_obs_info); auto rk = std::make_shared(); rk->SetDelta(options_.lidar_loc_outlier_th); e->SetRobustKernel(rk); optimizer_->AddEdge(e); lidar_loc_edges_.emplace_back(e); } void PGOImpl::AddLidarOdomFactors() { size_t num = lo_relative_constraints_num_; // 该循环负责在不直接相邻的帧之间也添加相对位姿约束，每一帧最多向后找‘num’帧；这个策略让graph更稳定 for (auto iter = frames_.rbegin(); iter != frames_.rend(); ++iter) { auto frame = *iter; if (frame == current_frame_) { continue; } if ((current_frame_->frame_id_ - frame->frame_id_) >= num) { continue; } auto pre_key_frame = frame; auto cur_key_frame = current_frame_; if (!pre_key_frame->lidar_odom_valid_ || !cur_key_frame->lidar_odom_valid_) { continue; } SE3 lo_obs_pose = pre_key_frame->lidar_odom_pose_.inverse() * cur_key_frame->lidar_odom_pose_; Mat6d lo_obs_cov = Mat6d::Zero(); const Vec6d lo_noise = lidar_odom_rel_noise_; lo_obs_cov(0, 0) = lo_noise[0] * lo_noise[0]; lo_obs_cov(1, 1) = lo_noise[1] * lo_noise[1]; lo_obs_cov(2, 2) = lo_noise[2] * lo_noise[2]; lo_obs_cov(3, 3) = lo_noise[3] * lo_noise[3]; lo_obs_cov(4, 4) = lo_noise[4] * lo_noise[4]; lo_obs_cov(5, 5) = lo_noise[5] * lo_noise[5]; // 取两帧中更小的权重作为实际权重 —— 取“木桶短板效应”原理 Vec6d lo_obs_weight = lo_noise; // 添加边 auto e = std::make_shared(); auto v1 = optimizer_->GetVertex(pre_key_frame->frame_id_); auto v2 = optimizer_->GetVertex(cur_key_frame->frame_id_); if (v1 == nullptr || v2 == nullptr) { continue; } e->SetVertex(0, v1); e->SetVertex(1, v2); e->SetMeasurement(lo_obs_pose); Mat6d rel_obs_info = lo_obs_cov.inverse(); e->SetInformation(rel_obs_info); /// 这个e仍然需要robust kernel auto rk = std::make_shared(); rk->SetDelta(options_.lidar_odom_ang_noise); e->SetRobustKernel(rk); optimizer_->AddEdge(e); lidar_odom_edges_.emplace_back(e); } } void PGOImpl::AddDRFactors() { size_t num = dr_relative_constraints_num_; for (auto iter = frames_.rbegin(); iter != frames_.rend(); ++iter) { auto frame = *iter; if (frame == current_frame_) { continue; } if ((current_frame_->frame_id_ - frame->frame_id_) >= num) { continue; } auto pre_key_frame = frame; auto cur_key_frame = current_frame_; if (!pre_key_frame->dr_valid_ || !cur_key_frame->dr_valid_) { continue; } SE3 dr_rel_pose = pre_key_frame->dr_pose_.inverse() * cur_key_frame->dr_pose_; Mat6d dr_obs_cov = Mat6d::Zero(); const Vec6d dr_noise = dr_rel_noise_; dr_obs_cov(0, 0) = dr_noise[0] * dr_noise[0]; dr_obs_cov(1, 1) = dr_noise[1] * dr_noise[1]; dr_obs_cov(2, 2) = dr_noise[2] * dr_noise[2]; dr_obs_cov(3, 3) = dr_noise[3] * dr_noise[3]; dr_obs_cov(4, 4) = dr_noise[4] * dr_noise[4]; dr_obs_cov(5, 5) = dr_noise[5] * dr_noise[5]; // 添加边 auto e = std::make_shared(); auto v1 = optimizer_->GetVertex(pre_key_frame->frame_id_); auto v2 = optimizer_->GetVertex(cur_key_frame->frame_id_); if (v1 == nullptr || v2 == nullptr) { continue; } e->SetVertex(0, optimizer_->GetVertex(pre_key_frame->frame_id_)); e->SetVertex(1, optimizer_->GetVertex(cur_key_frame->frame_id_)); e->SetMeasurement(dr_rel_pose); Mat6d dr_obs_info = dr_obs_cov.inverse(); // DR无法给出weight，我们直接tune noise即可。 e->SetInformation(dr_obs_info); optimizer_->AddEdge(e); dr_edges_.emplace_back(e); } } void PGOImpl::AddPriorFactors() { auto frame = current_frame_; if (frame->prior_set_ && frame->prior_valid_) { SE3 prior_obs_pose = frame->prior_pose_; auto e = std::make_shared(); e->SetVertex(0, optimizer_->GetVertex(frame->frame_id_)); e->SetMeasurement(prior_obs_pose); // 有必要实现noise的融合吗，毕竟还有权重项可以tune // Vec6d prior_noise = rtk_fix_noise_; // Vec6d prior_noise = lidar_odom_rel_noise_; Vec6d prior_noise = lidar_loc_noise_; // 先验（边缘化）约束的cov应该参考lidarLoc和lidarOdom，是一个可信任的观测 Mat6d prior_obs_cov = Mat6d::Zero(); prior_obs_cov(0, 0) = prior_noise[0] * prior_noise[0]; prior_obs_cov(1, 1) = prior_noise[1] * prior_noise[1]; prior_obs_cov(2, 2) = prior_noise[2] * prior_noise[2]; prior_obs_cov(3, 3) = prior_noise[3] * prior_noise[3]; prior_obs_cov(4, 4) = prior_noise[4] * prior_noise[4]; prior_obs_cov(5, 5) = prior_noise[5] * prior_noise[5]; Mat6d prior_obs_info = prior_obs_cov.inverse(); const Vec6d prior_obs_weight = frame->prior_normalized_weight_; prior_obs_info(0, 0) *= prior_obs_weight[0]; prior_obs_info(1, 1) *= prior_obs_weight[1]; prior_obs_info(2, 2) *= prior_obs_weight[2]; prior_obs_info(3, 3) *= prior_obs_weight[3]; prior_obs_info(4, 4) *= prior_obs_weight[4]; prior_obs_info(5, 5) *= prior_obs_weight[5]; e->SetInformation(prior_obs_info); // 个人觉得应该是不用加核函数的（像lidarOdom一样） // auto* rk = new g2o::RobustKernelHuber(); // rk->setDelta(pgo_option::lidar_loc_outlier_th/*使用lidarLoc的核参数*/); // e->setRobustKernel(rk); // e->setParameterId(0, 0); optimizer_->AddEdge(e); prior_edges_.emplace_back(e); } } void PGOImpl::RemoveOutliers() { int cnt_outliers = 0; auto remove_outlier = [&cnt_outliers, this](miao::Edge* e) { e->ComputeError(); if (e->Chi2() > e->GetRobustKernel()->Delta()) { e->SetLevel(1); cnt_outliers++; // set rtk outlier in frame auto frame = frames_by_id_.find(e->GetVertex(0)->GetId())->second; if (debug_) { LOG(INFO) << "frame " << frame->frame_id_ << " has outlier gps, chi2 = " << e->Chi2() << " > " << e->GetRobustKernel()->Delta(); } } else { auto frame = frames_by_id_.find(e->GetVertex(0)->GetId())->second; if (debug_) { LOG(INFO) << "frame " << frame->frame_id_ << " has inlier gps, chi2 = " << e->Chi2() << " < " << e->GetRobustKernel()->Delta(); } e->SetRobustKernel(nullptr); } }; } void PGOImpl::UpdatePoseGraphState() {} void PGOImpl::CollectOptimizationStatistics() { /// 打印必要信息 if (debug_) { LOG(INFO) << std::string("LidarLoc -- ") + print_info(lidar_loc_edges_, options_.lidar_loc_outlier_th); LOG(INFO) << std::string("LidarOdom -- ") + print_info(lidar_odom_edges_); LOG(INFO) << std::string("DR -- ") + print_info(dr_edges_); LOG(INFO) << std::string("Marginal Prior -- ") + print_info(prior_edges_); } // 把滑窗中最新帧的信息更新到result中，在需要时输出到外部 if (frames_.size() < kMinNumRequiredForOptimization) { // 只有一个frame的时候，优化被跳过，各种chi2也无效 UpdateFinalResultByLastFrame(); } else { UpdateFinalResultByWindow(); } if (output_func_) { output_func_(result_); } // 打印必要的误差信息(lidarOdom的error大于0为有效，否则invalid) static std::stringstream ss; std::string lo_error_vert = "invalid", lo_error_hori = "invalid"; ss << std::fixed << std::setprecision(3); if (result_.lidar_odom_error_vert_ > 0) { ss.str(""); ss << result_.lidar_odom_error_vert_; lo_error_vert = ss.str(); } if (result_.lidar_odom_error_hori_ > 0) { ss.str(""); ss << result_.lidar_odom_error_hori_; lo_error_hori = ss.str(); } const double loc_err = std::sqrt(result_.lidar_loc_error_vert_ * result_.lidar_loc_error_vert_ + result_.lidar_loc_error_hori_ * result_.lidar_loc_error_hori_); const double lo_err = std::sqrt(result_.lidar_odom_error_vert_ * result_.lidar_odom_error_vert_ + result_.lidar_odom_error_hori_ * result_.lidar_odom_error_hori_); } void PGOImpl::UpdateFinalResultByLastFrame() { auto& lf = frames_.back(); PGOFrameToResult(lf, result_); // 近似计算lidarOdom与优化位姿的误差 auto& ff = frames_.front(); if (ff->lidar_odom_valid_ && lf->lidar_odom_valid_) { SE3 lo_pose_w = ff->opti_pose_ * ff->lidar_odom_pose_.inverse() * lf->lidar_odom_pose_; Vec3d lo_error = lo_pose_w.translation() - lf->opti_pose_.translation(); Vec3d lo_err_body = lf->opti_pose_.so3().inverse() * lo_error; result_.lidar_odom_error_vert_ = fabs(lo_err_body[0]); result_.lidar_odom_error_hori_ = fabs(lo_err_body[1]); } else { result_.lidar_odom_error_vert_ = -999; result_.lidar_odom_error_hori_ = -999; } // 一些需要从PGOFrame以外获取的信息 result_.valid_ = true; // result_.is_in_map_ = is_in_map_; // result_.absolute_pose_valid_ = lf->rtk_valid_; // outlier也算valid // result_.rtk_status_ = gps_queue_.back().gps_status_; // 发送给下游：rtk实时状态 /// 确定整体定位状态 // if (lf->rtk_valid_ && lf->lidar_loc_valid_ && lf->lidar_odom_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_ALL; // } else if (lf->rtk_valid_ && lf->lidar_loc_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_BOTH1; // } else if (lf->rtk_valid_ && lf->lidar_odom_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_BOTH2; // } else if (lf->lidar_loc_valid_ && lf->lidar_odom_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_LIDAR_FULLY; // } else if (lf->rtk_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_GNSS; // } else if (lf->lidar_loc_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_LIDAR_LOC; // } else if (lf->lidar_odom_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_LiDAR_ODOM; // } else if (lf->dr_valid_) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_DR; // } else { // result_.status_ = common::GlobalPoseStatus::OTHER; // } SetPgoGraphVertexes(); } void PGOImpl::UpdateFinalResultByWindow() { auto& lf = frames_.back(); PGOFrameToResult(lf, result_); // 近似计算lidarOdom与优化位姿的误差 auto& ff = frames_.front(); if (ff->lidar_odom_valid_ && lf->lidar_odom_valid_) { SE3 lo_pose_w = ff->opti_pose_ * ff->lidar_odom_pose_.inverse() * lf->lidar_odom_pose_; Vec3d lo_error = lo_pose_w.translation() - lf->opti_pose_.translation(); Vec3d lo_err_body = lf->opti_pose_.so3().inverse() * lo_error; result_.lidar_odom_error_vert_ = fabs(lo_err_body[0]); result_.lidar_odom_error_hori_ = fabs(lo_err_body[1]); } else { result_.lidar_odom_error_vert_ = -999; result_.lidar_odom_error_hori_ = -999; } // 一些需要从PGOFrame以外获取的信息 result_.valid_ = true; // result_.is_in_map_ = is_in_map_; // result_.absolute_pose_valid_ = lf->rtk_valid_; // outlier也算valid // result_.rtk_status_ = gps_queue_.back().gps_status_; // 发送给下游：rtk实时状态 // LOG(INFO) << "PGO assigned result with position [" << result_.pose_.translation().transpose() << "]."; /// 确定各个source的定位状态 bool following_gps = false; bool following_lidar_loc = false; bool following_lidar_odom = lf->lidar_odom_valid_; bool following_dr = lf->dr_valid_; // if (lf->lidar_odom_rot_degenerated && lf->lidar_odom_trans_degenerated) { // following_lidar_odom = false; // } // 判定RTK状态【至多使用滑窗中最新的3帧】 & 保留chi2信息 // for (const auto& edge : gps_edges_) { // edge->computeError(); // frames_by_id_.find(edge->vertex(0)->id())->second->rtk_chi2_ = edge->chi2(); // } // double min_rtk_chi2 = 999999.; // int used_frames = 0; // for (auto it = gps_edges_.rbegin(); it != gps_edges_.rend(); ++it) { // if ((*it)->chi2() < pgo_option::rtk_outlier_th) { // following_gps = true; // } // if ((*it)->chi2() < min_rtk_chi2) { // min_rtk_chi2 = (*it)->chi2(); // } // ++used_frames; // if (used_frames >= 3) { // break; // } // } // // LOG(INFO) << "------- recent 5 minimum rtk chi2 is " << min_rtk_chi2; // if (!lf->rtk_valid_) { // // RTK 设置了但是RTK状态位无效，所以自动是outlier // result_.rtk_loc_inlier_ = false; // } else { // // RTK 给进来为有效，要看chi2是否满足 // if (lf->rtk_chi2_ < pgo_option::rtk_outlier_th) { // result_.rtk_loc_inlier_ = true; // } else { // result_.rtk_loc_inlier_ = false; // LOG(WARNING) << "rtk is outlier, chi2 = " << lf->rtk_chi2_ << ", > " << pgo_option::rtk_outlier_th; // } // } // 判定lidarLoc状态 & 保留chi2信息 // for (const auto& edge : lidar_loc_edges_) { // edge->ComputeError(); // auto frame = frames_by_id_.find(edge->GetVertex(0)->GetId()); // if (edge->Chi2() < options_.lidar_loc_outlier_th && frame->second->lidar_loc_valid_) { // following_lidar_loc = true; // } // frames_by_id_.find(edge->GetVertex(0)->GetId())->second->lidar_loc_chi2_ = edge->Chi2(); // } // if (lf->lidar_loc_set_) { // lidarKLoc有效性仅看chi2，和输入时是否有效无关 // // if (lf->lidar_loc_set_ && lf->lidar_loc_valid_) { // result_.lidar_loc_inlier_ = lf->lidar_loc_chi2_ < options_.lidar_loc_outlier_th; // if (!result_.lidar_loc_inlier_) { // LOG(WARNING) << "lidar loc is outlier, chi2 = " << lf->lidar_loc_chi2_ << ", > " // << options_.lidar_loc_outlier_th; // } // } // 给出全局定位状态 // if (following_lidar_odom && following_lidar_loc && following_gps) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_ALL; // } else if (following_lidar_loc && following_gps) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_BOTH1; // } else if (following_lidar_odom && following_gps) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_BOTH2; // } else if (following_lidar_loc && following_lidar_odom) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_LIDAR_FULLY; // } else if (following_gps) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_GNSS; // } else if (following_lidar_loc) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_LIDAR_LOC; // } else if (following_lidar_odom) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_LiDAR_ODOM; // } else if (following_dr) { // result_.status_ = common::GlobalPoseStatus::FOLLOWING_DR; // } else { // result_.status_ = common::GlobalPoseStatus::OTHER; // } // LOG(INFO) << "PGO assigned result with global status [" << common::StatusToString(result_.status_) << "]."; // // 判定是否需要尝试从RTK pose来搜索激光定位 // /// 如果最近的rtk有效 (且与优化pose有明显差异)，无论如何都可以尝试一下（试试又不会怀孕） // if (lf->rtk_valid_ && // (lf->rtk_pose_.translation().head<2>() - lf->lidar_loc_pose_.translation().head<2>()).norm() > 0.2) { // LOG(INFO) << "should try rtk pose: " << lf->rtk_pose_.translation().head<2>().transpose() // << " and opti: " << lf->opti_pose_.translation().head<2>().transpose(); // should_try_rtk_pose_for_localization_ = true; // } // SetPgoGraphVertexes(); } void PGOImpl::PGOFrameToResult(const PGOFramePtr& frame, LocalizationResult& result) { if ((result.timestamp_ - frame->timestamp_) > 0.3) { /// 激光定位与当前时刻相差太多，则放弃设置此结果 return; } // 复制PGOFrame中的信息到Result中 result.timestamp_ = frame->timestamp_; result.pose_ = frame->opti_pose_; result.lidar_loc_valid_ = frame->lidar_loc_valid_; result.lidar_loc_inlier_ = frame->lidar_loc_inlier_; result.lidar_loc_delta_t_ = frame->lidar_loc_delta_t_; result.confidence_ = frame->confidence_; Vec3d loc_error = frame->lidar_loc_pose_.translation() - result.pose_.translation(); Vec3d loc_err_body = result.pose_.so3().inverse() * loc_error; result.lidar_loc_error_vert_ = fabs(loc_err_body[0]); result.lidar_loc_error_hori_ = fabs(loc_err_body[1]); // 用lidarOdom和DR一起给出相对观测 result.rel_pose_set_ = frame->lidar_odom_set_; result.rel_pose_ = frame->lidar_odom_pose_; result.vel_b_ = frame->dr_vel_b_; result.lidar_odom_delta_t_ = frame->lidar_odom_delta_t_; result.dr_delta_t_ = frame->dr_delta_t_; /// 此时检查外推历史队列和融合定位队列 // SE3 output_history_pose; // common::TimedPose match; // if (result.lidar_loc_valid_ && // common::math::PoseInterp( // result.timestamp_, output_pose_queue_, [](const common::TimedPose& p) { return p.time_; }, // [](const common::TimedPose& p) { return p.pose_; }, output_history_pose, match)) { // /// 检查历史位姿和当前定位的差异。如果差异较少，则取一个平均以保证外推平滑性 // SE3 delta = result.pose_.inverse() * output_history_pose; // if (delta.translation().norm() < 0.3 && delta.so3().log().norm() < 1.5 * M_PI / 180.0) { // const double fusion_ratio = 0.7; // 更偏向于历史外推？ // result.pose_ = result.pose_ * SE3::exp(fusion_ratio * delta.log()); // } else { // const double fusion_ratio = 0.3; // 更偏向于自身定位？ // result.pose_ = result.pose_ * SE3::exp(fusion_ratio * delta.log()); // } // } } void PGOImpl::SetPgoGraphVertexes() { if (frames_.empty() || current_frame_ == nullptr) { return; } } void PGOImpl::SlideWindowAdaptively() { // NOTE 由于这里的frames要和optimizer中的保持一致，所以只能移除最新的关键帧 // 保持窗口有一定宽度，但关键帧间距也不要太长 if (frames_.size() < options_.PGO_MAX_FRAMES) { return; } auto frame_to_remove = *(frames_.begin()); auto frame_to_keep = *(++frames_.begin()); Marginalize(frame_to_remove, frame_to_keep); frames_.erase(frames_.begin()); frames_by_id_.erase(frame_to_remove->frame_id_); return; // 看是移除队头还是次新帧 // int n = frames_.size(); // auto last_frame = frames_[n - 1]; // auto last_frame2 = frames_[n - 2]; // double dp = (last_frame->opti_pose_.translation() - last_frame2->opti_pose_.translation()).norm(); // double da = (last_frame->opti_pose_.so3().inverse() * last_frame2->opti_pose_.so3()).log().norm(); // if (dp < options_.PGO_DISTANCE_TH_LAST_FRAMES && da < options_.PGO_ANGLE_TH_LAST_FRAMES) { // // remove 次新帧 // auto frame_to_remove = frames_.back(); // frames_.pop_back(); // frames_by_id_.erase(frame_to_remove->frame_id_); // } else { // assert(frames_.size() > 1); // // remove 队头 -- 需要执行动态策略和边缘化 // auto frame_to_remove = *(frames_.begin()); // auto frame_to_keep = *(++frames_.begin()); // if (frame_to_remove->opti_times_ > 2) { // SE3 pose_error = frame_to_remove->last_opti_pose_.inverse() * frame_to_remove->opti_pose_; // const double delta_p = pose_error.translation().norm(); // const double delta_a = pose_error.so3().log().norm(); // bool is_converged = // (delta_p < options_.pgo_frame_converge_pos_th) && (delta_a < options_.pgo_frame_converge_ang_th); // if (is_converged) { // // 队头帧已经收敛了，边缘化后丢掉它 // Marginalize(frame_to_remove, frame_to_keep); // frames_.erase(frames_.begin()); // frames_by_id_.erase(frame_to_remove->frame_id_); // } // } // if (frames_.size() > (options_.PGO_MAX_FRAMES + 5)) { // // 最坏的情况，即使不收敛，我们也不能让滑窗过大 // auto frame_to_remove = *(frames_.begin()); // auto frame_to_keep = *(++frames_.begin()); // Marginalize(frame_to_remove, frame_to_keep); // frames_.erase(frames_.begin()); // frames_by_id_.erase(frame_to_remove->frame_id_); // } // } } void PGOImpl::Marginalize(const PGOFramePtr& frame_to_remove, const PGOFramePtr& frame_to_keep) { /// 严格来说，我们需要按照边缘化理论实现这套东西； /// TODO: @wgh 实现边缘化算法 /// 但受限于时间，我们先极简处理一下，跑通流程要紧。 frame_to_keep->prior_pose_ = frame_to_keep->opti_pose_; // frame_to_keep->prior_cov_; frame_to_keep->prior_normalized_weight_ = Vec6d::Ones(); frame_to_keep->prior_set_ = true; frame_to_keep->prior_valid_ = true; } void PGOImpl::log_window_status(std::ostringstream& report) { report.str(""); // report << std::fixed << std::setprecision(6); report << std::setprecision(6); report << "Pose Graph Vertices: "; for (auto& vertice : vertices_) { report << " ------ id=" << vertice->GetId() << " ------ | "; } report << "\n"; report << "LidarLoc edges : "; for (auto& edge : lidar_loc_edges_) { edge->ComputeError(); report << " (id" << edge->GetVertex(0)->GetId() << ")chi2=" << edge->Chi2() << " | "; } report << "\n"; report << "Prior edges : "; for (auto& edge : prior_edges_) { edge->ComputeError(); report << " (id" << edge->GetVertex(0)->GetId() << ")chi2=" << edge->Chi2() << " | "; } report << "\n"; // 相对约束：仅打印异常边 report << "LidarOdom edges : "; for (auto& edge : lidar_odom_edges_) { edge->ComputeError(); if (edge->Chi2() > 10) { report << " (id" << edge->GetVertex(0)->GetId() << "&" << edge->GetVertex(1)->GetId() << ")chi2=" << edge->Chi2() << " | "; } } report << "\n"; // 相对约束：仅打印异常边 report << "DR edges : "; for (auto& edge : dr_edges_) { edge->ComputeError(); if (edge->Chi2() > 10) { report << " (id" << edge->GetVertex(0)->GetId() << "&" << edge->GetVertex(1)->GetId() << ")chi2=" << edge->Chi2() << " | "; } } report << "\n"; } } // namespace lightning::loc ================================================ FILE: src/core/localization/pose_graph/pgo_impl.h ================================================ #pragma once #include #include #include #include #include #include #include #include #include "common/constant.h" #include "common/eigen_types.h" #include "common/nav_state.h" #include "common/timed_pose.h" #include "core/graph/optimizer.h" #include "core/localization/localization_result.h" #include "core/types/edge_se3.h" #include "core/types/edge_se3_prior.h" namespace lightning::loc { using FrameId = unsigned long; /** * 保存一个优化单元需要的所有观测信息 */ struct PGOFrame { EIGEN_MAKE_ALIGNED_OPERATOR_NEW PGOFrame() = default; double timestamp_ = 0; FrameId frame_id_ = 0; // 优化后的位姿 SE3 opti_pose_; // 最新的优化位姿 SE3 last_opti_pose_; // 上一轮优化位姿，用于判断是否收敛 int opti_times_ = 0; // 当前帧被优化的次数 // GNSS/RTK观测【插值量】 | 提供绝对约束 // bool rtk_set_ = false; // RTK是否已经设置（可以已设置但状态位无效） // bool rtk_valid_ = false; // RTK在[状态位]角度来看是否有效 // bool rtk_inlier_ = true; // RTK在PGO看来是否为inlier // SE3 rtk_pose_; // 插值得到的RTK pose（roll&pitch为零，z为零） // double rtk_delta_t_ = 0; // 插值时，bestmatch相对于上一帧rtk消息的时延 // double rtk_interp_time_error = 0; // 当前帧在rtk队列上做插值时的时间误差 // common::UTMCoordinate rtk_utm_; // 与PGO帧最接近的原始RTK观测 // double rtk_chi2_ = 0.0; // RTK卡方误差 // 激光定位观测 | 提供绝对约束 bool lidar_loc_set_ = false; // lidarLoc是否已经设置（PGO由lodarLoc触发，正常是有效的） bool lidar_loc_valid_ = false; // lidarLoc是否有效（只要设置了就有效） bool lidar_loc_inlier_ = true; // lidarLoc在PGO看来是否为inlier SE3 lidar_loc_pose_; // lidarLoc定位观测 double lidar_loc_delta_t_ = 0; // 插值时，bestmatch相对于上一帧lidarLoc消息的时延 double confidence_ = 0; // lidarLoc给出的原始置信度 Vec6d lidar_loc_normalized_weight_ = Vec6d::Ones(); // 归一化权重，表征退化情况，位于(0, 1]区间，平移在前旋转在后 bool lidar_loc_rot_degenerated = false; // 根据归一化权重来决定 bool lidar_loc_trans_degenerated = false; // 根据归一化权重来决定 double lidar_loc_chi2_ = 0.0; // lidarLoc观测的卡方误差 // 激光里程计观测 | 提供帧间相对约束 bool lidar_odom_set_ = false; // LO是否已经设置（由lidarLoc设置或者PGO中插值） bool lidar_odom_valid_ = false; // LO是否有效（只要设置了就是有效） SE3 lidar_odom_pose_; // LO自系位姿观测 double lidar_odom_delta_t_ = 0; // 插值时，bestmatch相对于上一帧lidarOdom消息的时延 Vec6d lidar_odom_normalized_weight_ = Vec6d::Ones(); // 归一化权重，表征退化情况，位于(0, 1]区间，平移在前旋转在后 bool lidar_odom_rot_degenerated = false; // 根据归一化权重来决定（无论是否退化都会添加约束） bool lidar_odom_trans_degenerated = false; // 根据归一化权重来决定 Vec3d lidar_odom_vel_ = Vec3d::Zero(); // 删除这个量？ double lidar_odom_chi2_ = 0.; // LO观测的卡方误差 bool lo_reliable_ = true; // DR观测 | 提供帧间相对约束 bool dr_set_ = false; // DR是否已经设置（正常都是设置了的） bool dr_valid_ = false; // DR是否有效（正常跑是有效的，但边界上不一定） SE3 dr_pose_; // DR自系位姿观测 Vec3d dr_vel_b_ = Vec3d::Zero(); // 来自DR的vel,车体系下的 double dr_delta_t_ = 0; // 插值时，bestmatch相对于上一帧DR消息的时延 // 先验“观测”（对滑出帧【边缘化】之后导出的虚拟观测） | 提供绝对约束 bool prior_set_ = false; // 边缘化约束是否设置（仅滑窗的第一帧会设置） bool prior_valid_ = false; // 边缘化约束是否有效（只要设置了就是有效） SE3 prior_pose_; // 边缘化约束提供的位姿 Mat6d prior_cov_ = Mat6d::Identity(); // 边缘化约束的协方差（来自边缘化计算） Vec6d prior_normalized_weight_ = Vec6d::Ones(); // 归一化权重，位于(0, 1]区间，平移在前旋转在后 double prior_chi2_ = 0.; // 边缘化“观测”的卡方误差 }; using PGOFramePtr = std::shared_ptr; /** * Pose Graph Implementation * 基于 Pose Graph 的多源信息融合：实现。 */ struct PGOImpl { using UL = std::unique_lock; struct Options { Options() {} bool verbose_ = false; // 调试打印 static constexpr int PGO_MAX_FRAMES = 5; // PGO所持的最大帧数 static constexpr int PGO_MAX_SIZE_OF_RELATIVE_POSE_QUEUE = 10000; // PGO 相对定位队列最大长度 static constexpr int PGO_MAX_SIZE_OF_RTK_POSE_QUEUE = 200; // PGO RTK观测队列最大长度 static constexpr double PGO_DISTANCE_TH_LAST_FRAMES = 2.5; // PGO 滑窗时，最近两帧的最小距离 static constexpr double PGO_ANGLE_TH_LAST_FRAMES = 10 * M_PI / 360.0; // PGO 滑窗时，最近两帧的最小角度 double lidar_loc_pos_noise = 0.3; // lidar定位位置噪声 // 0.3 double lidar_loc_ang_noise = 1.0 * constant::kDEG2RAD; // lidar定位角度噪声 double lidar_loc_outlier_th = 30.0; // lidar定位异常阈值 double lidar_odom_pos_noise = 0.3; // LidarOdom相对定位位置噪声 double lidar_odom_ang_noise = 1.0 * constant::kDEG2RAD; // LidarOdom相对定位角度噪声 double lidar_odom_outlier_th = 0.01; // LidarOdom异常值检测 double dr_pos_noise = 1.0; // DR相对定位位置噪声 // 0.05 double dr_ang_noise = 0.5 * constant::kDEG2RAD; // DR相对定位角度噪声 double dr_pos_noise_ratio = 1.0; // DR位置噪声倍率 double pgo_frame_converge_pos_th = 0.05; // PGO帧位置收敛阈值 double pgo_frame_converge_ang_th = 1.0 * constant::kDEG2RAD; // PGO帧姿态收敛阈值 double pgo_smooth_factor = 0.01; // PGO帧平滑因子 }; PGOImpl(Options options = {}); ~PGOImpl() {} /// 总逻辑 void AddPGOFrame(std::shared_ptr frame); /// 为frame分配相对定位pose，如果已经有了就什么也不做。 /// 注意有 lidarOdom pose 和 DR pose 两个；要求至少有一个，否则返回 false。 // bool AssignRelativePoseIfNeeded(std::shared_ptr frame); bool AssignLidarOdomPoseIfNeeded(std::shared_ptr frame); bool AssignDRPoseIfNeeded(std::shared_ptr frame); void UpdateLidarOdomStatusInFrame(NavState& lio_result, std::shared_ptr frame); /// 执行优化的逻辑 void RunOptimization(); // 建立g2o优化问题 void BuildProblem(); // 清空优化问题 void CleanProblem(); // 添加顶点 void AddVertex(); // 添加lidar定位约束 void AddLidarLocFactors(); // 添加帧间相对约束，我们更愿意相信LidarOdom，如果LO退化才会用DR void AddLidarOdomFactors(); void AddDRFactors(); // 如果滑窗第一帧存在边缘化约束，添加之 void AddPriorFactors(); // 剔除异常值：假设所有信息源都可能有异常值？目前仅实现了对RTK判outlier void RemoveOutliers(); // 获取优化详细信息 void CollectOptimizationStatistics(); /// 是否打开调试输出 void SetDebug(bool debug) { debug_ = debug; } /// 清空并重置 bool Reset(); private: /// 更新系统状态 void UpdatePoseGraphState(); /// 保存结果 void UpdateFinalResultByLastFrame(); void UpdateFinalResultByWindow(); inline void PGOFrameToResult(const PGOFramePtr& frame, LocalizationResult& result); inline void SetPgoGraphVertexes(); /// 自适应调整窗口大小：如果早期帧的优化结果已经充分收敛，删除一部分；否则，适当多保留一些 void SlideWindowAdaptively(); /// 边缘化处理：把即将被滑出窗口的PGOFrame的约束转化为下一个PGOFrame的先验约束 void Marginalize(const PGOFramePtr& frame_to_remove, const PGOFramePtr& frame_to_keep); void log_window_status(std::ostringstream& report); public: Options options_; // 全局数据锁 std::mutex data_mutex_; const size_t kMinNumRequiredForOptimization = 2; const double kLidarOdomTransDegenThres = 0.3; const double kLidarOdomRotDegenThres = 0.3; /// data std::vector> frames_; // PGO的帧队列 std::shared_ptr last_frame_ = nullptr; // 上一帧，用于处理延时 std::map> frames_by_id_; // PGO帧按照Id进行索引 std::shared_ptr current_frame_ = nullptr; // 记录当前PGO帧 std::deque dr_pose_queue_; // DR位姿观测队列 std::deque lidar_odom_pose_queue_; // LidarOdom观测队列 std::deque lidar_loc_pose_queue_; // LidarLoc观测队列 std::deque output_pose_queue_; // 输出位姿队列 // internal variables double last_gps_time_ = 0; // 上一个gps时间戳,用于判断重复/回流数据 FrameId accumulated_frame_id_ = 0; // 每次自加1，作为新进PGOFrame的Id /// the miao optimizer std::shared_ptr optimizer_ = nullptr; /// miao 里的指针接口是shared_ptr std::vector> vertices_; // pose graph顶点 std::vector> lidar_odom_edges_; // 相对运动观测（来自LidarOdom） std::vector> dr_edges_; // 相对运动观测（来自DR） std::vector> lidar_loc_edges_; // 激光定位边 std::vector> prior_edges_; // 边缘化约束 // output variables LocalizationResult result_; // pgo融合定位结果 // output call back std::function output_func_; /// params Vec6d rtk_fix_noise_ = Vec6d::Zero(); // 固定解RTK噪声 Vec6d rtk_other_noise_ = Vec6d::Zero(); // 其他解RTK噪声 Vec6d lidar_loc_noise_ = Vec6d::Zero(); // lidarLoc噪声 Vec6d lidar_odom_rel_noise_ = Vec6d::Zero(); // lidarOdom噪声 Vec6d dr_rel_noise_ = Vec6d::Zero(); // DR噪声 int lo_relative_constraints_num_ = 5; // LidarOdom相对位姿提供几个连续观测 int dr_relative_constraints_num_ = 5; // DR相对位姿提供几个连续观测 bool debug_ = true; // debug模式将打印更多信息 int invalid_lidar_num_ = 0; // 未启用 int valid_lidar_num_ = 0; // 未启用 bool is_in_map_ = false; // 未启用 bool gps2lidar_ = false; // 未启用 bool lidar2gps_ = false; // 未启用 /// 判定Lidar odom是否有效 bool lidar_odom_valid_ = true; bool lidar_odom_conflict_with_dr_ = false; // LO和DR是否有冲突 int lidar_odom_conflict_with_dr_cnt_ = 0; int lidar_odom_valid_cnt_ = 0; // 需要缓冲一个计时之后才算有效 std::ostringstream oss; // log相关 }; } // namespace lightning::loc ================================================ FILE: src/core/localization/pose_graph/pose_extrapolator.cc ================================================ #include "core/localization/pose_graph/pose_extrapolator.h" #include "core/lightning_math.hpp" #include namespace lightning::loc { PoseExtrapolator::PoseExtrapolator() {} PoseExtrapolator::~PoseExtrapolator() {} bool PoseExtrapolator::AddDRLocAndExtrapolate(const NavState& dr_loc, SE3& output_pose) { UL lock(data_mutex_); if (!initialized) return false; // 预测源不能回退 if (dr_loc.timestamp_ < current_time_) { return false; } // 常规处理流传1：根据DR增量，更新currentPose和currTime static std::shared_ptr last_dr_loc_; if (last_dr_loc_ == nullptr) { last_dr_loc_.reset(new NavState); } else { SE3 pose_incre = last_dr_loc_->GetPose().inverse() * dr_loc.GetPose(); current_pose_ *= pose_incre; } current_time_ = dr_loc.timestamp_; // 常规处理流传2：补偿pose（如有） if (pgo_compensate_trans_needed_) { const double delta_t = dr_loc.timestamp_ - last_dr_loc_->timestamp_; const double curr_dr_speed = (dr_loc.GetPose().translation() - last_dr_loc_->GetPose().translation()).norm() / delta_t; const double compensate_speed = std::max(kMinTransVelocity, 0.5 * curr_dr_speed); const double compensate_distance = compensate_speed * delta_t; if (compensate_distance > pgo_curr_trans_gap_.norm()) { current_pose_.translation() += pgo_curr_trans_gap_; pgo_compensate_trans_needed_ = false; } else { Vec3d compensate_trans = pgo_curr_trans_gap_.normalized() * compensate_distance; pgo_curr_trans_gap_ -= compensate_trans; current_pose_.translation() += compensate_trans; if ((pgo_curr_trans_gap_.array() < 0).any()) { current_pose_.translation() += pgo_curr_trans_gap_; pgo_curr_trans_gap_ = Vec3d{0, 0, 0}; pgo_compensate_trans_needed_ = false; } } LOG(INFO) << "Extrapolator: compensated translation for " << compensate_distance << "m."; } if (pgo_compensate_rot_needed_) { // } // 常规处理流传3：输出与更新 output_pose = current_pose_; *last_dr_loc_ = dr_loc; return true; } bool PoseExtrapolator::AddLidarOdomLoc(const NavState& lo_loc) { UL lock(data_mutex_); if (!initialized) { return false; } // 矫正源应该慢于预测源 if (lo_loc.timestamp_ > current_time_) { // } // return true; } bool PoseExtrapolator::AddPGOLoc(const LocalizationResult& pgo_loc) { UL lock(data_mutex_); // 系统只能由PGOLoc来初始化 if (!initialized) { current_time_ = pgo_loc.timestamp_; current_pose_ = pgo_loc.pose_; { NavState init_nav_state; init_nav_state.timestamp_ = current_time_; init_nav_state.SetPose(current_pose_); output_pose_queue_.push_back(init_nav_state); } last_pgo_loc_ = pgo_loc; initialized = true; return true; } // 时间戳回退的PGOLoc没有意义 if (pgo_loc.timestamp_ < last_pgo_loc_.timestamp_) { return false; } // PGO源正常要慢于预测源，如果快了，我们单独处理 if (pgo_loc.timestamp_ > current_time_) { current_time_ = pgo_loc.timestamp_; current_pose_ = pgo_loc.pose_; { NavState nav_state; nav_state.timestamp_ = current_time_; nav_state.SetPose(current_pose_); output_pose_queue_.push_back(nav_state); } last_pgo_loc_ = pgo_loc; return true; } // 以下为常规处理逻辑 // 当前PGOLoc在history队列上插值，重置补偿量。 SE3 interp_pose; NavState best_match; bool pgo_interp_success = math::PoseInterp( pgo_loc.timestamp_, output_pose_queue_, [](const NavState& nav_state) { return nav_state.timestamp_; }, [](const NavState& nav_state) { return nav_state.GetPose(); }, interp_pose, best_match); if (pgo_interp_success) { pgo_curr_trans_gap_ = pgo_loc.pose_.translation() - interp_pose.translation(); pgo_curr_rot_gap_ = (interp_pose.inverse() * pgo_loc.pose_).so3().log(); pgo_compensate_trans_needed_ = true; pgo_compensate_rot_needed_ = true; last_pgo_loc_ = pgo_loc; return true; } last_pgo_loc_ = pgo_loc; return false; } } // namespace lightning::loc ================================================ FILE: src/core/localization/pose_graph/pose_extrapolator.h ================================================ #ifndef TEAPROT_POSE_EXTRAPOLATOR_H_ #define TEAPROT_POSE_EXTRAPOLATOR_H_ #include #include "common/eigen_types.h" #include "common/nav_state.h" #include "common/std_types.h" #include "core/localization/localization_result.h" namespace lightning::loc { /** * High Frequency Pose Extrapolator * 高频位姿内插外推器，主要目的是外推。 * * 思路： * * 要求： * -- 多线程安全，外部只管访问 * -- IMU/或者DR频率【总之得高频】外推 * -- 外推策略要保守，尤其避免过快，不要造成定位回退？ * -- 互相校验，0.5s的误差百分比控制在x%内 */ class PoseExtrapolator { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW PoseExtrapolator(); ~PoseExtrapolator(); /// DR增量 + 内部补偿量 = 外推结果 bool AddDRLocAndExtrapolate(const NavState& dr_loc, SE3& output_pose); bool AddDRLocAndExtrapolate(const NavState& dr_loc, LocalizationResult& output_loc_res); /// 查询同一时刻的pose，给出补偿量（只补偿平移，是相对量） bool AddLidarOdomLoc(const NavState& lo_loc); /// 重置PGO视角下的补偿量（平移+旋转，map系下的绝对量）（第一次添加PGOLoc时启动内部逻辑） bool AddPGOLoc(const LocalizationResult& pgo_loc); bool is_initialized() { UL lock(data_mutex_); return initialized; } bool GetCurrentLoc(NavState& output_nav_state); private: std::mutex data_mutex_; // 全局数据锁 // 原始数据 NavState last_lo_loc_; LocalizationResult last_pgo_loc_; bool initialized = false; // 只能被PGOLoc初始化。 // 当前状态 double current_time_; SE3 current_pose_; // 历史上向外发布的pose std::deque output_pose_queue_; // 当前平移误差 // 当前旋转误差 bool pgo_compensate_trans_needed_ = false; bool pgo_compensate_rot_needed_ = false; Vec3d pgo_curr_trans_gap_; // local系 Vec3d pgo_curr_rot_gap_; // 轴角旋转 bool lo_compensation_needed_ = false; Vec3d lo_curr_trans_gap_; // 绝对系 Vec3d lo_curr_rot_gap_; // 轴角旋转 // 当前平移误差补偿速度（随DR更新，取DR平移速度的1/2；限定下限） // 当前旋转误差补偿速度（固定） double trans_velo(); // 标量速度 double rot_velo(); // 轴角标量速度 // 固定参数 const double kTimeForCompensate = 0.5; // 规定释放补偿所用的时间 const double kMinTransVelocity = 0.5; // 0.5米每秒，0.01米每20ms const double kMinRotVelocity = 0.0873; // 5度每秒，0.1度每20ms }; } // namespace lightning::loc #endif ================================================ FILE: src/core/localization/pose_graph/smoother.h ================================================ // // Created by xiang on 23-2-24. // #pragma once #include #include "common/eigen_types.h" #include "common/std_types.h" namespace lightning::loc { /** * 输出位姿的平滑器 */ class PoseSmoother { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** * 构造器 * @param factor 越接近于0,则越平滑（但滞后），越接近于1,则平滑效果越弱 */ explicit PoseSmoother(double factor = 0.3) { smooth_factor_ = factor; } PoseSmoother(const PoseSmoother&) = default; void PushDRPose(const SE3& dr_pose) { UL lock(data_mutex_); /// 检查dr pose和末尾那个pose之间的距离 if (!dr_queue_.empty()) { if ((dr_pose.translation() - dr_queue_.back().translation()).norm() >= smoother_dr_limit_trans_) { /// 无效 LOG(WARNING) << "smoother motion is too large: " << (dr_pose.translation() - dr_queue_.back().translation()).norm(); dr_queue_.clear(); motion_effective_ = false; return; } } motion_effective_ = true; dr_queue_.emplace_back(dr_pose); while (dr_queue_.size() > max_size_) { dr_queue_.pop_front(); } } void PushPose(const SE3& pose) { UL lock(data_mutex_); if (pose_queue_.size() < 3) { pose_queue_.push_back(pose); output_pose_ = pose; return; } int n = dr_queue_.size(); SE3 motion, pred; if (motion_effective_) { if (n >= 2) { motion = dr_queue_[n - 2].inverse() * dr_queue_[n - 1]; } else { motion = SE3(); } pred = pose_queue_.back() * motion; // 预测pose output_pose_.translation() = pred.translation() * (1.0 - smooth_factor_) + pose.translation() * smooth_factor_; Vec3d rot_err = (pred.so3().inverse() * pose.so3()).log(); output_pose_.so3() = pred.so3() * SO3::exp(rot_err * smooth_factor_); } else { pred = pose; output_pose_ = pose; } double dn = (output_pose_.translation() - pose.translation()).head<2>().norm(); if (dn > smoother_trans_limit_) { // 平滑器差别太大，直接跳过去并重置 LOG(WARNING) << "smoother is too large, jump to target: " << (output_pose_.translation() - pose.translation()).transpose() << ", given: " << pose.translation().transpose() << ", output:" << output_pose_.translation().transpose(); output_pose_ = pose; pose_queue_.clear(); return; } else if (dn > smoother_trans_limit2_) { // 快速收敛到正确位姿 LOG(INFO) << "smoother factor set to 0.2"; smooth_factor_ = 0.2; } else { // 回到默认值 smooth_factor_ = 0.01; } pose_queue_.emplace_back(output_pose_); while (pose_queue_.size() > max_size_) { pose_queue_.pop_front(); } motion_effective_ = false; } SE3 GetPose() const { return output_pose_; } void Reset() { UL lock(data_mutex_); pose_queue_.clear(); motion_effective_ = false; } private: std::mutex data_mutex_; bool motion_effective_ = true; // DR的运动是否有效 size_t max_size_ = 10; double smooth_factor_ = 0.1; // 平滑因子 double smoother_trans_limit_ = 5.0; // 平滑器从输入到输出允许的最大平移量 double smoother_trans_limit2_ = 2.0; // 平滑器从输入到输出允许的最大平移量 double smoother_dr_limit_trans_ = 0.3; // 平滑器允许的DR跳变量 SE3 output_pose_; std::deque pose_queue_; // 平滑之后的 std::deque dr_queue_; // 平滑之后的 }; } // namespace lightning::loc ================================================ FILE: src/core/loop_closing/loop_closing.cc ================================================ // // Created by xiang on 25-4-21. // #include "core/loop_closing/loop_closing.h" #include "common/keyframe.h" #include "common/loop_candidate.h" #include "utils/pointcloud_utils.h" #include #include #include "core/opti_algo/algo_select.h" #include "core/robust_kernel/cauchy.h" #include "core/types/edge_se3.h" #include "core/types/edge_se3_height_prior.h" #include "core/types/vertex_se3.h" #include "io/yaml_io.h" namespace lightning { LoopClosing::~LoopClosing() { if (options_.online_mode_) { kf_thread_.Quit(); } } void LoopClosing::Init(const std::string yaml_path) { /// setup miao miao::OptimizerConfig config(miao::AlgorithmType::LEVENBERG_MARQUARDT, miao::LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN, false); config.incremental_mode_ = true; optimizer_ = miao::SetupOptimizer<6, 3>(config); info_motion_.setIdentity(); info_motion_.block<3, 3>(0, 0) = Mat3d::Identity() * 1.0 / (options_.motion_trans_noise_ * options_.motion_trans_noise_); info_motion_.block<3, 3>(3, 3) = Mat3d::Identity() * 1.0 / (options_.motion_rot_noise_ * options_.motion_rot_noise_); info_loops_.setIdentity(); info_loops_.block<3, 3>(0, 0) = Mat3d::Identity() * 1.0 / (options_.loop_trans_noise_ * options_.loop_trans_noise_); info_loops_.block<3, 3>(3, 3) = Mat3d::Identity() * 1.0 / (options_.loop_rot_noise_ * options_.loop_rot_noise_); if (!yaml_path.empty()) { YAML_IO yaml(yaml_path); options_.loop_kf_gap_ = yaml.GetValue("loop_closing", "loop_kf_gap"); options_.min_id_interval_ = yaml.GetValue("loop_closing", "min_id_interval"); options_.closest_id_th_ = yaml.GetValue("loop_closing", "closest_id_th"); options_.max_range_ = yaml.GetValue("loop_closing", "max_range"); options_.ndt_score_th_ = yaml.GetValue("loop_closing", "ndt_score_th"); options_.with_height_ = yaml.GetValue("loop_closing", "with_height"); } if (options_.online_mode_) { LOG(INFO) << "loop closing module is running in online mode"; kf_thread_.SetProcFunc([this](Keyframe::Ptr kf) { HandleKF(kf); }); kf_thread_.SetName("handle loop closure"); kf_thread_.Start(); } } void LoopClosing::AddKF(Keyframe::Ptr kf) { if (options_.online_mode_) { kf_thread_.AddMessage(kf); } else { HandleKF(kf); } } void LoopClosing::HandleKF(Keyframe::Ptr kf) { if (kf == last_kf_) { return; } cur_kf_ = kf; all_keyframes_.emplace_back(kf); // 检测回环候选 DetectLoopCandidates(); if (options_.verbose_) { LOG(INFO) << "lc: get kf " << cur_kf_->GetID() << " candi: " << candidates_.size(); } // 计算回环位姿 ComputeLoopCandidates(); // 位姿图优化 PoseOptimization(); last_kf_ = kf; } void LoopClosing::DetectLoopCandidates() { candidates_.clear(); auto& kfs_mapping = all_keyframes_; Keyframe::Ptr check_first = nullptr; if (last_loop_kf_ == nullptr) { last_loop_kf_ = cur_kf_; return; } if (last_loop_kf_ && (cur_kf_->GetID() - last_loop_kf_->GetID()) <= options_.loop_kf_gap_) { LOG(INFO) << "skip because last loop kf: " << last_loop_kf_->GetID(); return; } for (auto kf : kfs_mapping) { if (check_first != nullptr && abs(int(kf->GetID() - check_first->GetID())) <= options_.min_id_interval_) { // 同条轨迹内，跳过一定的ID区间 continue; } if (abs(int(kf->GetID() - cur_kf_->GetID())) < options_.closest_id_th_) { /// 在同一条轨迹中，如果间隔太近，就不考虑回环 break; } Vec3d dt = kf->GetOptPose().translation() - cur_kf_->GetOptPose().translation(); double t2d = dt.head<2>().norm(); // x-y distance double range_th = options_.max_range_; if (t2d < range_th) { LoopCandidate c(kf->GetID(), cur_kf_->GetID()); c.Tij_ = kf->GetLIOPose().inverse() * cur_kf_->GetLIOPose(); candidates_.emplace_back(c); check_first = kf; } } if (!candidates_.empty()) { last_loop_kf_ = cur_kf_; } if (options_.verbose_ && !candidates_.empty()) { LOG(INFO) << "lc candi: " << candidates_.size(); } } void LoopClosing::ComputeLoopCandidates() { if (candidates_.empty()) { return; } // 执行计算 std::for_each(candidates_.begin(), candidates_.end(), [this](LoopCandidate& c) { ComputeForCandidate(c); }); // 保存成功的候选 std::vector succ_candidates; for (const auto& lc : candidates_) { // LOG(INFO) << "candi " << lc.idx1_ << ", " << lc.idx2_ << " s: " << lc.ndt_score_; if (lc.ndt_score_ > options_.ndt_score_th_) { succ_candidates.emplace_back(lc); } } if (options_.verbose_) { LOG(INFO) << "success: " << succ_candidates.size() << "/" << candidates_.size(); } candidates_.swap(succ_candidates); } void LoopClosing::ComputeForCandidate(lightning::LoopCandidate& c) { // LOG(INFO) << "aligning " << c.idx1_ << " with " << c.idx2_; const int submap_idx_range = 40; auto kf1 = all_keyframes_.at(c.idx1_), kf2 = all_keyframes_.at(c.idx2_); auto build_submap = [this](int given_id, bool build_in_world) -> CloudPtr { CloudPtr submap(new PointCloudType); for (int idx = -submap_idx_range; idx < submap_idx_range; idx += 4) { int id = idx + given_id; if (id < 0 || id >= all_keyframes_.size()) { continue; } auto kf = all_keyframes_[id]; CloudPtr cloud = kf->GetCloud(); // RemoveGround(cloud, 0.1); if (cloud->empty()) { continue; } // 转到世界系下 SE3 Twb = kf->GetOptPose(); if (!build_in_world) { Twb = all_keyframes_.at(given_id)->GetOptPose().inverse() * Twb; } CloudPtr cloud_trans(new PointCloudType); pcl::transformPointCloud(*cloud, *cloud_trans, Twb.matrix()); *submap += *cloud_trans; } return submap; }; auto submap_kf1 = build_submap(kf1->GetID(), true); CloudPtr submap_kf2 = kf2->GetCloud(); if (submap_kf1->empty() || submap_kf2->empty()) { c.ndt_score_ = 0; return; } Mat4f Tw2 = kf2->GetOptPose().matrix().cast(); /// 不同分辨率下的匹配 CloudPtr output(new PointCloudType); std::vector res{10.0, 5.0, 2.0, 1.0}; CloudPtr rough_map1, rough_map2; for (auto& r : res) { pcl::NormalDistributionsTransform ndt; ndt.setTransformationEpsilon(0.05); ndt.setStepSize(0.7); ndt.setMaximumIterations(40); ndt.setResolution(r); rough_map1 = VoxelGrid(submap_kf1, r * 0.1); rough_map2 = VoxelGrid(submap_kf2, r * 0.1); ndt.setInputTarget(rough_map1); ndt.setInputSource(rough_map2); ndt.align(*output, Tw2); Tw2 = ndt.getFinalTransformation(); c.ndt_score_ = ndt.getTransformationProbability(); } Mat4d T = Tw2.cast(); Quatd q(T.block<3, 3>(0, 0)); q.normalize(); Vec3d t = T.block<3, 1>(0, 3); c.Tij_ = kf1->GetOptPose().inverse() * SE3(q, t); // pcl::io::savePCDFileBinaryCompressed( // "./data/lc_" + std::to_string(c.idx1_) + "_" + std::to_string(c.idx2_) + "_out.pcd", *output); // pcl::io::savePCDFileBinaryCompressed( // "./data/lc_" + std::to_string(c.idx1_) + "_" + std::to_string(c.idx2_) + "_tgt.pcd", *rough_map1); } void LoopClosing::PoseOptimization() { auto v = std::make_shared(); v->SetId(cur_kf_->GetID()); v->SetEstimate(cur_kf_->GetOptPose()); optimizer_->AddVertex(v); kf_vert_.emplace_back(v); /// 上一个关键帧的运动约束 for (int i = 1; i < 3; i++) { int id = cur_kf_->GetID() - i; if (id >= 0) { auto last_kf = all_keyframes_[id]; auto e = std::make_shared(); e->SetVertex(0, optimizer_->GetVertex(last_kf->GetID())); e->SetVertex(1, v); SE3 motion = last_kf->GetLIOPose().inverse() * cur_kf_->GetLIOPose(); e->SetMeasurement(motion); e->SetInformation(info_motion_); optimizer_->AddEdge(e); } } if (options_.with_height_) { /// 高度约束 auto e = std::make_shared(); e->SetVertex(0, v); e->SetMeasurement(0); e->SetInformation(Mat1d::Identity() * 1.0 / (options_.height_noise_ * options_.height_noise_)); optimizer_->AddEdge(e); } /// 回环的约束 for (auto& c : candidates_) { auto e = std::make_shared(); e->SetVertex(0, optimizer_->GetVertex(c.idx1_)); e->SetVertex(1, optimizer_->GetVertex(c.idx2_)); e->SetMeasurement(c.Tij_); e->SetInformation(info_loops_); auto rk = std::make_shared(); rk->SetDelta(options_.rk_loop_th_); e->SetRobustKernel(rk); optimizer_->AddEdge(e); edge_loops_.emplace_back(e); } if (optimizer_->GetEdges().empty()) { return; } if (candidates_.empty()) { return; } optimizer_->InitializeOptimization(); optimizer_->SetVerbose(false); optimizer_->Optimize(20); /// remove outliers int cnt_outliers = 0; for (auto& e : edge_loops_) { if (e->GetRobustKernel() == nullptr) { continue; } if (e->Chi2() > e->GetRobustKernel()->Delta()) { e->SetLevel(1); cnt_outliers++; } else { e->SetRobustKernel(nullptr); } } if (options_.verbose_) { LOG(INFO) << "loop outliers: " << cnt_outliers << "/" << edge_loops_.size(); } /// get results for (auto& vert : kf_vert_) { SE3 pose = vert->Estimate(); all_keyframes_[vert->GetId()]->SetOptPose(pose); } if (loop_cb_) { loop_cb_(); } LOG(INFO) << "optimize finished, loops: " << edge_loops_.size(); // LOG(INFO) << "lc: cur kf " << cur_kf_->GetID() << ", opt: " << cur_kf_->GetOptPose().translation().transpose() // << ", lio: " << cur_kf_->GetLIOPose().translation().transpose(); } } // namespace lightning ================================================ FILE: src/core/loop_closing/loop_closing.h ================================================ // // Created by xiang on 25-4-21. // #ifndef LIGHTNING_LOOP_CLOSING_H #define LIGHTNING_LOOP_CLOSING_H #include "common/keyframe.h" #include "common/loop_candidate.h" #include "utils/async_message_process.h" #include "core/graph/optimizer.h" #include "core/types/edge_se3.h" namespace lightning { /** * 基于grid ndt的回环检测 */ class LoopClosing { public: struct Options { Options() {} bool verbose_ = true; // 输出调试信息 bool online_mode_ = false; // 切换离线-在线模式 int loop_kf_gap_ = 20; // 每隔多少个关键帧检查一次 int min_id_interval_ = 20; // 被检查的关键帧ID间隔 int closest_id_th_ = 50; // 历史关键帧与当前帧的ID间隔 double max_range_ = 30.0; // 候选帧的最大距离 double ndt_score_th_ = 1.0; // ndt位姿分值 /// 图优化权重 double motion_trans_noise_ = 0.1; // 位移权重 double motion_rot_noise_ = 3.0 * M_PI / 180.0; // 旋转权重 double loop_trans_noise_ = 0.2; // 位移权重 double loop_rot_noise_ = 3.0 * M_PI / 180.0; // 旋转权重 double rk_loop_th_ = 5.2 / 5; // 回环的RK阈值 bool with_height_ = true; double height_noise_ = 0.1; }; LoopClosing(Options options = Options()) { options_ = options; } ~LoopClosing(); void Init(const std::string yaml_path); /// 向回环中添加一个关键帧 void AddKF(Keyframe::Ptr kf); /// 如果检测到新地回环并发生了优化，则调用回调 using LoopClosedCallback = std::function; void SetLoopClosedCB(LoopClosedCallback cb) { loop_cb_ = cb; } protected: void HandleKF(Keyframe::Ptr kf); void DetectLoopCandidates(); /// 计算回环候选位姿 void ComputeLoopCandidates(); /// 计算单个回环候选 void ComputeForCandidate(LoopCandidate& c); /// 优化位姿 void PoseOptimization(); Options options_; Keyframe::Ptr last_kf_ = nullptr; Keyframe::Ptr last_loop_kf_ = nullptr; Keyframe::Ptr cur_kf_ = nullptr; std::vector all_keyframes_; std::vector candidates_; AsyncMessageProcess kf_thread_; std::shared_ptr optimizer_ = nullptr; Mat6d info_motion_ = Mat6d::Identity(); // 关键帧间的运动信息阵 Mat6d info_loops_ = Mat6d::Identity(); // 回环帧的信息矩阵 std::vector> kf_vert_; std::vector> edge_loops_; LoopClosedCallback loop_cb_; }; } // namespace lightning #endif // LIGHTNING_LOOP_CLOSING_H ================================================ FILE: src/core/maps/tiled_map.cc ================================================ // // Created by xiang on 23-2-7. // #include "core/maps/tiled_map.h" #include "io/file_io.h" #include #include namespace lightning { bool TiledMap::ConvertFromFullPCD(CloudPtr map, const SE3& start_pose, const std::string& map_path) { origin_.setZero(); assert(map != nullptr && !map->empty()); options_.map_path_ = map_path; func_points_.emplace_back(FunctionalPoint("start", start_pose)); chunk_id_ = 0; for (const auto& pt : map->points) { Vec2i grid = Pos2Grid(math::ToEigen(pt)); auto iter = static_chunks_.find(grid); if (iter != static_chunks_.end()) { iter->second->AddPoint(pt); } else { int id = chunk_id_; auto new_chunk = std::make_shared(id, grid, ""); static_chunks_.emplace(grid, new_chunk); id_to_grid_.emplace(id, grid); chunk_id_++; } } SaveToBin(false); return true; } void TiledMap::SaveToBin(bool only_dynamic) { LOG(INFO) << "map is saved to " << options_.map_path_ << ", sz: " << static_chunks_.size(); if (!only_dynamic) { /// 原点+索引 std::ofstream fout(options_.map_path_ + "/index.txt"); fout << std::setprecision(18) << origin_[0] << " " << origin_[1] << " " << origin_[2] << std::endl; for (auto& cp : static_chunks_) { if (cp.second->cloud_->empty()) { continue; } std::string filename = options_.map_path_ + "/" + std::to_string(cp.second->id_) + ".pcd"; pcl::io::savePCDFileBinaryCompressed(filename, *math::VoxelGrid(cp.second->cloud_, options_.voxel_size_in_chunk_)); fout << cp.second->id_ << " " << cp.first[0] << " " << cp.first[1] << " " << filename << std::endl; } /// functional points fout << "# functional points" << std::endl; for (const auto& fp : func_points_) { auto t = fp.pose_.translation(); auto q = fp.pose_.unit_quaternion(); fout << fp.name_ << " " << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl; } return; } for (auto& cp : dynamic_chunks_) { /// 只存储动态图层 /// 如果同一栅格还存在动态图层，应该把动态图层也存下来 if (cp.second->cloud_ != nullptr && !cp.second->cloud_->empty()) { std::string filename = options_.map_path_ + "/" + std::to_string(cp.second->id_) + "_dyn.pcd"; cp.second->cloud_->width = cp.second->cloud_->size(); pcl::io::savePCDFileBinaryCompressed(filename, *math::VoxelGrid(cp.second->cloud_, options_.voxel_size_in_chunk_)); } } } bool TiledMap::LoadMapIndex() { std::ifstream fin(options_.map_path_ + "/index.txt"); if (!fin) { LOG(ERROR) << "cannot load map index from: " << options_.map_path_; return false; } LOG(INFO) << "loading maps"; ClearMap(); bool first_line = true; bool reading_fp = false; UL lock(static_data_mutex_); UL lock2(dynamic_data_mutex_); while (!fin.eof()) { std::string line; std::getline(fin, line); if (line.empty()) { continue; } std::stringstream ss; ss << line; if (first_line) { // 读地图原点 ss >> origin_[0] >> origin_[1] >> origin_[2]; first_line = false; continue; } if (line == "# functional points") { reading_fp = true; continue; } if (reading_fp) { /// 读功能点 std::string name; double data[7]; ss >> name; for (int i = 0; i < 7; ++i) { ss >> data[i]; } func_points_.emplace_back(name, SE3(Quatd(data[6], data[3], data[4], data[5]), Vec3d(data[0], data[1], data[2]))); } else { /// 读地图区块 int id; Vec2i grid; std::string path; ss >> id >> grid[0] >> grid[1] >> path; // 将path改为yaml中配置的路径 path = options_.map_path_ + "/" + std::to_string(id) + ".pcd"; auto ck = std::make_shared(id, grid, path); ck->LoadCloud(); static_chunks_.emplace(grid, ck); id_to_grid_.emplace(id, grid); if (id > chunk_id_) { chunk_id_ = id; } if (options_.policy_ == DynamicCloudPolicy::PERSISTENT) { // 动态图层策略为永久时，从硬盘中读取动态区域信息 // 看是否有该栅格的动态图层 std::string dyn_filename = options_.map_path_ + "/" + std::to_string(id) + "_dyn.pcd"; if (PathExists(dyn_filename)) { LOG(INFO) << "load dynamic chunk: " << dyn_filename; auto dyn_chunk = std::make_shared(id, grid, dyn_filename); dynamic_chunks_.emplace(grid, dyn_chunk); if (options_.load_dyn_cloud_) { /// 从硬盘中读点云 dyn_chunk->LoadCloud(); } } } } } LOG(INFO) << "loaded chunks: " << static_chunks_.size() << ", fps: " << func_points_.size(); fin.close(); // 尝试载入动态区域文件 if (!options_.enable_dynamic_polygon_) { return true; } fin.open(options_.map_path_ + "/dynamic_polygon.txt"); if (!fin) { options_.enable_dynamic_polygon_ = false; LOG(ERROR) << "找不到动态区域文件，不使用动态区域多边形："; return true; } while (!fin.eof()) { std::string line; std::getline(fin, line); if (line.empty()) { continue; } std::stringstream ss; ss << line; int id; Vec2d pos; std::string path; ss >> id >> pos[0] >> pos[1]; // 减原点 pos[0] -= origin_[0]; pos[1] -= origin_[1]; auto iter = dynamic_polygon_.find(id); if (iter == dynamic_polygon_.end()) { dynamic_polygon_.emplace(id, DynamicPolygon{id, pos}); } else { iter->second.polygon_.emplace_back(cv::Point2f(pos[0], pos[1])); } } LOG(INFO) << "dynamic polygons: " << dynamic_polygon_.size(); chunk_id_++; return true; } void TiledMap::ClearMap() { UL lock(static_data_mutex_); UL lock2(dynamic_data_mutex_); static_chunks_.clear(); dynamic_chunks_.clear(); origin_.setZero(); } void TiledMap::AddStaticCloud(CloudPtr cloud) { std::set> active_voxels; // 记录哪些voxel被更新 for (const auto& p : cloud->points) { auto pt = ToVec3f(p); auto key = (pt * ndt_map_options_.inv_voxel_size_).cast(); auto iter = static_grids_.find(key); if (iter == static_grids_.end()) { // 栅格不存在 static_grids_.insert({key, VoxelData(pt)}); } else { // 栅格存在，添加点，更新缓存 iter->second.AddPoint(pt); } active_voxels.emplace(key); } // 更新active_voxels std::for_each(active_voxels.begin(), active_voxels.end(), [this](const auto& key) { UpdateVoxel(static_grids_[key]); }); flag_first_static_scan_ = false; } void TiledMap::AddDynamicCloud(CloudPtr cloud) { std::set> active_voxels; // 记录哪些voxel被更新 for (const auto& p : cloud->points) { auto pt = ToVec3f(p); auto key = (pt * ndt_map_options_.inv_voxel_size_).cast(); auto iter = dynamic_grids_.find(key); if (iter == dynamic_grids_.end()) { // 栅格不存在 dynamic_grids_.insert({key, VoxelData(pt)}); } else { // 栅格存在，添加点，更新缓存 iter->second.AddPoint(pt); } active_voxels.emplace(key); } // 更新active_voxels std::for_each(active_voxels.begin(), active_voxels.end(), [this](const auto& key) { UpdateVoxel(dynamic_grids_[key]); }); flag_first_dynamic_scan_ = false; } void TiledMap::LoadOnPose(const SE3& pose) { Vec2d p = pose.translation().head<2>(); auto this_grid = Pos2Grid(p); if (last_load_grid_set_ && last_load_grid_ == this_grid) { map_updated_ = false; return; } // 加载近邻范围内的地图 UL lock(static_data_mutex_); UL lock2(dynamic_data_mutex_); for (const auto& cp : static_chunks_) { Vec2i d = cp.first - this_grid; int n = abs(d[0]) + abs(d[1]); if (n <= options_.load_map_size_) { if (loaded_chunks_.find(cp.first) == loaded_chunks_.end()) { /// 载入静态地图 map_updated_ = true; if (cp.second->cloud_ == nullptr) { cp.second->LoadCloud(); // 载入点云 } loaded_chunks_.emplace(cp.first); } auto dyn_iter = dynamic_chunks_.find(cp.first); if (dyn_iter == dynamic_chunks_.end()) { // 创建这个动态区块 auto new_chunk = std::make_shared( cp.second->id_, cp.first, options_.map_path_ + "/" + std::to_string(cp.second->id_) + "_dyn.pcd"); dynamic_chunks_.emplace(cp.first, new_chunk); dynamic_map_updated_ = true; } else { /// 如果该区块已经被卸载，那么重新读取该区块点云 if (!dyn_iter->second->loaded_) { dyn_iter->second->LoadCloud(); } } } } // LOG(INFO) << "static_grids_ size is: " << static_grids_.size() // << ", dynamic_grids_ size is: " << dynamic_grids_.size(); // 卸载过远的点云 for (auto iter = loaded_chunks_.begin(); iter != loaded_chunks_.end();) { Vec2i g = *iter; Vec2i d = g - this_grid; int n = abs(d[0]) + abs(d[1]); if (n > options_.unload_map_size_) { /// 卸载静态地图 map_updated_ = true; // static_chunks_[g]->Unload(); iter = loaded_chunks_.erase(iter); auto d = dynamic_chunks_.find(g); if (options_.policy_ == DynamicCloudPolicy::SHORT && d != dynamic_chunks_.end()) { dynamic_chunks_[g]->cloud_ = nullptr; dynamic_map_updated_ = true; } if (options_.policy_ == DynamicCloudPolicy::PERSISTENT && d != dynamic_chunks_.end() && d->second->loaded_) { if (d->second->cloud_ == nullptr || d->second->cloud_->empty()) { continue; } // 将本区块存盘并卸载点云 if (options_.save_dyn_when_unload_) { std::string filename = options_.map_path_ + "/" + std::to_string(d->second->id_) + "_dyn.pcd"; d->second->cloud_->width = d->second->cloud_->size(); pcl::io::savePCDFileBinaryCompressed(filename, *d->second->cloud_); } if (options_.delete_when_unload_) { d->second->Unload(); } } } else { iter++; } } last_load_grid_ = this_grid; last_load_grid_set_ = true; } CloudPtr TiledMap::GetAllMap() { CloudPtr cloud(new PointCloudType); cloud->reserve(100000 * 25); { UL lock(static_data_mutex_); for (auto& idx : loaded_chunks_) { *cloud += *static_chunks_[idx]->cloud_; } } { UL lock(dynamic_data_mutex_); for (auto& idx : loaded_chunks_) { if (dynamic_chunks_.find(idx) != dynamic_chunks_.end()) { if (dynamic_chunks_[idx]->cloud_ != nullptr) { *cloud += *dynamic_chunks_[idx]->cloud_; } } } } return cloud; } std::map TiledMap::GetStaticCloud() { std::map cloud; UL lock(static_data_mutex_); for (auto& idx : loaded_chunks_) { CloudPtr c(new PointCloudType); *c += *static_chunks_[idx]->cloud_; cloud.emplace(static_chunks_[idx]->id_, c); } return cloud; } std::map TiledMap::GetDynamicCloud() { std::map cloud; UL lock(dynamic_data_mutex_); for (auto& idx : loaded_chunks_) { if (dynamic_chunks_.find(idx) != dynamic_chunks_.end()) { if (dynamic_chunks_[idx]->cloud_ != nullptr) { /// 也可能没有这个块 CloudPtr c(new PointCloudType); *c = *dynamic_chunks_[idx]->cloud_; // needs deep copy cloud.emplace(dynamic_chunks_[idx]->id_, c); } } } return cloud; } TiledMap::GridHashMap TiledMap::GetStaticGridMap() { UL lock(static_data_mutex_); return static_grids_; } TiledMap::GridHashMap TiledMap::GetDynamicGridMap() { UL lock(dynamic_data_mutex_); return dynamic_grids_; } void TiledMap::ResetDynamicCloud() { UL lock(dynamic_data_mutex_); dynamic_chunks_.clear(); dynamic_chunks_.reserve(0); dynamic_grids_.clear(); dynamic_grids_.reserve(0); if (id_to_grid_.empty()) { LOG(WARNING) << "重置动态图层时，原始地图索引为空"; return; } if (options_.policy_ == DynamicCloudPolicy::PERSISTENT) { // 沿用静态图层索引 int init_dy_size = 0; for (auto it = id_to_grid_.begin(); it != id_to_grid_.end(); it++) { int id = it->first; Vec2i grid = it->second; std::string dyn_filename = options_.map_path_ + "/" + std::to_string(id) + "_dyn.pcd"; if (PathExists(dyn_filename)) { LOG(INFO) << "reset dynamic chunk: " << dyn_filename; auto dyn_chunk = std::make_shared(id, grid, dyn_filename); dynamic_chunks_.emplace(grid, dyn_chunk); if (options_.load_dyn_cloud_) { /// 从硬盘中读点云 dyn_chunk->LoadCloud(); init_dy_size++; } } } if (init_dy_size > 0) { LOG(INFO) << "reset dynamic map success, load size: " << init_dy_size; } } return; } void TiledMap::UpdateDynamicCloud(CloudPtr cloud_world, bool remove_old) { std::set> updated_grids; // 被更新的chunks UL lock(dynamic_data_mutex_); for (auto& pt : cloud_world->points) { if (FallsInDynamicArea(pt.getVector3fMap()) < 0) { continue; } dynamic_map_updated_ = true; // 在动态区域增加点云 Vec2i grid = Pos2Grid(math::ToEigen(pt)); auto iter = dynamic_chunks_.find(grid); auto iter_static = static_chunks_.find(grid); if (iter != dynamic_chunks_.end()) { /// 该区块已经存在，则加入这个区块的点云 if (!iter->second->loaded_) { iter->second->LoadCloud(); continue; } if (!remove_old && iter->second->cloud_->size() >= options_.max_pts_in_dyn_chunk_) { /// 若不移除旧区域点，则满了之后也不需要加 continue; } iter->second->AddPoint(pt); updated_grids.emplace(grid); } else { int id = 0; if (iter_static != static_chunks_.end()) { if (options_.policy_ == DynamicCloudPolicy::SHORT) { // 要求该点至少不在unload范围内 Vec2i d = last_load_grid_ - grid; int n = abs(d[0]) + abs(d[1]); if (n > options_.unload_map_size_) { continue; } } // 使用和static相同的id id = iter_static->second->id_; auto new_chunk = std::make_shared(id, grid, options_.map_path_ + "/" + std::to_string(id) + "_dyn.pcd"); new_chunk->AddPoint(pt); dynamic_chunks_.emplace(grid, new_chunk); updated_grids.emplace(grid); } else { // statics当中并不存在，目前不会创建动态图层 } } } CloudPtr cloud(new PointCloudType); for (const auto& g : updated_grids) { if (dynamic_chunks_[g]->cloud_->size() > 1.5 * options_.max_pts_in_dyn_chunk_) { dynamic_chunks_[g]->cloud_ = math::VoxelGrid(dynamic_chunks_[g]->cloud_, 0.5); } size_t full_size = dynamic_chunks_[g]->cloud_->size(); if (remove_old && full_size > options_.max_pts_in_dyn_chunk_) { // 先随机截取掉一部分点云，非全部清空 dynamic_chunks_[g]->cloud_->points.erase( dynamic_chunks_[g]->cloud_->points.begin(), dynamic_chunks_[g]->cloud_->points.begin() + size_t(0.7 * full_size)); dynamic_chunks_[g]->cloud_->width = full_size - size_t(0.7 * full_size); dynamic_chunks_[g]->cloud_->points.shrink_to_fit(); } } } void TiledMap::UpdateVoxel(VoxelData& v, bool flag_first_scan) { bool flag_first_scan_ = flag_first_scan; if (flag_first_scan_) { if (v.pts_.size() > 1) { math::ComputeMeanAndCov(v.pts_, v.mu_, v.sigma_, [this](const Vec3f& p) { return p; }); v.info_ = (v.sigma_ + Mat3f::Identity() * 1e-3).inverse(); // 避免出nan } else { v.mu_ = v.pts_[0]; v.info_ = Mat3f::Identity() * 1e2; } v.ndt_estimated_ = true; v.pts_.clear(); return; } if (v.ndt_estimated_ && v.num_pts_ > ndt_map_options_.max_pts_in_voxel_) { return; } if (!v.ndt_estimated_ && v.pts_.size() > ndt_map_options_.min_pts_in_voxel_) { // 新增的voxel math::ComputeMeanAndCov(v.pts_, v.mu_, v.sigma_, [this](const Vec3f& p) { return p; }); v.info_ = (v.sigma_ + Mat3f::Identity() * 1e-3).inverse(); // 避免出nan v.ndt_estimated_ = true; v.pts_.clear(); } else if (v.ndt_estimated_ && v.pts_.size() > ndt_map_options_.min_pts_in_voxel_) { // 已经估计，而且还有新来的点 Vec3f cur_mu, new_mu; Mat3f cur_var, new_var; math::ComputeMeanAndCov(v.pts_, cur_mu, cur_var, [this](const Vec3f& p) { return p; }); math::UpdateMeanAndCov(v.num_pts_, v.pts_.size(), v.mu_, v.sigma_, cur_mu, cur_var, new_mu, new_var); v.mu_ = new_mu; v.sigma_ = new_var; v.num_pts_ += v.pts_.size(); v.pts_.clear(); // SVD 检查最大与最小奇异值，限制最小奇异值 Eigen::JacobiSVD svd(v.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV); Vec3f lambda = svd.singularValues(); for (int i = 1; i < 3; ++i) { if (lambda[i] < lambda[0] * 1e-3) { lambda[i] = lambda[0] * 1e-3; } } Mat3f inv_lambda = Vec3f(1.0 / lambda[0], 1.0 / lambda[1], 1.0 / lambda[2]).asDiagonal(); v.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose(); } } int TiledMap::FallsInDynamicArea(const Vec3f& pt) { if (!options_.enable_dynamic_polygon_) { return true; } for (auto& pp : dynamic_polygon_) { if (cv::pointPolygonTest(pp.second.polygon_, cv::Point2f(pt[0], pt[1]), false) > 0) { return pp.first; } } return -1; } } // namespace lightning ================================================ FILE: src/core/maps/tiled_map.h ================================================ // // Created by xiang on 23-2-7. // #ifndef LIGHNING_TILED_MAP_H #define LIGHNING_TILED_MAP_H #include #include #include #include #include "common/functional_points.h" #include "common/point_def.h" #include "common/std_types.h" #include "core/lightning_math.hpp" #include "core/maps/tiled_map_chunk.h" namespace lightning { /** * 包含动静态的点云 * * 静态地图从文件目录中读取（先切分，切分使用同一个接口） * 动态地图实时从lidar odom中构建 * * TODO: * - **done** 动态区域也需要分块，不然太大 * - 动态区域应该有生命周期和最大采样点（目前在option中设置） * - 动态图层的保存与读取 * - thread safety */ class TiledMap { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// 动态区域的处理策略 enum class DynamicCloudPolicy { /// 短时间保留，车辆移出该区域就会清空 SHORT = 0, /// 长时间保留在内存，车辆移出区域后仍然保留，但不会存盘并在下次开机时使用 LONG, /// 更长时间的保留，会存盘，下次运行也会读取，超过一定时间后才会清除 PERSISTENT, }; struct Options { Options() {} std::string map_path_ = "./data/maps/"; // 地图存储路径 float chunk_size_ = 100.0; // 单个chunk大小 float inv_chunk_size_ = 1.0 / 100.0; // 反向chunk size float voxel_size_in_chunk_ = 0.1; // chunk内部的分辨率 const int load_nearby_chunks_ = 8; // 载入邻近范围内的点云 bool save_dynamic_layer_ = false; // 是否将动态图层也存入地图文件夹 int load_map_size_ = 2; // 载入的邻近网格数 int unload_map_size_ = 3; // 卸载的网格数 bool enable_dynamic_polygon_ = false; // 是否使用动态区域的多边形 size_t max_pts_in_dyn_chunk_ = 50000; // 每个动态区块中最多点数 DynamicCloudPolicy policy_ = DynamicCloudPolicy::PERSISTENT; // 动态区域处理策略 bool delete_when_unload_ = false; // 卸载区域时是否清空点云 bool load_dyn_cloud_ = true; // 初始化时是否加载动态点云 bool save_dyn_when_quit_ = true; // 退出时是否保存动态点云 bool save_dyn_when_unload_ = true; // 卸载时是否保存动态点云 }; /// 动态区域的多边形 struct DynamicPolygon { DynamicPolygon() {} DynamicPolygon(int id, const Vec2d& pt) : id_(id) { polygon_.emplace_back(cv::Point2f(pt[0], pt[1])); } int id_ = 0; std::vector polygon_; }; enum class NearbyType { CENTER, NEARBY6, }; struct NdtMapOptions { NdtMapOptions() {} float static_cloud_weight_ = 1.0; // 静态点云权重 float dynamic_cloud_weight_ = 0.1; // 动态点云权重 NearbyType nearby_type_ = NearbyType::CENTER; // 邻近类型 float voxel_size_ = 1.0; // voxel大小 int max_iteration_ = 5; // 最大迭代次数 double eps_ = 1e-2; // 收敛判定条件 double res_outlier_th_ = 50.0; // 异常值拒绝阈值 float inv_voxel_size_ = 1.0; // 栅格尺寸倒数 int min_pts_in_voxel_ = 3; // 每个栅格中最小点数 int min_effective_pts_ = 500; // 最小有效点数 size_t capacity_ = 100000; // 缓存的体素数量 int max_pts_in_voxel_ = 50; // 每个栅格中最大点数 bool verbose_ = false; }; using KeyType = Eigen::Matrix; // 体素的索引 struct VoxelData { VoxelData() {} VoxelData(size_t id) { idx_.emplace_back(id); } VoxelData(const Vec3f& pt) { pts_.emplace_back(pt); num_pts_ = 1; } void AddPoint(const Vec3f& pt) { pts_.emplace_back(pt); if (!ndt_estimated_) { num_pts_++; } } std::vector pts_; // 内部点，多于一定数量之后再估计均值和协方差 std::vector idx_; // 点云中点的索引 Vec3f mu_ = Vec3f::Zero(); // 均值 Mat3f sigma_ = Mat3f::Identity(); // 协方差 Mat3f info_ = Mat3f::Zero(); // 协方差之逆 bool ndt_estimated_ = false; // NDT是否已经估计 int num_pts_ = 0; // 总共的点数，用于更新估计 }; using GridHashMap = std::unordered_map>; using ChunkHashMap = std::unordered_map, math::hash_vec<2>>; TiledMap(Options options = Options()) : options_(options) {} /** * 从单个PCD进行转换 * @param map PCD地图 * @param start_pose 地图起点 * @return */ bool ConvertFromFullPCD(CloudPtr map, const SE3& start_pose, const std::string& map_path = "./data/maps/"); /// 载入地图索引文件 bool LoadMapIndex(); /// 载入某个位置附近的点云 void LoadOnPose(const SE3& pose); // /// 载入某个位置附近的点云 // void LoadOnPose(const SE3& pose, bool wndt = true); /// 获取当前载入的静态点云（分块形式） /// 现在会调用深拷贝 std::map GetStaticCloud(); /// 获取当前载入的动态点云（分块形式） /// 现在会调用深拷贝 std::map GetDynamicCloud(); /// 获取拼接好的地图 CloudPtr GetAllMap(); /** * 使用最近配准之后的scan更新动态图层 * @param cloud_world 世界坐标系下点云 * @param remove_old_pts 是否会移除历史的数据 */ void UpdateDynamicCloud(CloudPtr cloud_world, bool remove_old_pts = true); /// 重置动态图层 void ResetDynamicCloud(); /// accessors /// 获取地图原点 Vec3d GetOrigin() const { return origin_; } /// 添加地图功能点 void AddFP(const FunctionalPoint& fp) { func_points_.emplace_back(fp); } /// 获取所有功能点 std::vector GetAllFP() const { return func_points_; } int NumActiveChunks() const { return loaded_chunks_.size(); } bool MapUpdated() const { return map_updated_; } bool DynamicMapUpdated() const { return dynamic_map_updated_; } void CleanMapUpdate() { map_updated_ = false; dynamic_map_updated_ = false; } /// 更新静态点云地图 void AddStaticCloud(CloudPtr cloud); /// 更新动态点云地图 void AddDynamicCloud(CloudPtr cloud); /// 更新体素内部数据, 根据新加入的pts和历史的估计情况来确定自己的估计 void UpdateVoxel(VoxelData& v, bool flag_first_scan = true); // 获取静态NDT栅格地图 GridHashMap GetStaticGridMap(); // 获取动态NDT栅格地图 GridHashMap GetDynamicGridMap(); GridHashMap static_grids_; // 静态地图栅格 GridHashMap dynamic_grids_; // 动态地图栅格 std::map GetDynamicPolygons() const { return dynamic_polygon_; } /// 存储到二进制pcd文件 void SaveToBin(bool only_dynamic = false); template void SetNewTargetForNDT(T ndt) { bool has_cloud = false; { UL lock(static_data_mutex_); for (auto& idx : loaded_chunks_) { if (!static_chunks_[idx]->cloud_->empty()) { has_cloud = true; } ndt->AddTarget(static_chunks_[idx]->cloud_); } } { UL lock(dynamic_data_mutex_); for (auto& idx : loaded_chunks_) { if (dynamic_chunks_.find(idx) != dynamic_chunks_.end()) { if (dynamic_chunks_[idx]->cloud_ != nullptr) { has_cloud = true; ndt->AddTarget(dynamic_chunks_[idx]->cloud_); } } } } ndt->ComputeTargetGrids(); if (has_cloud) { ndt->initCompute(); } } /// 清理地图 void ClearMap(); private: /** * 测试某个点是否落在动态区域 * @param pt 给定点 * @return 动态区域ID */ int FallsInDynamicArea(const Vec3f& pt); private: /// 注意这里有个边长 inline Vec2i Pos2Grid(const Vec2f& pt) const { Vec2d p = ((pt.cast() * options_.inv_chunk_size_) + Vec2d(0.5, 0.5)); return Vec2i(floor(p[0]), floor(p[1])); } inline Vec2i Pos2Grid(const Vec2d& pt) const { Vec2d p = ((pt * options_.inv_chunk_size_) + Vec2d(0.5, 0.5)); return Vec2i(floor(p[0]), floor(p[1])); } Options options_; NdtMapOptions ndt_map_options_; std::mutex static_data_mutex_; // 静态数据锁 std::mutex dynamic_data_mutex_; // 动态数据锁 Vec3d origin_ = Vec3d::Zero(); ChunkHashMap static_chunks_; // 静态地图 ChunkHashMap dynamic_chunks_; // 动态地图 std::map id_to_grid_; // 从ID找grid int chunk_id_ = 0; // chunk生成时的ID Vec2i last_load_grid_ = Vec2i::Zero(); // 上次加载时的网格 bool last_load_grid_set_ = false; // 上次加载的flag std::set> loaded_chunks_; // 已经载入的区块，以网格ID为索引 bool map_updated_ = false; bool dynamic_map_updated_ = false; std::map dynamic_polygon_; // 动态区域，从txt中读取 std::vector func_points_; // 功能点 bool flag_first_dynamic_scan_ = true; // 首帧动态点云特殊处理 bool flag_first_static_scan_ = true; // 首帧静态点云特殊处理 }; } // namespace lightning #endif ================================================ FILE: src/core/maps/tiled_map_chunk.cc ================================================ // // Created by xiang on 23-2-7. // #include "tiled_map_chunk.h" #include namespace lightning { void MapChunk::AddPoint(const PointType& pt) { if (cloud_ == nullptr) { cloud_.reset(new PointCloudType); cloud_->reserve(50000); } cloud_->points.emplace_back(pt); } void MapChunk::LoadCloud() { if (cloud_ == nullptr) { cloud_.reset(new PointCloudType); } pcl::io::loadPCDFile(filename_, *cloud_); loaded_ = true; } void MapChunk::Unload() { cloud_ = nullptr; loaded_ = false; } } // namespace lightning ================================================ FILE: src/core/maps/tiled_map_chunk.h ================================================ // // Created by xiang on 23-2-7. // #ifndef LIGHTNING_TILED_MAP_CHUNK_H #define LIGHTNING_TILED_MAP_CHUNK_H #include "common/eigen_types.h" #include "common/point_def.h" namespace lightning { /** * 一个地图区块 * * TODO thread-safe */ struct MapChunk { EIGEN_MAKE_ALIGNED_OPERATOR_NEW MapChunk() = default; MapChunk(int id, Vec2i grid, std::string filename) : id_(id), grid_(std::move(grid)), filename_(std::move(filename)), cloud_(new PointCloudType()) { cloud_->reserve(50000); } ~MapChunk() = default; /** * 增加一个点 * @param pt */ void AddPoint(const PointType& pt); /// 加载地图 void LoadCloud(); /// 卸载本区块 void Unload(); int id_ = 0; // 该区块ID Vec2i grid_ = Vec2i::Zero(); // 网格中心 std::string filename_; // 地图文件名 bool loaded_ = false; // 该区域是否已经被载入 std::mutex data_mutex_; // 数据锁 CloudPtr cloud_ = nullptr; // 该区块的点云 }; } // namespace lightning #endif // TEAPORT_TILED_MAP_CHUNK_H ================================================ FILE: src/core/miao/CMakeLists.txt ================================================ add_subdirectory(core) # add_subdirectory(examples) add_subdirectory(utils) ================================================ FILE: src/core/miao/core/CMakeLists.txt ================================================ add_library(miao.core SHARED graph/graph.cc graph/optimizer.cc common/string_tools.cc math/marginal_covariance_cholesky.cc opti_algo/optimization_algorithm.cc opti_algo/opti_algo_gauss_newton.cc opti_algo/opti_algo_lm.cc opti_algo/opti_algo_dogleg.cc solver/solver.cc ) target_include_directories(miao.core PRIVATE ${PROJECT_SOURCE_DIR}/src/core/miao ) target_link_libraries( miao.core glog tbb ) ================================================ FILE: src/core/miao/core/common/config.h ================================================ // // Created by xiang on 24-6-4. // #ifndef MIAO_CONFIG_H #define MIAO_CONFIG_H namespace lightning::miao { /// 优化器使用的优化方法 enum class AlgorithmType { GAUSS_NEWTON = 0, LEVENBERG_MARQUARDT, DOGLEG, }; /// 线性方程的求解方法 enum class LinearSolverType { LINEAR_SOLVER_DENSE, // eigen 稠密求解器 LINEAR_SOLVER_SPARSE_EIGEN, // eigen 稀疏求解器 LINEAR_SOLVER_PCG, // PCG 求解器，快但是糙一些 /// 后面的还未移植，目前不是特别想做 // LINEAR_SOLVER_CSPARSE, // Csparse 求解器 // LINEAR_SOLVER_CHOLMOD, // Cholmod 求解器 }; /// 配置优化器 struct OptimizerConfig { OptimizerConfig() = default; explicit OptimizerConfig(AlgorithmType algo_type, LinearSolverType linear_type = LinearSolverType::LINEAR_SOLVER_DENSE, bool is_dense = true) : algo_type_(algo_type), is_dense_(is_dense), linear_solver_type_(linear_type) {} // 优化方法,从Gauss-newton, Levenberg-marquardt, dogleg中选 AlgorithmType algo_type_ = AlgorithmType::LEVENBERG_MARQUARDT; // 线性方程求解方法,从eigen, dense, pcg中选 LinearSolverType linear_solver_type_ = LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN; // 稀疏还是稠密 // 稠密问题不允许有marg // 稀疏问题可以有marg, 也可以没有marg bool is_dense_ = true; /// 增量模式下配置 /// ============================================================================== /// 增量模式可用于实时定位、实时pose graph等应用场景，但增量模式不适用于带Marg的顶点（因为Hessian index需要重排）， /// 所以增量适用于带各种约束下的pose graph /// NOTE: 目前增加模式不支持SetFixed的节点，因为setFixed会跳过fixed nodes，导致Hessian index重排 // 打开增量模式 bool incremental_mode_ = false; // 最大顶点数量限制，为-1时，不会限制最大vertex size,可用于在线slam模式下pose graph之类的应用 // 不为-1时，optimizer会用新的顶点替换掉最旧的顶点，保持block solver不变 int max_vertex_size_ = -1; // 并发策略 // 打开并发时，求解器会使用最快的并发进行求解 bool parallel_ = true; double eps_chi2_ = 1e-4; // 允许的退出chi2误差 }; } // namespace lightning::miao #endif // MIAO_CONFIG_H ================================================ FILE: src/core/miao/core/common/macros.h ================================================ // // Created by xiang on 24-3-19. // #ifndef MIAO_MACROS_H #define MIAO_MACROS_H namespace lightning::miao { /// 禁止拷贝与复制 #define DISALLOW_COPY(T) \ public: \ T(const T&) = delete; \ T& operator=(const T&) = delete; constexpr int invalid_id = -2; } // namespace lightning::miao #endif // MIAO_MACROS_H ================================================ FILE: src/core/miao/core/common/math.h ================================================ // // Created by xiang on 24-6-17. // #ifndef MIAO_MATH_H #define MIAO_MATH_H #include #include "common/eigen_types.h" namespace lightning::miao { inline int _q2m(double& S, double& qw, const double& r00, const double& r10, const double& r20, const double& r01, const double& r11, const double& r21, const double& r02, const double& r12, const double& r22) { double tr = r00 + r11 + r22; if (tr > 0) { S = sqrt(tr + 1.0) * 2; // S=4*qw qw = 0.25 * S; // qx = (r21 - r12) / S; // qy = (r02 - r20) / S; // qz = (r10 - r01) / S; return 0; } else if ((r00 > r11) & (r00 > r22)) { S = sqrt(1.0 + r00 - r11 - r22) * 2; // S=4*qx qw = (r21 - r12) / S; // qx = 0.25 * S; // qy = (r01 + r10) / S; // qz = (r02 + r20) / S; return 1; } else if (r11 > r22) { S = sqrt(1.0 + r11 - r00 - r22) * 2; // S=4*qy qw = (r02 - r20) / S; // qx = (r01 + r10) / S; // qy = 0.25 * S; return 2; } else { S = sqrt(1.0 + r22 - r00 - r11) * 2; // S=4*qz qw = (r10 - r01) / S; // qx = (r02 + r20) / S; // qy = (r12 + r21) / S; // qz = 0.25 * S; return 3; } } inline void compute_dq_dR_w(Eigen::Matrix& dq_dR_w, const double& qw, const double& r00, const double& r10, const double& r20, const double& r01, const double& r11, const double& r21, const double& r02, const double& r12, const double& r22) { (void)r00; (void)r11; (void)r22; double _aux1 = 1 / pow(qw, 3); double _aux2 = -0.03125 * (r21 - r12) * _aux1; double _aux3 = 1 / qw; double _aux4 = 0.25 * _aux3; double _aux5 = -0.25 * _aux3; double _aux6 = 0.03125 * (r20 - r02) * _aux1; double _aux7 = -0.03125 * (r10 - r01) * _aux1; dq_dR_w(0, 0) = _aux2; dq_dR_w(0, 1) = 0; dq_dR_w(0, 2) = 0; dq_dR_w(0, 3) = 0; dq_dR_w(0, 4) = _aux2; dq_dR_w(0, 5) = _aux4; dq_dR_w(0, 6) = 0; dq_dR_w(0, 7) = _aux5; dq_dR_w(0, 8) = _aux2; dq_dR_w(1, 0) = _aux6; dq_dR_w(1, 1) = 0; dq_dR_w(1, 2) = _aux5; dq_dR_w(1, 3) = 0; dq_dR_w(1, 4) = _aux6; dq_dR_w(1, 5) = 0; dq_dR_w(1, 6) = _aux4; dq_dR_w(1, 7) = 0; dq_dR_w(1, 8) = _aux6; dq_dR_w(2, 0) = _aux7; dq_dR_w(2, 1) = _aux4; dq_dR_w(2, 2) = 0; dq_dR_w(2, 3) = _aux5; dq_dR_w(2, 4) = _aux7; dq_dR_w(2, 5) = 0; dq_dR_w(2, 6) = 0; dq_dR_w(2, 7) = 0; dq_dR_w(2, 8) = _aux7; } inline void compute_dq_dR_x(Eigen::Matrix& dq_dR_x, const double& qx, const double& r00, const double& r10, const double& r20, const double& r01, const double& r11, const double& r21, const double& r02, const double& r12, const double& r22) { (void)r00; (void)r11; (void)r21; (void)r12; (void)r22; double _aux1 = 1 / qx; double _aux2 = -0.125 * _aux1; double _aux3 = 1 / pow(qx, 3); double _aux4 = r10 + r01; double _aux5 = 0.25 * _aux1; double _aux6 = 0.03125 * _aux3 * _aux4; double _aux7 = r20 + r02; double _aux8 = 0.03125 * _aux3 * _aux7; dq_dR_x(0, 0) = 0.125 * _aux1; dq_dR_x(0, 1) = 0; dq_dR_x(0, 2) = 0; dq_dR_x(0, 3) = 0; dq_dR_x(0, 4) = _aux2; dq_dR_x(0, 5) = 0; dq_dR_x(0, 6) = 0; dq_dR_x(0, 7) = 0; dq_dR_x(0, 8) = _aux2; dq_dR_x(1, 0) = -0.03125 * _aux3 * _aux4; dq_dR_x(1, 1) = _aux5; dq_dR_x(1, 2) = 0; dq_dR_x(1, 3) = _aux5; dq_dR_x(1, 4) = _aux6; dq_dR_x(1, 5) = 0; dq_dR_x(1, 6) = 0; dq_dR_x(1, 7) = 0; dq_dR_x(1, 8) = _aux6; dq_dR_x(2, 0) = -0.03125 * _aux3 * _aux7; dq_dR_x(2, 1) = 0; dq_dR_x(2, 2) = _aux5; dq_dR_x(2, 3) = 0; dq_dR_x(2, 4) = _aux8; dq_dR_x(2, 5) = 0; dq_dR_x(2, 6) = _aux5; dq_dR_x(2, 7) = 0; dq_dR_x(2, 8) = _aux8; } inline void compute_dq_dR_y(Eigen::Matrix& dq_dR_y, const double& qy, const double& r00, const double& r10, const double& r20, const double& r01, const double& r11, const double& r21, const double& r02, const double& r12, const double& r22) { (void)r00; (void)r20; (void)r11; (void)r02; (void)r22; double _aux1 = 1 / pow(qy, 3); double _aux2 = r10 + r01; double _aux3 = 0.03125 * _aux1 * _aux2; double _aux4 = 1 / qy; double _aux5 = 0.25 * _aux4; double _aux6 = -0.125 * _aux4; double _aux7 = r21 + r12; double _aux8 = 0.03125 * _aux1 * _aux7; dq_dR_y(0, 0) = _aux3; dq_dR_y(0, 1) = _aux5; dq_dR_y(0, 2) = 0; dq_dR_y(0, 3) = _aux5; dq_dR_y(0, 4) = -0.03125 * _aux1 * _aux2; dq_dR_y(0, 5) = 0; dq_dR_y(0, 6) = 0; dq_dR_y(0, 7) = 0; dq_dR_y(0, 8) = _aux3; dq_dR_y(1, 0) = _aux6; dq_dR_y(1, 1) = 0; dq_dR_y(1, 2) = 0; dq_dR_y(1, 3) = 0; dq_dR_y(1, 4) = 0.125 * _aux4; dq_dR_y(1, 5) = 0; dq_dR_y(1, 6) = 0; dq_dR_y(1, 7) = 0; dq_dR_y(1, 8) = _aux6; dq_dR_y(2, 0) = _aux8; dq_dR_y(2, 1) = 0; dq_dR_y(2, 2) = 0; dq_dR_y(2, 3) = 0; dq_dR_y(2, 4) = -0.03125 * _aux1 * _aux7; dq_dR_y(2, 5) = _aux5; dq_dR_y(2, 6) = 0; dq_dR_y(2, 7) = _aux5; dq_dR_y(2, 8) = _aux8; } inline void compute_dq_dR_z(Eigen::Matrix& dq_dR_z, const double& qz, const double& r00, const double& r10, const double& r20, const double& r01, const double& r11, const double& r21, const double& r02, const double& r12, const double& r22) { (void)r00; (void)r10; (void)r01; (void)r11; (void)r22; double _aux1 = 1 / pow(qz, 3); double _aux2 = r20 + r02; double _aux3 = 0.03125 * _aux1 * _aux2; double _aux4 = 1 / qz; double _aux5 = 0.25 * _aux4; double _aux6 = r21 + r12; double _aux7 = 0.03125 * _aux1 * _aux6; double _aux8 = -0.125 * _aux4; dq_dR_z(0, 0) = _aux3; dq_dR_z(0, 1) = 0; dq_dR_z(0, 2) = _aux5; dq_dR_z(0, 3) = 0; dq_dR_z(0, 4) = _aux3; dq_dR_z(0, 5) = 0; dq_dR_z(0, 6) = _aux5; dq_dR_z(0, 7) = 0; dq_dR_z(0, 8) = -0.03125 * _aux1 * _aux2; dq_dR_z(1, 0) = _aux7; dq_dR_z(1, 1) = 0; dq_dR_z(1, 2) = 0; dq_dR_z(1, 3) = 0; dq_dR_z(1, 4) = _aux7; dq_dR_z(1, 5) = _aux5; dq_dR_z(1, 6) = 0; dq_dR_z(1, 7) = _aux5; dq_dR_z(1, 8) = -0.03125 * _aux1 * _aux6; dq_dR_z(2, 0) = _aux8; dq_dR_z(2, 1) = 0; dq_dR_z(2, 2) = 0; dq_dR_z(2, 3) = 0; dq_dR_z(2, 4) = _aux8; dq_dR_z(2, 5) = 0; dq_dR_z(2, 6) = 0; dq_dR_z(2, 7) = 0; dq_dR_z(2, 8) = 0.125 * _aux4; } inline void compute_dR_dq(Eigen::Matrix& dR_dq, const double& qx, const double& qy, const double& qz, const double& qw) { double _aux1 = -4 * qy; double _aux2 = -4 * qz; double _aux3 = 1 / qw; double _aux4 = 2 * qx * qz; double _aux5 = -_aux3 * (_aux4 - 2 * qw * qy); double _aux6 = 2 * qy * qz; double _aux7 = -_aux3 * (_aux6 - 2 * qw * qx); double _aux8 = -2 * pow(qw, 2); double _aux9 = _aux8 + 2 * pow(qz, 2); double _aux10 = 2 * qw * qz; double _aux11 = (_aux10 + 2 * qx * qy) * _aux3; double _aux12 = _aux8 + 2 * pow(qy, 2); double _aux13 = _aux3 * (_aux6 + 2 * qw * qx); double _aux14 = _aux3 * (_aux4 + 2 * qw * qy); double _aux15 = -4 * qx; double _aux16 = _aux8 + 2 * pow(qx, 2); double _aux17 = (_aux10 - 2 * qx * qy) * _aux3; dR_dq(0, 0) = 0; dR_dq(0, 1) = _aux1; dR_dq(0, 2) = _aux2; dR_dq(1, 0) = _aux5; dR_dq(1, 1) = _aux7; dR_dq(1, 2) = -_aux3 * _aux9; dR_dq(2, 0) = _aux11; dR_dq(2, 1) = _aux12 * _aux3; dR_dq(2, 2) = _aux13; dR_dq(3, 0) = _aux14; dR_dq(3, 1) = _aux13; dR_dq(3, 2) = _aux3 * _aux9; dR_dq(4, 0) = _aux15; dR_dq(4, 1) = 0; dR_dq(4, 2) = _aux2; dR_dq(5, 0) = -_aux16 * _aux3; dR_dq(5, 1) = _aux17; dR_dq(5, 2) = _aux5; dR_dq(6, 0) = _aux17; dR_dq(6, 1) = -_aux12 * _aux3; dR_dq(6, 2) = _aux7; dR_dq(7, 0) = _aux16 * _aux3; dR_dq(7, 1) = _aux11; dR_dq(7, 2) = _aux14; dR_dq(8, 0) = _aux15; dR_dq(8, 1) = _aux1; dR_dq(8, 2) = 0; } inline void compute_dq_dR(Eigen::Matrix& dq_dR, const double& r11, const double& r21, const double& r31, const double& r12, const double& r22, const double& r32, const double& r13, const double& r23, const double& r33) { double qw; double S; int whichCase = _q2m(S, qw, r11, r21, r31, r12, r22, r32, r13, r23, r33); S *= .25; switch (whichCase) { case 0: compute_dq_dR_w(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33); break; case 1: compute_dq_dR_x(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33); break; case 2: compute_dq_dR_y(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33); break; case 3: compute_dq_dR_z(dq_dR, S, r11, r21, r31, r12, r22, r32, r13, r23, r33); break; } if (qw <= 0) dq_dR *= -1; } template inline void skew(Eigen::MatrixBase& s, const Eigen::MatrixBase& v) { const double x = 2 * v(0); const double y = 2 * v(1); const double z = 2 * v(2); if (transposed) s << 0., -z, y, z, 0, -x, -y, x, 0; else s << 0., z, -y, -z, 0, x, y, -x, 0; } template inline void skewT(Eigen::MatrixBase& s, const Eigen::MatrixBase& v) { skew(s, v); } template inline void skew(Eigen::MatrixBase& Sx, Eigen::MatrixBase& Sy, Eigen::MatrixBase& Sz, const Eigen::MatrixBase& R) { const double r11 = 2 * R(0, 0), r12 = 2 * R(0, 1), r13 = 2 * R(0, 2), r21 = 2 * R(1, 0), r22 = 2 * R(1, 1), r23 = 2 * R(1, 2), r31 = 2 * R(2, 0), r32 = 2 * R(2, 1), r33 = 2 * R(2, 2); if (transposed) { Sx << 0, 0, 0, r31, r32, r33, -r21, -r22, -r23; Sy << -r31, -r32, -r33, 0, 0, 0, r11, r12, r13; Sz << r21, r22, r23, -r11, -r12, -r13, 0, 0, 0; } else { Sx << 0, 0, 0, -r31, -r32, -r33, r21, r22, r23; Sy << r31, r32, r33, 0, 0, 0, -r11, -r12, -r13; Sz << -r21, -r22, -r23, r11, r12, r13, 0, 0, 0; } } template inline void skewT(Eigen::MatrixBase& Sx, Eigen::MatrixBase& Sy, Eigen::MatrixBase& Sz, const Eigen::MatrixBase& R) { skew(Sx, Sy, Sz, R); } } // namespace lightning::miao #endif // MIAO_MATH_H ================================================ FILE: src/core/miao/core/common/string_tools.cc ================================================ // // Created by xiang on 24-4-24. // #include "string_tools.h" #include #include #include namespace lightning::miao { std::string trim(const std::string& s) { if (s.length() == 0) { return s; } std::string::size_type b = s.find_first_not_of(" \t\n"); std::string::size_type e = s.find_last_not_of(" \t\n"); if (b == std::string::npos) { return ""; } return std::string(s, b, e - b + 1); } std::string trimLeft(const std::string& s) { if (s.length() == 0) { return s; } std::string::size_type b = s.find_first_not_of(" \t\n"); std::string::size_type e = s.length() - 1; if (b == std::string::npos) { return ""; } return std::string(s, b, e - b + 1); } std::string trimRight(const std::string& s) { if (s.length() == 0) { return s; } std::string::size_type b = 0; std::string::size_type e = s.find_last_not_of(" \t\n"); if (e == std::string::npos) { return ""; } return std::string(s, b, e - b + 1); } std::string strToLower(const std::string& s) { std::string ret; ret.reserve(s.size()); std::transform(s.begin(), s.end(), back_inserter(ret), [](unsigned char c) { return std::tolower(c); }); return ret; } std::string strToUpper(const std::string& s) { std::string ret; ret.reserve(s.size()); std::transform(s.begin(), s.end(), back_inserter(ret), [](unsigned char c) { return std::toupper(c); }); return ret; } std::string strExpandFilename(const std::string& filename) { #if (defined(UNIX) || defined(CYGWIN)) && !defined(ANDROID) std::string result = filename; wordexp_t p; wordexp(filename.c_str(), &p, 0); if (p.we_wordc > 0) { result = p.we_wordv[0]; } wordfree(&p); return result; #else (void)filename; LOG(WARNING) << "not implemented"; return std::string(); #endif } std::vector strSplit(const std::string& str, const std::string& delimiters) { std::vector tokens; if (str.empty()) { return tokens; } std::string::size_type lastPos = 0; std::string::size_type pos = 0; do { pos = str.find_first_of(delimiters, lastPos); tokens.push_back(str.substr(lastPos, pos - lastPos)); lastPos = pos + 1; } while (std::string::npos != pos); return tokens; } bool strStartsWith(const std::string& s, const std::string& start) { if (s.size() < start.size()) { return false; } return equal(start.begin(), start.end(), s.begin()); } bool strEndsWith(const std::string& s, const std::string& end) { if (s.size() < end.size()) { return false; } return equal(end.rbegin(), end.rend(), s.rbegin()); } int readLine(std::istream& is, std::stringstream& currentLine) { if (is.eof()) { return -1; } currentLine.str(""); currentLine.clear(); is.get(*currentLine.rdbuf()); if (is.fail()) { // fail is set on empty lines is.clear(); } skipLine(is); // read \n not read by get() if (currentLine.str().empty() && is.eof()) { return -1; } return static_cast(currentLine.str().size()); } void skipLine(std::istream& is) { char c = ' '; while (c != '\n' && is.good() && !is.eof()) { is.get(c); } } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/common/string_tools.h ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_STRING_TOOLS_H #define MIAO_STRING_TOOLS_H #include #include #include namespace lightning::miao { /** @addtogroup utils **/ // @{ /** \file stringTools.h * \brief utility functions for handling strings */ /** * remove whitespaces from the start/end of a string */ std::string trim(const std::string& s); /** * remove whitespaces from the left side of the string */ std::string trimLeft(const std::string& s); /** * remove whitespaced from the right side of the string */ std::string trimRight(const std::string& s); /** * convert the string to lower case */ std::string strToLower(const std::string& s); /** * convert a string to upper case */ std::string strToUpper(const std::string& s); /** * convert a string into an other type. */ template bool convertString(const std::string& s, T& x, bool failIfLeftoverChars = true) { std::istringstream i(s); char c; if (!(i >> x) || (failIfLeftoverChars && i.get(c))) return false; return true; } /** * convert a string into an other type. * Return the converted value. Throw error if parsing is wrong. */ template T stringToType(const std::string& s, bool failIfLeftoverChars = true) { T x; convertString(s, x, failIfLeftoverChars); return x; } /** * return true, if str starts with substr */ bool strStartsWith(const std::string& str, const std::string& substr); /** * return true, if str ends with substr */ bool strEndsWith(const std::string& str, const std::string& substr); /** * expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other * will get expanded. Also command substitution, e.g. `pwd` will give the * current directory. */ std::string strExpandFilename(const std::string& filename); /** * split a string into token based on the characters given in delim */ std::vector strSplit(const std::string& s, const std::string& delim); /** * read a line from is into currentLine. * @return the number of characters read into currentLine (excluding newline), * -1 on eof() */ int readLine(std::istream& is, std::stringstream& currentLine); /** * read from string until the end of a line is reached. */ void skipLine(std::istream& is); } // namespace lightning::miao #endif // MIAO_STRING_TOOLS_H ================================================ FILE: src/core/miao/core/graph/base_binary_edge.h ================================================ // // Created by xiang on 24-4-23. // #ifndef MIAO_BASE_BINARY_EDGE_H #define MIAO_BASE_BINARY_EDGE_H #include "base_fixed_sized_edge.h" namespace lightning::miao { template class BaseBinaryEdge : public BaseFixedSizedEdge { public: using VertexXiType = VertexXi; using VertexXjType = VertexXj; BaseBinaryEdge() : BaseFixedSizedEdge(){}; protected: typename BaseFixedSizedEdge::template JacobianType& jacobian_oplus_xi_ = std::get<0>(this->jacobian_oplus_); typename BaseFixedSizedEdge::template JacobianType& jacobian_oplus_xj_ = std::get<1>(this->jacobian_oplus_); }; } // namespace lightning::miao #endif // MIAO_BASE_BINARY_EDGE_H ================================================ FILE: src/core/miao/core/graph/base_edge.h ================================================ // // Created by xiang on 24-4-17. // #ifndef MIAO_BASE_EDGE_H #define MIAO_BASE_EDGE_H #include "common/eigen_types.h" #include "edge.h" namespace lightning::miao { /** * 模板化的edge * @tparam D 边的维度 * @tparam E 边的measurement type * * 由于只有edge本身的维度和measurement维度，没有关联的vertex维度，所以只能定义information矩阵维度，measurement维度，而没有jacobian维度 */ template class BaseEdge : public Edge { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW static constexpr int Dimension = D; using Measurement = E; using ErrorVector = Eigen::Matrix; using InformationType = Eigen::Matrix; BaseEdge() : Edge() { dimension_ = D; } // DISALLOW_COPY(BaseEdge); virtual ~BaseEdge() {} double Chi2() const override { return error_.dot(information_ * error_); } //! information matrix of the constraint EIGEN_STRONG_INLINE const InformationType& Information() const { return information_; } EIGEN_STRONG_INLINE InformationType& Information() { return information_; } void SetInformation(const InformationType& information) { information_ = information; } //! accessor functions for the measurement represented by the edge EIGEN_STRONG_INLINE const Measurement& GetMeasurement() const { return measurement_; } virtual void SetMeasurement(const Measurement& m) { measurement_ = m; } EIGEN_STRONG_INLINE const ErrorVector& GetError() const { return error_; } protected: /** * calculate the robust information matrix by updating the information matrix * of the error */ InformationType RobustInformation(const Vec3d& rho) const { return rho[1] * information_; } Measurement measurement_; ///< the measurement of the edge InformationType information_; ///< information matrix of the edge. ErrorVector error_; ///< error vector, stores the result after computeError() is called }; } // namespace lightning::miao #endif // MIAO_BASE_EDGE_H ================================================ FILE: src/core/miao/core/graph/base_fixed_sized_edge.h ================================================ // // Created by xiang on 24-4-18. // #ifndef MIAO_BASE_FIXED_SIZED_EDGE_H #define MIAO_BASE_FIXED_SIZED_EDGE_H #include "base_edge.h" namespace lightning::miao { namespace internal { // creating a bool array template std::array CreateBoolArray() { std::array aux = {false}; return aux; } template <> inline std::array CreateBoolArray<0>() { return std::array(); } constexpr int PairToIndex(const int i, const int j) { return j * (j - 1) / 2 + i; } /** * A trivial pair that has a constexpr c'tor. * std::pair in C++11 has not a constexpr c'tor. */ struct TrivialPair { int first_ = 0, second_ = 0; constexpr TrivialPair(int f, int s) : first_(f), second_(s) {} bool operator==(const TrivialPair &other) const { return first_ == other.first_ && second_ == other.second_; } }; /** * If we would have a constexpr for sqrt (cst_sqrt) it would be sth like. * For now we implement as a recursive function at compile time. * constexpr TrivialPair IndexToPair(n) { * constexpr int j = int(0.5 + cst_sqrt(0.25 + 2 * n)); * constexpr int base = PairToIndex(0, j); * constexpr int i = n - base; * return TrivialPair(i, j); * } */ constexpr TrivialPair IndexToPair(const int k, const int j = 0) { return k < j ? TrivialPair{k, j} : IndexToPair(k - j, j + 1); } //! helper function to call the c'tor of Eigen::Map template T CreateHessianMapK() { return T(); } //! helper function for creating a tuple of Eigen::Map template std::tuple CreateHessianMaps(const std::tuple &) { return std::tuple{CreateHessianMapK()...}; } template typename std::enable_if::type CreateNthVertexType(int, const EdgeType &, CtorArgs...) { return nullptr; } template typename std::enable_if::type CreateNthVertexType(int i, const EdgeType &t, CtorArgs... args) { if (i == I) { using VertexType = typename EdgeType::template VertexXnType; return new VertexType(args...); } return CreateNthVertexType(i, t, args...); } } // namespace internal /** * 编译期确定size的edge,unary, binary, multi edge都可以从这个类中定义 * 各顶点类型以tuple形式存储，可在编译期获得 * 真正实用化的edge * * 但由于VertexTypes是不定的，导致循环必须用tuple展开，写出一大堆魔法模板。。特别是针对jacobian和hessian的展开 * * @tparam D 维度 * @tparam E measurement type * @tparam VertexTypes 关联的各种顶点 */ template class BaseFixedSizedEdge : public BaseEdge { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// tuple 中第N个元素的type template using NthType = typename std::tuple_element>::type; //! The type of the N-th vertex /// 第N个顶点的type template using VertexXnType = NthType; //! Size of the N-th vertex at compile time /// 第N个顶点的维度 template static constexpr int VertexDimension() { return VertexXnType::Dimension; } /** * Return a pointer to the N-th vertex, directly casted to the correct type * 第N个顶点的指针，转换到子类的指针（shared_ptr） */ template VertexXnType *VertexXn() const { return (VertexXnType *)(vertices_[VertexN]); } static const int Dimension = BaseEdge::Dimension; using Measurement = typename BaseEdge::Measurement; using ErrorVector = typename BaseEdge::ErrorVector; using InformationType = typename BaseEdge::InformationType; template using JacobianType = typename Eigen::Matrix; template struct HessianType { double *hessian_in_solver_ = nullptr; // 远程地址 Eigen::Matrix hessian_; // 本地地址 }; //! it requires quite some ugly code to get the type of hessians... template using HessianBlockType = HessianType; template using HessianBlockTypeK = HessianBlockType::Dimension, VertexXnType::Dimension>; template using HessianBlockTypeKTransposed = HessianBlockType::Dimension, VertexXnType::Dimension>; template struct HessianTupleType; template struct HessianTupleType> { using type = std::tuple...>; using typeTransposed = std::tuple...>; }; static const std::size_t nr_of_vertices_ = sizeof...(VertexTypes); static const std::size_t nr_of_vertex_pairs_ = internal::PairToIndex(0, nr_of_vertices_); using HessianTuple = typename HessianTupleType>::type; using HessianTupleTransposed = typename HessianTupleType>::typeTransposed; using HessianRowMajorStorage = std::array; BaseFixedSizedEdge() : BaseEdge(), hessian_row_major_(internal::CreateBoolArray()), hessian_tuple_(internal::CreateHessianMaps(hessian_tuple_)), hessian_tuple_trans_(internal::CreateHessianMaps(hessian_tuple_trans_)) { vertices_.resize(nr_of_vertices_); } ~BaseFixedSizedEdge() override = default; template bool AllVerticesFixedNs(std::index_sequence) const; bool AllVerticesFixed() const override; /** * Linearizes the oplus operator in the vertex, and stores * the result in temporary variables _jacobianOplus * * 默认的linearize oplus，使用数值求导 */ void LinearizeOplus() override; template void LinearizeOplusNs(std::index_sequence); template void LinearizeOplusN(); /** * computes the (Block) elements of the Hessian matrix of the linearized least * squares. * * 计算二次型部分，包括hessian(AtA)和b */ void ConstructQuadraticForm() override; /// 拷贝hessian至solver void CopyHessianToSolver() override; template void CopyHessianToSolverN(); template void CopyHessianToSolverNs(std::index_sequence); template void CopyEdgeHessianMs(std::index_sequence); template void CopyEdgeHessianM(); /// 主对角线部分 template void ConstructQuadraticFormNs(const InformationType &omega, const ErrorVector &weightedError, std::index_sequence); template void ConstructQuadraticFormN(const InformationType &omega, const ErrorVector &weightedError); /// 非主对角线部分 template void ConstructOffDiagonalQuadraticFormMs(const AtOType &AtO, std::index_sequence); template void ConstructOffDiagonalQuadraticFormM(const AtOType &AtO); void MapHessianMemory(double *d, int i, int j, bool rowMajor) override; using BaseEdge::Resize; using BaseEdge::ComputeError; protected: using BaseEdge::measurement_; using BaseEdge::information_; using BaseEdge::error_; using BaseEdge::vertices_; using BaseEdge::dimension_; HessianRowMajorStorage hessian_row_major_; HessianTuple hessian_tuple_; HessianTupleTransposed hessian_tuple_trans_; std::tuple...> jacobian_oplus_; // 以tuple存储的jacobian，注意这是个map }; } // namespace lightning::miao #include "base_fixed_sized_edge.hpp" #endif // MIAO_BASE_FIXED_SIZED_EDGE_H ================================================ FILE: src/core/miao/core/graph/base_fixed_sized_edge.hpp ================================================ // // Created by xiang on 24-5-8. // #ifndef BASE_FIXED_SIZED_EDGE_HPP #define BASE_FIXED_SIZED_EDGE_HPP #include "base_vertex.h" #include "core/math/misc.h" #include "core/robust_kernel/robust_kernel.h" #include "utils/tuple_tools.h" #include "vertex.h" #include namespace lightning::miao { template template void BaseFixedSizedEdge::LinearizeOplusN() { auto vertex = VertexXn(); if (vertex->Fixed()) { return; } // 为了并发，现在需要拷贝这个顶点的估计值 // auto vertex_copy = vertex; auto vertex_copy = std::make_shared>(*vertex); vertices_[N] = vertex_copy.get(); auto &jacobianOplus = std::get(jacobian_oplus_); const double delta = cst(1e-9); const double scalar = 1 / (2 * delta); // estimate the jacobian numerically double add_vertex[VertexDimension()] = {}; // add small step along the unit vector in each dimension for (int d = 0; d < VertexDimension(); ++d) { vertex_copy->Push(); add_vertex[d] = delta; vertex_copy->Oplus(add_vertex); ComputeError(); auto errorBak = this->error_; vertex_copy->Pop(); vertex_copy->Push(); add_vertex[d] = -delta; vertex_copy->Oplus(add_vertex); ComputeError(); errorBak -= this->error_; vertex_copy->Pop(); add_vertex[d] = 0.0; jacobianOplus.col(d) = scalar * errorBak; } /// recover the copied vertex vertices_[N] = vertex; } template template void BaseFixedSizedEdge::LinearizeOplusNs(std::index_sequence) { (void(LinearizeOplusN()), ...); } template void BaseFixedSizedEdge::LinearizeOplus() { if (AllVerticesFixed()) { return; } ErrorVector errorBeforeNumeric = error_; LinearizeOplusNs(std::make_index_sequence()); error_ = errorBeforeNumeric; } template template bool BaseFixedSizedEdge::AllVerticesFixedNs(std::index_sequence) const { return (... && VertexXn()->Fixed()); } template bool BaseFixedSizedEdge::AllVerticesFixed() const { return AllVerticesFixedNs(std::make_index_sequence()); } template void BaseFixedSizedEdge::CopyHessianToSolver() { CopyHessianToSolverNs(std::make_index_sequence()); } template template void BaseFixedSizedEdge::CopyHessianToSolverNs(std::index_sequence) { (void(CopyHessianToSolverN()), ...); } template template void BaseFixedSizedEdge::CopyHessianToSolverN() { CopyEdgeHessianMs(std::make_index_sequence()); } template template void BaseFixedSizedEdge::CopyEdgeHessianMs(std::index_sequence) { (void(CopyEdgeHessianM()), ...); } template template void BaseFixedSizedEdge::CopyEdgeHessianM() { constexpr auto fromId = N; constexpr auto toId = N + M + 1; assert(fromId < toId && "Index mixed up"); constexpr auto K = internal::PairToIndex(fromId, toId); if (hessian_row_major_[K]) { auto &h = std::get(hessian_tuple_trans_); if (h.hessian_in_solver_ == nullptr) { return; } memcpy(h.hessian_in_solver_, h.hessian_.data(), sizeof(double) * h.hessian_.rows() * h.hessian_.cols()); h.hessian_.setZero(); } else { auto &h = std::get(hessian_tuple_); if (h.hessian_in_solver_ == nullptr) { return; } memcpy(h.hessian_in_solver_, h.hessian_.data(), sizeof(double) * h.hessian_.rows() * h.hessian_.cols()); h.hessian_.setZero(); } } template void BaseFixedSizedEdge::ConstructQuadraticForm() { if (this->GetRobustKernel()) { double error = this->Chi2(); Vector3 rho; this->GetRobustKernel()->Robustify(error, rho); Eigen::Matrix omega_r = -information_ * error_; omega_r *= rho[1]; ConstructQuadraticFormNs(this->RobustInformation(rho), omega_r, std::make_index_sequence()); } else { ConstructQuadraticFormNs(information_, -information_ * error_, std::make_index_sequence()); } } template template void BaseFixedSizedEdge::ConstructQuadraticFormNs(const InformationType &omega, const ErrorVector &weightedError, std::index_sequence) { (void(ConstructQuadraticFormN(omega, weightedError)), ...); } template template void BaseFixedSizedEdge::ConstructQuadraticFormN(const InformationType &omega, const ErrorVector &weightedError) { auto from = VertexXn(); const auto &A = std::get(jacobian_oplus_); if (from->Fixed()) { return; } const auto AtO = A.transpose() * omega; // from->GetB().noalias() += A.transpose() * weightedError; // from->A().noalias() += AtO * A; from->UpdateHessianAndBias(AtO * A, A.transpose() * weightedError); ConstructOffDiagonalQuadraticFormMs(AtO, std::make_index_sequence()); }; template template void BaseFixedSizedEdge::ConstructOffDiagonalQuadraticFormMs(const AtOType &AtO, std::index_sequence) { (void(ConstructOffDiagonalQuadraticFormM(AtO)), ...); } template template void BaseFixedSizedEdge::ConstructOffDiagonalQuadraticFormM(const AtOType &AtO) { constexpr auto fromId = N; constexpr auto toId = N + M + 1; assert(fromId < toId && "Index mixed up"); auto to = VertexXn(); if (to->Fixed()) { return; } const auto &B = std::get(jacobian_oplus_); constexpr auto K = internal::PairToIndex(fromId, toId); if (hessian_row_major_[K]) { // we have to write to the block as transposed auto &hessianTransposed = std::get(hessian_tuple_trans_); hessianTransposed.hessian_.noalias() += B.transpose() * AtO.transpose(); } else { auto &hessian = std::get(hessian_tuple_); hessian.hessian_.noalias() += AtO * B; } } /** * Helper functor class to construct the Hessian Eigen::Map object. * We have to pass the size at runtime to allow dynamically sized verices. */ struct MapHessianMemoryK { double *d; int rows; int cols; template void operator()(HessianT &hessian) { hessian.hessian_in_solver_ = d; hessian.hessian_.setZero(); // new (&hessian) typename std::remove_reference::type(d, rows, cols); } }; template void BaseFixedSizedEdge::MapHessianMemory(double *d, int i, int j, bool rowMajor) { assert(i < j && "index assumption violated"); // get the size of the vertices int vi_dim = Edge::GetVertex(i)->Dimension(); int vj_dim = Edge::GetVertex(j)->Dimension(); int k = internal::PairToIndex(i, j); hessian_row_major_[k] = rowMajor; if (rowMajor) { tuple_apply_i(MapHessianMemoryK{d, vj_dim, vi_dim}, hessian_tuple_trans_, k); } else { tuple_apply_i(MapHessianMemoryK{d, vi_dim, vj_dim}, hessian_tuple_, k); } } } // namespace lightning::miao #endif // BASE_FIXED_SIZED_EDGE_HPP ================================================ FILE: src/core/miao/core/graph/base_unary_edge.h ================================================ // // Created by xiang on 24-4-23. // #ifndef MIAO_BASE_UNARY_EDGE_H #define MIAO_BASE_UNARY_EDGE_H #include "base_fixed_sized_edge.h" namespace lightning::miao { template class BaseUnaryEdge : public BaseFixedSizedEdge { public: using VertexXiType = VertexXi; BaseUnaryEdge() : BaseFixedSizedEdge(){}; protected: typename BaseFixedSizedEdge::template JacobianType &jacobian_oplus_xi_ = std::get<0>(this->jacobian_oplus_); }; } // namespace lightning::miao #endif // MIAO_BASE_UNARY_EDGE_H ================================================ FILE: src/core/miao/core/graph/base_vec_vertex.h ================================================ // // Created by xiang on 24-6-4. // #ifndef MIAO_BASE_VEC_VERTEX_H #define MIAO_BASE_VEC_VERTEX_H #include "core/graph/base_vertex.h" namespace lightning::miao { /** * 内部以向量形式存储的顶点 * 外边只需定义维度即可 */ template class BaseVecVertex : public BaseVertex> { public: void OplusImpl(const double *d) override { typename Eigen::Matrix::ConstMapType v( d, BaseVertex>::Dimension); BaseVertex>::estimate_ += v; } }; } // namespace lightning::miao #endif // MIAO_BASE_VEC_VERTEX_H ================================================ FILE: src/core/miao/core/graph/base_vertex.h ================================================ // // Created by xiang on 24-3-19. // #ifndef MIAO_BASE_VERTEX_H #define MIAO_BASE_VERTEX_H #include "common/eigen_types.h" #include "common/std_types.h" #include "vertex.h" #include #include namespace lightning::miao { /** * 带模板参数的基类顶点 * @tparam D 顶点维度 * @tparam T 顶点的估计值变量类型,可以为自定义的结构体 * * 由于模板参数指定了维度，此时我们可以实际定义vertex的估计值，hessian块大小等变量了，而在基类的vertex只能定义接口 * * 由于顶点维度在编译期已知，Hessian Blocks的矩阵块维度也是已知的 * 在block solver中，优化器先建立整个优化问题的结构，再调用每个顶点和边的hessian构造函数 */ template class BaseVertex : public Vertex { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using EstimateType = T; using BackupStackType = std::stack>; /// NOTE base vertex 既可以通过Dimension变量获取维度（编译期），也可以通过Dimension()函数获取（运行期） static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space /// hessian 矩阵类型，本质上是一个映射，实际数据在block solver里, 为稀疏矩阵块 using HessianBlockType = Eigen::Matrix; public: BaseVertex() : Vertex() { dimension_ = D; hessian_.setZero(); } BaseVertex(const BaseVertex &other) : Vertex(other) { dimension_ = other.dimension_; hessian_ = other.hessian_; b_ = other.b_; hessian_in_solver_ = other.hessian_in_solver_; estimate_ = other.estimate_; backup_ = other.backup_; } const double &Hessian(int i, int j) const override { return hessian_(i, j); } double &Hessian(int i, int j) override { return hessian_(i, j); } /** * 在hessian位置上创建一个矩阵块 * @param d 实际存储的数据地址 */ inline void MapHessianMemory(double *d) override { hessian_in_solver_ = d; // CopyHessianFromSolver(); // new (&hessian_) HessianBlockType(d, Dimension, Dimension); } inline double *GetHessianMap() override { return hessian_in_solver_; } void ClearHessian() override { hessian_.setZero(); } /// 拷贝hessian至solver void CopyHessianToSolver() override { if (hessian_in_solver_) { memcpy(hessian_in_solver_, hessian_.data(), Dimension * Dimension * sizeof(double)); } } /// 将b中的元素拷贝到成员变量 int CopyB(double *b) const override { memcpy(b, b_.data(), Dimension * sizeof(double)); return Dimension; } /// 获取整个的B vector Eigen::Matrix &GetB() { return b_; } const Eigen::Matrix &GetB() const { return b_; } //! return the hessian Block associated with the vertex HessianBlockType &A() { return hessian_; } const HessianBlockType &A() const { return hessian_; } /// 更新内部的H, b值 virtual void UpdateHessianAndBias(const HessianBlockType &H, const Eigen::Matrix &b); /// 估计值放入缓存 void Push() override { backup_.push(estimate_); } /// 从缓存中提取估计值 void Pop() override { assert(!backup_.empty()); estimate_ = backup_.top(); backup_.pop(); } /// 放弃缓存中的估计值 void DiscardTop() override { assert(!backup_.empty()); backup_.pop(); } //! return the current estimate of the vertex const EstimateType &Estimate() const { return estimate_; } //! set the estimate for the vertex also calls updateCache() void SetEstimate(const EstimateType &et) { estimate_ = et; } /// 清空b inline void ClearQuadraticForm() override { hessian_.setZero(); b_.setZero(); } protected: /// NOTE: 现在hessian和b都存储于本地，通过Copy操作再进入solver,而不是直接map to solver std::mutex hessian_mutex_; HessianBlockType hessian_; Eigen::Matrix b_ = Eigen::Matrix::Zero(); double *hessian_in_solver_ = nullptr; EstimateType estimate_; // 估计值 BackupStackType backup_; // 备份值 }; template void BaseVertex::UpdateHessianAndBias(const BaseVertex::HessianBlockType &H, const Eigen::Matrix &b) { UL lock(hessian_mutex_); hessian_.noalias() += H; b_.noalias() += b; } } // namespace lightning::miao #endif // MIAO_BASE_VERTEX_H ================================================ FILE: src/core/miao/core/graph/edge.h ================================================ // // Created by xiang on 24-3-19. // #ifndef MIAO_EDGE_H #define MIAO_EDGE_H #include #include #include #include #include #include "core/common/macros.h" namespace lightning::miao { class Vertex; class Graph; class RobustKernel; /** * edge 是图中的一条边，也是图的基本元素 * edge 表示vertex之间的约束，与measurement一起构成误差的计算 * 继承关系：edge - base_edge - base_fixed_sized_edge - 各种实用edge */ class Edge { public: /// 基类Edge using VertexContainer = std::vector; explicit Edge(int id = invalid_id) : id_(id) {} virtual ~Edge() = default; /// interface =========================================================================================== /// 是否所有顶点都是固定的 virtual bool AllVerticesFixed() const = 0; /// 计算误差，用户来定义 virtual void ComputeError() = 0; /// 获取核函数 std::shared_ptr GetRobustKernel() const { return robust_kernel_; } /// 设置核函数 void SetRobustKernel(std::shared_ptr rb) { robust_kernel_ = rb; } /// 获取卡方 virtual double Chi2() const = 0; /// 线性化 virtual void ConstructQuadraticForm() = 0; /** * 将hessian转换到外部某个矩阵 * maps the internal matrix to some external memory location, * you need to provide the memory before calling constructQuadraticForm * @param d the memory location to which we map * @param i index of the vertex i * @param j index of the vertex j (j > i, upper triangular fashion) * @param rowMajor if true, will write in rowMajor order to the Block. Since * EIGEN is columnMajor by default, this results in writing the transposed */ virtual void MapHessianMemory(double *d, int i, int j, bool rowMajor) = 0; /// 将edge本地的jacobians拷贝至solver virtual void CopyHessianToSolver() = 0; /** * 线性化，并把jacobian放到某个矢量空间 */ virtual void LinearizeOplus() = 0; /// accessors =========================================================================================== /// Resize the vertices container virtual void Resize(size_t size) { vertices_.resize(size); } /// accessors const VertexContainer &GetVertices() const { return vertices_; } VertexContainer &GetVertices() { return vertices_; } /// 获取中间某个vertex Vertex *GetVertex(size_t i) const { assert(i < vertices_.size()); return vertices_[i]; } /// 设置 vertex void SetVertex(size_t i, std::shared_ptr v) { assert(i < vertices_.size()); vertices_[i] = v.get(); } int GetId() const { return id_; } void SetId(int id) { id_ = id; } /// 获取非法的顶点数量 int NumUndefinedVertices() const { return std::count_if(vertices_.begin(), vertices_.end(), [](const Vertex *ptr) { return ptr == nullptr; }); } int Level() const { return level_; } void SetLevel(int level) { level_ = level; } int Dimension() const { return dimension_; } long long GetInternalId() const { return internal_id_; } void SetInternalId(long long internal_id) { internal_id_ = internal_id; } protected: VertexContainer vertices_; // 顶点 int dimension_ = -1; // edge的维度 int level_ = 0; // 优化层级，默认0级的边才参与优化 std::shared_ptr robust_kernel_ = nullptr; // 核函数 int id_ = 0; // id long long internal_id_ = -1; // 内部id }; } // namespace lightning::miao #endif // MIAO_EDGE_H ================================================ FILE: src/core/miao/core/graph/graph.cc ================================================ // // Created by xiang on 24-3-19. // #include "graph.h" #include "core/common/string_tools.h" #include "core/graph/edge.h" #include "vertex.h" #include #include #include namespace lightning::miao { std::shared_ptr Graph::GetVertex(int id) { auto it = vertices_.find(id); if (it == vertices_.end()) { return nullptr; } return it->second; } bool Graph::AddVertex(std::shared_ptr v) { auto result = vertices_.insert(std::make_pair(v->GetId(), v)); return result.second; } bool Graph::AddEdge(std::shared_ptr e) { for (const auto& v : e->GetVertices()) { if (v == nullptr) { return false; } } /// 检查edge中是否有重复顶点 std::unordered_set vertexPointer; auto ves = e->GetVertices(); for (const auto& v : ves) { vertexPointer.insert(v); } if (vertexPointer.size() != ves.size()) { return false; } auto result = edges_.insert(e); if (!result.second) { return false; } for (const auto& v : vertexPointer) { // connect the vertices to this edge v->AddEdge(e); } e->SetInternalId(next_edge_id_++); if (e->NumUndefinedVertices()) { LOG(ERROR) << "this edge has undefined vertex."; return true; } return true; } bool Graph::SetEdgeVertex(Edge* e, int pos, std::shared_ptr v) { auto vOld = e->GetVertex(pos); if (vOld) { vOld->RemoveEdge(e); } e->SetVertex(pos, v); if (v) { v->AddEdge(e); } return true; } bool Graph::DetachVertex(std::shared_ptr v) { auto it = vertices_.find(v->GetId()); if (it == vertices_.end()) { return false; } assert(it->second == v); for (auto& iter : v->GetEdges()) { auto e = iter; for (size_t i = 0; i < e->GetVertices().size(); i++) { if (v.get() == e->GetVertex(i)) { SetEdgeVertex(e, i, nullptr); } } } return true; } Graph::~Graph() { Clear(); } bool Graph::RemoveEdge(Edge* e) { auto it = std::find_if(edges_.begin(), edges_.end(), [&](const auto& item) { return e == item.get(); }); if (it == edges_.end()) { return false; } /// note: 直接在edges里移除it会导致shared_ptr计数清零，e变为dangling pointer std::shared_ptr edge_to_remove = *it; edges_.erase(it); for (const auto& vit : e->GetVertices()) { if (vit == nullptr) { continue; } vit->RemoveEdge((*it).get()); } return true; } bool Graph::RemoveEdge(std::shared_ptr e) { return RemoveEdge(e.get()); } void Graph::Clear() { vertices_.clear(); edges_.clear(); next_edge_id_ = 0; } bool Graph::RemoveVertex(std::shared_ptr v, bool detach) { if (detach) { bool result = DetachVertex(v); if (!result) { assert(0 && "inconsistency in detaching vertex, "); } } auto it = vertices_.find(v->GetId()); if (it == vertices_.end()) { return false; } assert(it->second == v); // remove all edges which are entering or leaving v; int cnt_removed_edges = 0; for (auto& e : v->GetEdges()) { RemoveEdge(e); cnt_removed_edges++; } vertices_.erase(it); return true; } double Graph::Chi2() const { return std::accumulate(edges_.begin(), edges_.end(), 0, [](float sum, const std::shared_ptr& e) { return e->Chi2() + sum; }); } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/graph/graph.h ================================================ // // Created by xiang on 24-3-19. // #ifndef MIAO_GRAPH_H #define MIAO_GRAPH_H #include #include #include #include #include #include #include #include #include "core/common/macros.h" namespace lightning::miao { class Vertex; class Edge; /** * 图优化中的图 * * vertex和edge的指针实际由graph持有，而vertex内部的edge, * 或edge内部的vertex,使用原生指针，不需要delete，避免weak_ptr的性能问题 */ using VertexIDMap = std::unordered_map>; class Graph { public: Graph() = default; using EdgeSet = std::set>; using VertexSet = std::set>; using VertexContainer = std::vector; using EdgeContainer = std::vector>; /// graph 不允许拷贝 DISALLOW_COPY(Graph) /// 析构时清空 virtual ~Graph(); /// 添加顶点 virtual bool AddVertex(std::shared_ptr v); /// 往图中添加一个边 /// 如果边已经关联了vertex,也会在vertex中设置这个边 virtual bool AddEdge(std::shared_ptr e); /// 获取某个id的顶点，如果不存在返回nullptr std::shared_ptr GetVertex(int id); /// 移除一个顶点 virtual bool RemoveVertex(std::shared_ptr v, bool detach = false); /// 移除一个边 virtual bool RemoveEdge(std::shared_ptr e); virtual bool RemoveEdge(Edge* e); /// 清空整个图 virtual void Clear(); /// 获取所有顶点 const VertexIDMap& GetVertices() const { return vertices_; } VertexIDMap& GetVertices() { return vertices_; } /// 获取所有边 const std::set>& GetEdges() const { return edges_; } std::set>& GetEdges() { return edges_; } /// 关联某个edge和某个vertex virtual bool SetEdgeVertex(Edge* e, int pos, std::shared_ptr v); /// 分离某个vertex bool DetachVertex(std::shared_ptr v); /// 执行优化 virtual int Optimize(int interations) { return 0; } /// 整个优化问题的chi2 double Chi2() const; protected: VertexIDMap vertices_; // 所有顶点 std::set> edges_; // 所有边 long long next_edge_id_ = 0; // edge内部id }; } // namespace lightning::miao #endif // MIAO_GRAPH_H ================================================ FILE: src/core/miao/core/graph/optimizer.cc ================================================ // // Created by xiang on 24-4-23. // #include "optimizer.h" #include "vertex.h" #include "core/graph/edge.h" #include "core/opti_algo/optimization_algorithm.h" #include "core/robust_kernel/robust_kernel.h" #include #include namespace lightning::miao { Optimizer::Optimizer() {} Optimizer::~Optimizer() { algorithm_ = nullptr; } bool Optimizer::InitializeOptimization(int level) { if (!config_.incremental_mode_) { InitFromRaw(level); return true; } InitFromLast(level); return true; } void Optimizer::InitFromRaw(int level) { std::set vset; for (const auto &vp : vertices_) { vset.insert(vp.second.get()); } if (edges_.empty()) { LOG(WARNING) << "Attempt to initialize an empty graph"; return; } ClearIndexMapping(); active_vertices_.clear(); active_vertices_.reserve(vset.size()); active_edges_.clear(); std::set auxEdgeSet; // temporary structure to avoid duplicates for (const auto &v : vset) { const auto &vEdges = v->GetEdges(); // count if there are edges in that level. If not remove from the pool int levelEdges = 0; for (auto &e : vEdges) { if (level < 0 || e->Level() == level) { bool allVerticesOK = true; for (const auto &vit : e->GetVertices()) { if (vset.find(vit) == vset.end()) { allVerticesOK = false; break; } } if (allVerticesOK && !e->AllVerticesFixed()) { auxEdgeSet.insert(e); levelEdges++; } } } if (levelEdges) { active_vertices_.push_back(v); } } active_edges_.reserve(auxEdgeSet.size()); for (const auto &e : auxEdgeSet) { active_edges_.push_back(e); } SortVectorContainers(); BuildIndexMapping(active_vertices_); } void Optimizer::InitFromLast(int level) { /// 检查所有顶点都不应该有marg的部分 for (const auto &v : vertices_) { if (v.second->Marginalized()) { LOG(ERROR) << "should not use marginalization in incremental mode. "; return; } } for (const auto &v : new_vertices_) { active_vertices_.emplace_back(v); if (v->Fixed()) { v->SetHessianIndex(-1); } else { v->SetHessianIndex(iv_map_.size()); iv_map_.emplace_back(v); } } for (const auto &e : new_edges_) { if (e->Level() == level) { active_edges_.emplace_back(e.get()); } } SortVectorContainers(); } bool Optimizer::BuildIndexMapping(const std::vector &vlist) { if (!vlist.size()) { iv_map_.clear(); return false; } iv_map_.resize(vlist.size()); size_t i = 0; for (int k = 0; k < 2; k++) { for (const auto &v : vlist) { if (!v->Fixed()) { if (static_cast(v->Marginalized()) == k) { v->SetHessianIndex(i); iv_map_[i] = v; i++; } } else { v->SetHessianIndex(-1); } } } iv_map_.resize(i); return true; } void Optimizer::ClearIndexMapping() { for (auto &iv : iv_map_) { iv->SetHessianIndex(-1); iv = nullptr; } } void Optimizer::SortVectorContainers() { std::sort(active_vertices_.begin(), active_vertices_.end(), [](const Vertex *v1, const Vertex *v2) { return v1->GetId() < v2->GetId(); }); std::sort(active_edges_.begin(), active_edges_.end(), [](const Edge *e1, const Edge *e2) { return e1->GetInternalId() < e2->GetInternalId(); }); } int Optimizer::Optimize(int iterations) { if (iv_map_.empty()) { LOG(WARNING) << "0 vertices to Optimize, maybe forgot to call " "InitializeOptimization()"; /// NOTE: 这个可以帮忙调，不用每次让用户调 return -1; } int cjIterations = 0; bool ok = true; ok = algorithm_->Init(); if (!ok) { LOG(ERROR) << "Error while initializing"; return -1; } auto result = OptimizationAlgorithm::SolverResult::OK; double chi2 = 0; for (int i = 0; i < iterations && !Terminate() && ok; i++) { result = algorithm_->Solve(i); ok = (result == OptimizationAlgorithm::SolverResult::OK); ComputeActiveErrors(); double this_chi2 = ActiveRobustChi2(); if (Verbose()) { LOG(INFO) << "iteration= " << i << "\t chi2= " << std::fixed << this_chi2 << "\t edges= " << active_edges_.size(); } if (i > 0 && fabs(this_chi2 - chi2) < config_.eps_chi2_) { break; } chi2 = this_chi2; ++cjIterations; } if (result == OptimizationAlgorithm::SolverResult::Fail) { return 0; } return cjIterations; } double Optimizer::ActiveChi2() const { double chi = 0.0; for (const auto &e : active_edges_) { chi += e->Chi2(); } return chi; } double Optimizer::ActiveRobustChi2() const { lightning::Vector3 rho; double chi = 0.0; for (const auto &e : active_edges_) { if (e->GetRobustKernel()) { e->GetRobustKernel()->Robustify(e->Chi2(), rho); chi += rho[0]; } else { chi += e->Chi2(); } } return chi; } bool Optimizer::RemoveVertex(std::shared_ptr v, bool detach) { if (!config_.incremental_mode_ && v->HessianIndex() >= 0) { ClearIndexMapping(); iv_map_.clear(); } else { // 增量模式下不需要重建index mapping } return Graph::RemoveVertex(v, detach); } void Optimizer::SetAlgorithm(std::shared_ptr algorithm) { if (algorithm_) { algorithm_->SetOptimizer(nullptr); algorithm_ = nullptr; } algorithm_ = algorithm; algorithm_->SetOptimizer(this); } void Optimizer::Push() { std::for_each(std::execution::par_unseq, active_vertices_.begin(), active_vertices_.end(), [](const auto &v) { v->Push(); }); } void Optimizer::Pop() { std::for_each(std::execution::par_unseq, active_vertices_.begin(), active_vertices_.end(), [](const auto &v) { v->Pop(); }); } void Optimizer::DiscardTop() { std::for_each(std::execution::par_unseq, active_vertices_.begin(), active_vertices_.end(), [](const auto &v) { v->DiscardTop(); }); } void Optimizer::Clear() { iv_map_.clear(); active_vertices_.clear(); active_edges_.clear(); new_vertices_.clear(); new_edges_.clear(); vertices_id_deque_.clear(); Graph::Clear(); } void Optimizer::ComputeActiveErrors() { for (const auto &e : active_edges_) { e->ComputeError(); } } void Optimizer::Update(const double *update) { // update the graph by calling oplus on the vertices for (const auto &v : iv_map_) { Eigen::Map upd(update); // LOG(INFO) << "vert: " << v->GetId() << ", upd: " << upd.transpose(); v->Oplus(update); update += v->Dimension(); } } bool Optimizer::AddVertex(std::shared_ptr v) { if (config_.incremental_mode_) { if (config_.max_vertex_size_ > 0 && vertices_.size() == config_.max_vertex_size_) { // 更改v的id,并移除最旧的顶点 // 由于是replacement,也不需要将它放入new vertices中 int id_to_remove = vertices_id_deque_.front(); auto v_remove = vertices_[id_to_remove]; assert(v_remove->Dimension() == v->Dimension()); // 移除的顶点应该和加入的顶点有同样的维度 v->SetHessianIndex(v_remove->HessianIndex()); v->SetColInHessian(v_remove->ColInHessian()); v->MapHessianMemory(v_remove->GetHessianMap()); // set iv map for (auto &vv : iv_map_) { if (vv == v_remove.get()) { vv = v.get(); break; } } for (auto &vv : active_vertices_) { if (vv == v_remove.get()) { vv = v.get(); break; } } if (verbose_) { LOG(INFO) << "replacing new vertex " << v->GetId() << " with old " << v_remove->GetId(); } /// 先remove active edges auto edges_to_remove = v_remove->GetEdges(); std::set er_set; for (auto &e : edges_to_remove) { er_set.emplace(e); } std::vector new_active_edges; new_active_edges.reserve(active_edges_.size()); for (auto &active_edge : active_edges_) { if (er_set.find(active_edge) == er_set.end()) { new_active_edges.emplace_back(active_edge); } else { continue; } } active_edges_.swap(new_active_edges); RemoveVertex(v_remove); v->SetId(id_to_remove); vertices_id_deque_.pop_front(); } else { new_vertices_.emplace_back(v.get()); } vertices_id_deque_.push_back(v->GetId()); } bool ret = Graph::AddVertex(v); return ret; } bool Optimizer::AddEdge(std::shared_ptr e) { bool ret = Graph::AddEdge(e); if (!ret) { return false; } if (config_.incremental_mode_) { new_edges_.emplace_back(e); } return true; } bool Optimizer::RemoveEdge(std::shared_ptr e) { auto it = std::find_if(active_edges_.begin(), active_edges_.end(), [&](const auto &item) { return e.get() == item; }); if (it == active_edges_.end()) { return false; } *it = active_edges_.back(); active_edges_.pop_back(); return Graph::RemoveEdge(e); } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/graph/optimizer.h ================================================ // // Created by xiang on 24-4-23. // #ifndef MIAO_OPTIMIZER_H #define MIAO_OPTIMIZER_H #include "core/common/config.h" #include "core/graph/graph.h" #include "core/sparse/sparse_block_matrix.h" #include namespace lightning::miao { class OptimizationAlgorithm; class Solver; /// 基础的优化器，也是graph的一种 // TODO: block solver是编译期的，不是运行期的 class Optimizer : public Graph { public: /// TODO: 根据优化器选择要用的solver Optimizer(); ~Optimizer() override; void SetConfig(OptimizerConfig config) { config_ = config; } OptimizerConfig GetConfig() const { return config_; } /** * Initializes the structures for optimizing the whole graph. * Before calling it be sure to invoke marginalized() and fixed() to the * vertices you want to include in the Schur complement or to set as fixed * during the optimization. * @param level: is the level (in multilevel optimization) * @returns false if somethings goes wrong */ /** * 初始化优化器 * @param level 优化的edge级别 * @return */ bool InitializeOptimization(int level = 0); /// 添加顶点 bool AddVertex(std::shared_ptr v) override; /// 添加边 bool AddEdge(std::shared_ptr e) override; /// 移除边 bool RemoveEdge(std::shared_ptr e) override; /** * starts one optimization run given the current configuration of the graph, * and the current settings stored in the class instance. * It can be called only after InitializeOptimization */ int Optimize(int iterations); /**returns the cached chi2 of the active portion of the graph*/ double ActiveChi2() const; /** * returns the cached chi2 of the active portion of the graph. * In contrast to ActiveChi2() this functions considers the weighting * of the error according to the robustification of the error functions. */ double ActiveRobustChi2() const; //! Verbose information during optimization bool Verbose() const { return verbose_; } void SetVerbose(bool verbose) { verbose_ = verbose; } /** * sets a variable checked at every iteration to force a user stop. The * iteration exits when the variable is true; */ void SetForceStopFlag(bool* flag) { force_stop_flag_ = flag; } bool* ForceStopFlag() const { return force_stop_flag_; }; //! if external stop flag is given, return its state. False otherwise bool Terminate() { return force_stop_flag_ != nullptr && (*force_stop_flag_); } //! the index mapping of the vertices const VertexContainer& IndexMapping() const { return iv_map_; } //! the vertices active in the current optimization const VertexContainer& ActiveVertices() const { return active_vertices_; } //! the edges active in the current optimization const std::vector& ActiveEdges() const { return active_edges_; } //! new verticies const VertexContainer& NewVertices() const { return new_vertices_; } //! new edges const EdgeContainer& NewEdges() const { return new_edges_; } /// 清空新增的部分 void ClearNewElements() { new_vertices_.clear(); new_edges_.clear(); } /** * Remove a vertex. If the vertex is contained in the currently active set * of vertices, then the internal temporary structures are cleaned, e.g., the * index mapping is erased. In case you need the index mapping for * manipulating the graph, you have to store it in your own copy. */ bool RemoveVertex(std::shared_ptr v, bool detach = false) override; /// TODO Algorithm 的 set optimizer 没用shared from this void SetAlgorithm(std::shared_ptr algorithm); //! Push all the active vertices onto a stack void Push(); //! Pop (restore) the estimate of the active vertices from the stack void Pop(); //! same as above, but for the active vertices void DiscardTop(); /** * clears the graph, and polishes some intermediate structures * Note that this only removes nodes / edges. Parameters can be removed * with clearParameters(). */ void Clear() override; /** * computes the error vectors of all edges in the activeSet, and caches them */ void ComputeActiveErrors(); /** * Update the estimate of the active vertices * @param update: the double vector containing the stacked * elements of the increments on the vertices. */ void Update(const double* update); protected: /** * builds the mapping of the active vertices to the (Block) row / column in * the Hessian */ bool BuildIndexMapping(const std::vector& vlist); /// 从原始的vertices, edges开始初始化 void InitFromRaw(int level); void InitFromLast(int level); void ClearIndexMapping(); OptimizerConfig config_; bool* force_stop_flag_ = nullptr; bool verbose_ = false; VertexContainer iv_map_; /// 按ID顺序排列好的vertex，被marg部分在前 VertexContainer active_vertices_; ///< sorted according to id std::vector active_edges_; ///< sorted according to id /// incremental 模式 VertexContainer new_vertices_; /// inc模式下，新增的vertex EdgeContainer new_edges_; /// inc模式下，新增的edges std::deque vertices_id_deque_; /// inc模式下，需要记录添加的顶点顺序 void SortVectorContainers(); std::shared_ptr algorithm_ = nullptr; }; } // namespace lightning::miao #endif // MIAO_OPTIMIZER_H ================================================ FILE: src/core/miao/core/graph/vertex.h ================================================ // // Created by xiang on 24-3-19. // #ifndef MIAO_VERTEX_H #define MIAO_VERTEX_H #include #include #include #include "core/common/macros.h" namespace lightning::miao { class Graph; class Edge; // 基类顶点 /** * 最基本的vertex,是图的基本元素 * 顶点表示一个优化的变量，内部存储变量的估计值，同时优化器会调用顶点的更新函数对其进行更新 * 同时，每个顶点在优化矩阵的Hessian中占据一个矩阵块，因此有一个hessian index * 继承关系：vertex - base_vertex - 各种实用的vertex实现 * * 尽管有些优化变量要在base_vertex中才有定义，但solver操作的是基础的vertex,所以在vertex中会有接口 */ class Vertex { public: using EdgeVector = std::vector; explicit Vertex(int id = invalid_id) : id_(id) {} virtual ~Vertex() = default; /// 优化相关 /// 为vertex关联一个edge void AddEdge(std::shared_ptr e) { edges_.emplace_back(e.get()); } void AddEdge(Edge *e) { edges_.emplace_back(e); } /// 移除edge void RemoveEdge(Edge *e) { for (auto iter = edges_.begin(); iter != edges_.end(); iter++) { if ((*iter) == e) { *iter = edges_.back(); edges_.pop_back(); return; } } } /** * 对v进行增量更新 */ void Oplus(const double *v) { OplusImpl(v); } /// 虚基类接口 ================================================================================== /// interface declare /// 创建Hessian矩阵的映射关系 virtual void MapHessianMemory(double *d) = 0; virtual double *GetHessianMap() = 0; /// 清空Hessian virtual void ClearHessian() = 0; /// 拷贝hessian至solver virtual void CopyHessianToSolver() = 0; /** * copies the GetB vector in the array b_ * 将b拷贝至给定的数组中 * @return the number of elements copied */ virtual int CopyB(double *b) const = 0; //! backup the position of the vertex to a stack // 将该顶点的估计值放入缓存栈 virtual void Push() = 0; //! restore the position of the vertex by retrieving the position from the stack /// 从缓存栈中恢复该顶点的估计值 virtual void Pop() = 0; //! Pop the last element from the stack, without restoring the current estimate /// 丢弃缓存栈中的估计值 virtual void DiscardTop() = 0; /// 获取Hessian virtual const double &Hessian(int i, int j) const = 0; virtual double &Hessian(int i, int j) = 0; /** * Update the position of the node from the parameters in v. * Implement in your class! */ virtual void OplusImpl(const double *v) = 0; /// accessors =================================================================================== int GetId() const { return id_; } virtual void SetId(int id) { id_ = id; } const EdgeVector &GetEdges() const { return edges_; } EdgeVector &GetEdges() { return edges_; } // accessors int HessianIndex() const { return hessian_index_; } void SetHessianIndex(int ti) { hessian_index_ = ti; } /// 是否固定 bool Fixed() const { return fixed_; } //! true => this node should be considered fixed during the optimization void SetFixed(bool fixed) { fixed_ = fixed; } /// 是否边缘化 //! true => this node is marginalized out during the optimization bool Marginalized() const { return marginalized_; } //! true => this node should be marginalized out during the optimization void SetMarginalized(bool marginalized) { marginalized_ = marginalized; } //! dimension of the estimated state belonging to this node int Dimension() const { return dimension_; } //! set the row of this vertex in the Hessian /** * 记录这个顶点在整个hessian中占第几列 * @param c */ void SetColInHessian(int c) { col_in_hessian_ = c; } //! get the row of this vertex in the Hessian int ColInHessian() const { return col_in_hessian_; } /** * set the GetB vector part of this vertex to zero */ virtual void ClearQuadraticForm() = 0; protected: int id_ = 0; // 顶点的id std::vector edges_; // 关联的edges 由于edges所属权在graph /// 优化参数 bool fixed_ = false; // 顶点是否固定 bool marginalized_ = false; // 顶点是否被边缘化 int dimension_ = 0; // 顶点的维度，在base_vertex中使用模板参数固定 int col_in_hessian_ = -1; // 顶点在整个Hessian矩阵中占第几列（含维度） int hessian_index_ = -1; }; } // namespace lightning::miao using VertexPtr = std::shared_ptr; #endif // MIAO_VERTEX_H ================================================ FILE: src/core/miao/core/math/marginal_covariance_cholesky.cc ================================================ // // Created by xiang on 24-4-26. // #include "marginal_covariance_cholesky.h" namespace lightning::miao { struct MatrixElem { int r, c; MatrixElem(int r_, int c_) : r(r_), c(c_) {} bool operator<(const MatrixElem& other) const { return c > other.c || (c == other.c && r > other.r); } }; MarginalCovarianceCholesky::MarginalCovarianceCholesky() = default; MarginalCovarianceCholesky::~MarginalCovarianceCholesky() = default; void MarginalCovarianceCholesky::SetCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv) { n_ = n; Ap_ = Lp; Ai_ = Li; Ax_ = Lx; perm_ = permInv; // pre-compute reciprocal values of the diagonal of L diag_.resize(n); for (int r = 0; r < n; ++r) { const int& sc = Ap_[r]; // L is lower triangular, thus the first elem in // the column is the diagonal entry assert(r == Ai_[sc] && "Error in CCS storage of L"); diag_[r] = 1.0 / Ax_[sc]; } } double MarginalCovarianceCholesky::ComputeEntry(int r, int c) { assert(r <= c); int idx = ComputeIndex(r, c); auto foundIt = map_.find(idx); if (foundIt != map_.end()) { return foundIt->second; } // compute the summation over column r double s = 0.; const int& sc = Ap_[r]; const int& ec = Ap_[r + 1]; for (int j = sc + 1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal const int& rr = Ai_[j]; double val = rr < c ? ComputeEntry(rr, c) : ComputeEntry(c, rr); s += val * Ax_[j]; } double result; if (r == c) { const double& diagElem = diag_[r]; result = diagElem * (diagElem - s); } else { result = -s * diag_[r]; } map_[idx] = result; return result; } void MarginalCovarianceCholesky::ComputeCovariance(double** covBlocks, const std::vector& blockIndices) { map_.clear(); int base = 0; std::vector elemsToCompute; for (size_t i = 0; i < blockIndices.size(); ++i) { int nbase = blockIndices[i]; int vdim = nbase - base; for (int rr = 0; rr < vdim; ++rr) { for (int cc = rr; cc < vdim; ++cc) { int r = perm_ ? perm_[rr + base] : rr + base; // apply permutation int c = perm_ ? perm_[cc + base] : cc + base; if (r > c) { // make sure it's still upper triangular after applying the permutation std::swap(r, c); } elemsToCompute.emplace_back(r, c); } } base = nbase; } // sort the elems to reduce the recursive calls sort(elemsToCompute.begin(), elemsToCompute.end()); // compute the inverse elements we need for (auto me : elemsToCompute) { ComputeEntry(me.r, me.c); } // set the marginal covariance for the vertices, by writing to the blocks // memory base = 0; for (size_t i = 0; i < blockIndices.size(); ++i) { int nbase = blockIndices[i]; int vdim = nbase - base; double* cov = covBlocks[i]; for (int rr = 0; rr < vdim; ++rr) { for (int cc = rr; cc < vdim; ++cc) { int r = perm_ ? perm_[rr + base] : rr + base; // apply permutation int c = perm_ ? perm_[cc + base] : cc + base; if (r > c) { // upper triangle std::swap(r, c); } int idx = ComputeIndex(r, c); auto foundIt = map_.find(idx); assert(foundIt != map_.end()); cov[rr * vdim + cc] = foundIt->second; if (rr != cc) { cov[cc * vdim + rr] = foundIt->second; } } } base = nbase; } } void MarginalCovarianceCholesky::ComputeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector >& blockIndices) { // allocate the sparse spinv = SparseBlockMatrix(&rowBlockIndices[0], &rowBlockIndices[0], rowBlockIndices.size(), rowBlockIndices.size(), true); map_.clear(); std::vector elemsToCompute; for (size_t i = 0; i < blockIndices.size(); ++i) { int blockRow = blockIndices[i].first; int blockCol = blockIndices[i].second; assert(blockRow >= 0); assert(blockRow < (int)rowBlockIndices.size()); assert(blockCol >= 0); assert(blockCol < (int)rowBlockIndices.size()); int rowBase = spinv.RowBaseOfBlock(blockRow); int colBase = spinv.ColBaseOfBlock(blockCol); lightning::MatrixX* block = spinv.Block(blockRow, blockCol, true); assert(block); for (int iRow = 0; iRow < block->rows(); ++iRow) for (int iCol = 0; iCol < block->cols(); ++iCol) { int rr = rowBase + iRow; int cc = colBase + iCol; int r = perm_ ? perm_[rr] : rr; // apply permutation int c = perm_ ? perm_[cc] : cc; if (r > c) { std::swap(r, c); } elemsToCompute.push_back(MatrixElem(r, c)); } } // sort the elems to reduce the number of recursive calls std::sort(elemsToCompute.begin(), elemsToCompute.end()); // compute the inverse elements we need for (const auto& me : elemsToCompute) { ComputeEntry(me.r, me.c); } // set the marginal covariance for (size_t i = 0; i < blockIndices.size(); ++i) { int blockRow = blockIndices[i].first; int blockCol = blockIndices[i].second; int rowBase = spinv.RowBaseOfBlock(blockRow); int colBase = spinv.ColBaseOfBlock(blockCol); lightning::MatrixX* block = spinv.Block(blockRow, blockCol); assert(block); for (int iRow = 0; iRow < block->rows(); ++iRow) for (int iCol = 0; iCol < block->cols(); ++iCol) { int rr = rowBase + iRow; int cc = colBase + iCol; int r = perm_ ? perm_[rr] : rr; // apply permutation int c = perm_ ? perm_[cc] : cc; if (r > c) { std::swap(r, c); } int idx = ComputeIndex(r, c); auto foundIt = map_.find(idx); assert(foundIt != map_.end()); (*block)(iRow, iCol) = foundIt->second; } } } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/math/marginal_covariance_cholesky.h ================================================ // // Created by xiang on 24-4-26. // #ifndef MIAO_MARGINAL_COVARIANCE_CHOLESKY_H #define MIAO_MARGINAL_COVARIANCE_CHOLESKY_H #include #include #include "common/eigen_types.h" #include "core/sparse/sparse_block_matrix.h" namespace lightning::miao { /** * \brief computing the marginal covariance given a cholesky factor (lower * triangle of the factor) * * 这个主要给LinearSolverCCS 用 */ class MarginalCovarianceCholesky { protected: /** * hash struct for storing the matrix elements needed to compute the * covariance */ typedef std::unordered_map LookupMap; public: MarginalCovarianceCholesky(); ~MarginalCovarianceCholesky(); /** * compute the marginal cov for the given Block indices, write the result to * the covBlocks memory (which has to be provided by the caller). */ void ComputeCovariance(double** covBlocks, const std::vector& blockIndices); /** * compute the marginal cov for the given Block indices, write the result in * spinv). */ void ComputeCovariance(SparseBlockMatrix& spinv, const std::vector& rowBlockIndices, const std::vector >& blockIndices); /** * set the CCS representation of the cholesky factor along with the inverse * permutation used to reduce the fill-in. permInv might be 0, will then not * permute the entries. * * The pointers provided by the user need to be still valid when calling * ComputeCovariance(). The pointers are owned by the caller, * MarginalCovarianceCholesky does not free the pointers. */ void SetCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv); protected: // information about the cholesky factor (lower triangle) int n_ = 0; ///< L is an n X n matrix int* Ap_ = nullptr; ///< column pointer of the CCS storage int* Ai_ = nullptr; ///< row indices of the CCS storage double* Ax_ = nullptr; ///< values of the cholesky factor int* perm_ = nullptr; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in LookupMap map_; ///< hash look up table for the already computed entries std::vector diag_; ///< cache 1 / H_ii to avoid recalculations //! compute the index used for hashing int ComputeIndex(int r, int c) const { /*assert(r <= c);*/ return r * n_ + c; } /** * compute one entry in the covariance, r and c are values after applying the * permutation, and upper triangular. May issue recursive calls to itself to * compute the missing values. */ double ComputeEntry(int r, int c); }; } // namespace lightning::miao #endif // MIAO_MARGINAL_COVARIANCE_CHOLESKY_H ================================================ FILE: src/core/miao/core/math/matrix_operations.h ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_MATRIX_OPERATIONS_H #define MIAO_MATRIX_OPERATIONS_H #include "common/eigen_types.h" namespace lightning::miao::internal { template inline void axpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff) += A * x.segment(xoff); } template inline void axpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff, A.rows()) += A * x.segment::ColsAtCompileTime>(xoff); } template <> inline void axpy(const lightning::MatrixX& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); } template inline void atxpy(const MatrixType& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff) += A.transpose() * x.segment(xoff); } template inline void atxpy(const Eigen::Matrix& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows()); } template <> inline void atxpy(const lightning::MatrixX& A, const Eigen::Map& x, int xoff, Eigen::Map& y, int yoff) { y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); } } // namespace lightning::miao::internal #endif // MIAO_MATRIX_OPERATIONS_H ================================================ FILE: src/core/miao/core/math/misc.h ================================================ // // Created by xiang on 24-5-8. // #ifndef MISC_H #define MISC_H #include namespace lightning::miao { /** * converts a number constant to a double constant at compile time * to avoid having to cast everything to avoid warnings. **/ inline constexpr double cst(long double v) { return (double)v; } } // namespace lightning::miao #endif // MISC_H ================================================ FILE: src/core/miao/core/opti_algo/algo_select.h ================================================ // // Created by xiang on 24-5-14. // #ifndef MIAO_ALGO_SELECT_H #define MIAO_ALGO_SELECT_H #include "optimization_algorithm.h" #include "core/common/config.h" #include "core/solver/block_solver.h" #include "core/solver/linear_solver_dense.h" #include "core/solver/linear_solver_eigen.h" #include "core/solver/linear_solver_pcg.h" #include "core/solver/solver.h" #include "core/opti_algo/opti_algo_dogleg.h" #include "core/opti_algo/opti_algo_gauss_newton.h" #include "core/opti_algo/opti_algo_lm.h" #include "core/graph/optimizer.h" namespace lightning::miao { template void SetupOptimizer(Optimizer& optimizer, OptimizerConfig options) { std::shared_ptr solver = nullptr; using BlockSolverType = BlockSolver>; if (options.is_dense_) { if (options.linear_solver_type_ != LinearSolverType::LINEAR_SOLVER_DENSE) { LOG(FATAL) << "不能为稠密的问题设置稀疏求解器"; } /// TODO: dense but not Block Solver solver = std::make_shared( std::make_unique>()); } else { if (options.linear_solver_type_ == LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN) { solver = std::make_shared( std::make_unique>()); } else if (options.linear_solver_type_ == LinearSolverType::LINEAR_SOLVER_PCG) { solver = std::make_shared( std::make_unique>()); } } std::shared_ptr algo = nullptr; switch (options.algo_type_) { case AlgorithmType::GAUSS_NEWTON: algo = std::make_shared(solver); break; case AlgorithmType::LEVENBERG_MARQUARDT: algo = std::make_shared(solver); break; case AlgorithmType::DOGLEG: algo = std::make_shared(solver); break; default: LOG(ERROR) << "unknown optimization type"; break; } optimizer.SetAlgorithm(algo); algo->Init(); optimizer.SetConfig(options); } /// creator func template std::shared_ptr SetupOptimizer(OptimizerConfig options) { auto opti = std::make_shared(); SetupOptimizer(*opti, options); return opti; } } // namespace lightning::miao #endif // MIAO_ALGO_SELECT_H ================================================ FILE: src/core/miao/core/opti_algo/opti_algo_dogleg.cc ================================================ // // Created by xiang on 24-5-13. // #include "opti_algo_dogleg.h" #include "core/graph/optimizer.h" #include "core/graph/vertex.h" #include "core/math/misc.h" #include "core/solver/solver.h" #include #include #include #include namespace lightning::miao { OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(std::shared_ptr solver) : OptimizationAlgorithm(solver) {} OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg() {} OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::Solve(int iteration) { assert(optimizer_ && "_optimizer not set"); if (iteration == 0) { // built up the CCS structure, here due to easy time measure bool ok = solver_->BuildStructure(); if (!ok) { LOG(WARNING) << "Failure while building CCS structure"; return SolverResult::Fail; } // init some members to the current size of the problem hsd_.resize(solver_->VectorSize()); hdl_.resize(solver_->VectorSize()); aux_vector_.resize(solver_->VectorSize()); delta_ = user_delta_init_; current_lambda_ = init_lambda_; was_pd_in_all_iterations_ = true; } optimizer_->ComputeActiveErrors(); double currentChi = optimizer_->ActiveRobustChi2(); solver_->BuildSystem(); lightning::VectorX::ConstMapType b(solver_->GetB(), solver_->VectorSize()); // compute alpha aux_vector_.setZero(); solver_->MultiplyHessian(aux_vector_.data(), solver_->GetB()); double bNormSquared = b.squaredNorm(); double alpha = bNormSquared / aux_vector_.dot(b); hsd_ = alpha * b; double hsdNorm = hsd_.norm(); double hgnNorm = -1.; bool solvedGaussNewton = false; bool goodStep = false; int& numTries = last_num_tries_; numTries = 0; do { ++numTries; if (!solvedGaussNewton) { const double minLambda = cst(1e-12); const double maxLambda = cst(1e3); solvedGaussNewton = true; // apply a damping factor to enforce positive definite Hessian, if the // matrix appeared to be not positive definite in at least one iteration // before. We apply a damping factor to obtain a PD matrix. bool solverOk = false; while (!solverOk) { if (!was_pd_in_all_iterations_) solver_->SetLambda(current_lambda_, true); // add _currentLambda to the diagonal solverOk = solver_->Solve(); if (!was_pd_in_all_iterations_) { solver_->RestoreDiagonal(); } was_pd_in_all_iterations_ = was_pd_in_all_iterations_ && solverOk; if (!was_pd_in_all_iterations_) { // simple strategy to control the damping factor if (solverOk) { current_lambda_ = std::max(minLambda, current_lambda_ / (cst(0.5) * lambda_factor_)); } else { current_lambda_ *= lambda_factor_; if (current_lambda_ > maxLambda) { current_lambda_ = maxLambda; return SolverResult::Fail; } } } } hgnNorm = lightning::VectorX::ConstMapType(solver_->GetX(), solver_->VectorSize()).norm(); } lightning::VectorX::ConstMapType hgn(solver_->GetX(), solver_->VectorSize()); assert(hgnNorm >= 0. && "Norm of the GN step is not computed"); if (hgnNorm < delta_) { hdl_ = hgn; } else if (hsdNorm > delta_) { hdl_ = delta_ / hsdNorm * hsd_; } else { aux_vector_ = hgn - hsd_; // b - a double c = hsd_.dot(aux_vector_); double bmaSquaredNorm = aux_vector_.squaredNorm(); double beta; if (c <= 0.) beta = (-c + sqrt(c * c + bmaSquaredNorm * (delta_ * delta_ - hsd_.squaredNorm()))) / bmaSquaredNorm; else { double hsdSqrNorm = hsd_.squaredNorm(); beta = (delta_ * delta_ - hsdSqrNorm) / (c + sqrt(c * c + bmaSquaredNorm * (delta_ * delta_ - hsdSqrNorm))); } assert(beta > 0. && beta < 1 && "Error while computing beta"); hdl_ = hsd_ + beta * (hgn - hsd_); assert(hdl_.norm() < delta_ + 1e-5 && "Computed step does not correspond to the trust region"); } // compute the linear gain aux_vector_.setZero(); solver_->MultiplyHessian(aux_vector_.data(), hdl_.data()); double linearGain = -1 * (aux_vector_.dot(hdl_)) + 2 * (b.dot(hdl_)); // apply the update and see what happens optimizer_->Push(); optimizer_->Update(hdl_.data()); optimizer_->ComputeActiveErrors(); double newChi = optimizer_->ActiveRobustChi2(); double nonLinearGain = currentChi - newChi; if (fabs(linearGain) < 1e-12) { linearGain = cst(1e-12); } double rho = nonLinearGain / linearGain; if (rho > 0) { // step is good and will be accepted optimizer_->DiscardTop(); goodStep = true; } else { // recover previous state optimizer_->Pop(); } // update trust region based on the step quality if (rho > 0.75) { delta_ = std::max(delta_, 3 * hdl_.norm()); } else if (rho < 0.25) { delta_ *= 0.5; } } while (!goodStep && numTries < max_trail_after_failure_); if (numTries == max_trail_after_failure_ || !goodStep) { return SolverResult::Terminate; } return SolverResult::OK; } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/opti_algo/opti_algo_dogleg.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_OPTI_ALGO_DOGLEG_H #define MIAO_OPTI_ALGO_DOGLEG_H #include "optimization_algorithm.h" namespace lightning::miao { /** * \brief Implementation of Powell's Dogleg Algorithm */ class OptimizationAlgorithmDogleg : public OptimizationAlgorithm { public: /** * construct the Dogleg Algorithm, which will use the given Solver for solving * the linearized system. */ explicit OptimizationAlgorithmDogleg(std::shared_ptr solver); ~OptimizationAlgorithmDogleg() override; virtual SolverResult Solve(int iteration) override; protected: // parameters int max_trail_after_failure_ = 100; double user_delta_init_ = 1e4; // damping to enforce positive definite matrix double init_lambda_ = 1e-7; double lambda_factor_ = 10.0; lightning::VectorX hsd_; ///< steepest decent step lightning::VectorX hdl_; ///< final dogleg step lightning::VectorX aux_vector_; ///< auxiliary vector used to perform multiplications or other stuff double current_lambda_ = 0.0; ///< the damping factor to force positive definite matrix double delta_ = 1e4; ///< trust region bool was_pd_in_all_iterations_ = true; ///< the matrix we Solve was positive definite in all iterations -> if not apply damping int last_num_tries_ = 0; }; } // namespace lightning::miao #endif // MIAO_OPTI_ALGO_DOGLEG_H ================================================ FILE: src/core/miao/core/opti_algo/opti_algo_gauss_newton.cc ================================================ // // Created by xiang on 24-4-26. // #include "opti_algo_gauss_newton.h" #include "core/graph/optimizer.h" #include "core/solver/solver.h" #include #include namespace lightning::miao { OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(std::shared_ptr solver) : OptimizationAlgorithm(std::move(solver)) {} OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::Solve(int iteration) { bool ok = true; // here so that correct component for max-mixtures can be computed before the // build structure optimizer_->ComputeActiveErrors(); if (iteration == 0) { // built up the CCS structure, here due to easy time measure ok = solver_->BuildStructure(); if (!ok) { LOG(WARNING) << "Failure while building CCS structure"; return OptimizationAlgorithm::SolverResult::Fail; } } solver_->BuildSystem(); ok = solver_->Solve(); optimizer_->Update(solver_->GetX()); if (ok) { return SolverResult::OK; } else { return SolverResult::Fail; } } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/opti_algo/opti_algo_gauss_newton.h ================================================ // // Created by xiang on 24-4-26. // #ifndef MIAO_OPTI_ALGO_GAUSS_NEWTON_H #define MIAO_OPTI_ALGO_GAUSS_NEWTON_H #include "optimization_algorithm.h" namespace lightning::miao { /** * \brief Implementation of the Gauss Newton Algorithm * * 最基本的algothrm 基本啥也没做，只是调solve函数 */ class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithm { public: /** * construct the Gauss Newton Algorithm, which use the given Solver for * solving the linearized system. */ explicit OptimizationAlgorithmGaussNewton(std::shared_ptr solver); ~OptimizationAlgorithmGaussNewton() override = default; SolverResult Solve(int iteration) override; }; } // namespace lightning::miao #endif // MIAO_OPTI_ALGO_GAUSS_NEWTON_H ================================================ FILE: src/core/miao/core/opti_algo/opti_algo_lm.cc ================================================ // // Created by xiang on 24-5-13. // #include "opti_algo_lm.h" #include "core/graph/edge.h" #include "core/graph/optimizer.h" #include "core/graph/vertex.h" #include "core/solver/solver.h" #include #include #include #include #include #include "utils/timer.h" namespace lightning::miao { OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::shared_ptr solver) : OptimizationAlgorithm(std::move(solver)), lm_iter_(0) {} OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::Solve(int iteration) { assert(optimizer_ && "_optimizer not set"); if (iteration == 0) { // built up the CCS structure, here due to easy time measure bool ok = false; ok = solver_->BuildStructure(); if (!ok) { LOG(WARNING) << "Failure while building CCS structure"; return SolverResult::Fail; } } optimizer_->ComputeActiveErrors(); double currentChi = optimizer_->ActiveRobustChi2(); solver_->BuildSystem(); // core part of the Levenbarg algorithm if (iteration == 0) { current_lambda_ = ComputeLambdaInit(); ni_ = 2; } double rho = 0; int &qmax = lm_iter_; qmax = 0; do { optimizer_->Push(); // update the diagonal of the system matrix solver_->SetLambda(current_lambda_, true); bool ok2 = true; bool should_break = false; ok2 = solver_->Solve(); optimizer_->Update(solver_->GetX()); // restore the diagonal solver_->RestoreDiagonal(); optimizer_->ComputeActiveErrors(); double tempChi = optimizer_->ActiveRobustChi2(); if (!ok2) { tempChi = std::numeric_limits::max(); } rho = (currentChi - tempChi); double scale = ok2 ? ComputeScale() + 1e-3 : 1; // make sure it's non-zero :) rho /= scale; if (rho > 0 && std::isfinite(tempChi) && ok2) { // last step was good double alpha = 1. - pow((2 * rho - 1), 3); // crop lambda between minimum and maximum factors alpha = (std::min)(alpha, good_step_upper_); double scaleFactor = (std::max)(good_step_lower_, alpha); current_lambda_ *= scaleFactor; ni_ = 2; currentChi = tempChi; optimizer_->DiscardTop(); } else { current_lambda_ *= ni_; ni_ *= 2; optimizer_->Pop(); // restore the last state before trying to optimize if (!std::isfinite(current_lambda_)) { should_break = true; } } qmax++; // LOG(INFO) << "lm iter: " << qmax << ", temp chi: " << tempChi << ", current chi: " << currentChi // << ", rho: " << rho; if (should_break) { break; } } while (rho < 0 && qmax < max_trails_after_failure_ && !optimizer_->Terminate()); if (qmax == max_trails_after_failure_ || rho == 0 || !std::isfinite(current_lambda_)) { // LOG(WARNING) << "solver terminated, rho: " << rho << ", qmax: " << qmax << ", " << current_lambda_; return SolverResult::Terminate; } // LOG(INFO) << "lm iter: " << qmax << ", lambda: " << current_lambda_; return SolverResult::OK; } double OptimizationAlgorithmLevenberg::ComputeLambdaInit() const { if (user_lambda_limit_ > 0) { return user_lambda_limit_; } double maxDiagonal = 0; for (size_t k = 0; k < optimizer_->IndexMapping().size(); k++) { auto v = optimizer_->IndexMapping()[k]; assert(v); int dim = v->Dimension(); for (int j = 0; j < dim; ++j) { maxDiagonal = std::max(fabs(v->Hessian(j, j)), maxDiagonal); } } return tau_ * maxDiagonal; } double OptimizationAlgorithmLevenberg::ComputeScale() const { double scale = 0; for (size_t j = 0; j < solver_->VectorSize(); j++) { scale += solver_->GetX()[j] * (current_lambda_ * solver_->GetX()[j] + solver_->GetB()[j]); } return scale; } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/opti_algo/opti_algo_lm.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_OPTI_ALGO_LM_H #define MIAO_OPTI_ALGO_LM_H #include "optimization_algorithm.h" namespace lightning::miao { /** * \brief Implementation of the Levenberg Algorithm */ class OptimizationAlgorithmLevenberg : public OptimizationAlgorithm { public: /** * construct the Levenberg Algorithm, which will use the given Solver for * solving the linearized system. */ explicit OptimizationAlgorithmLevenberg(std::shared_ptr solver); virtual ~OptimizationAlgorithmLevenberg() = default; virtual SolverResult Solve(int iteration); protected: /** * helper for Levenberg, this function computes the initial damping factor, if * the user did not specify an own value, see setUserLambdaInit() */ double ComputeLambdaInit() const; double ComputeScale() const; // Levenberg parameters int max_trails_after_failure_ = 10; double user_lambda_limit_ = 0; double current_lambda_ = -1; double tau_ = 1e-5; double good_step_lower_ = 1.0 / 3.0; ///< lower bound for lambda decrease if a good LM step double good_step_upper_ = 2.0 / 3.0; ///< upper bound for lambda decrease if a good LM step double ni_ = 2.0; int lm_iter_ = 0; ///< the number of levenberg iterations performed to accept the last step }; } // namespace lightning::miao #endif // MIAO_OPTI_ALGO_LM_H ================================================ FILE: src/core/miao/core/opti_algo/optimization_algorithm.cc ================================================ // // Created by xiang on 24-4-25. // #include "optimization_algorithm.h" #include "core/graph/optimizer.h" #include "core/graph/vertex.h" #include "core/solver/solver.h" namespace lightning::miao { OptimizationAlgorithm::~OptimizationAlgorithm() {} bool OptimizationAlgorithm::Init() { assert(optimizer_ && "optimizer not set"); bool useSchur = false; for (const auto &v : optimizer_->ActiveVertices()) { if (v->Marginalized()) { useSchur = true; break; } } if (useSchur) { if (solver_->SupportsSchur()) { solver_->SetSchur(true); } } else { if (solver_->SupportsSchur()) { solver_->SetSchur(false); } } return solver_->Init(optimizer_); } void OptimizationAlgorithm::SetOptimizer(Optimizer *optimizer) { optimizer_ = optimizer; } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/opti_algo/optimization_algorithm.h ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_OPTIMIZATION_ALGORITHM_H #define MIAO_OPTIMIZATION_ALGORITHM_H #include #include #include #include "common/eigen_types.h" #include "core/common/macros.h" #include "core/sparse/sparse_block_matrix.h" namespace lightning::miao { class Optimizer; class Vertex; class Edge; class Solver; /** * \brief Generic interface for a non-linear Solver operating on a graph * * Algorithm 的操作界面 */ class OptimizationAlgorithm { public: // 求解结果的状态 enum class SolverResult { Terminate = 2, OK = 1, Fail = -1 }; OptimizationAlgorithm(std::shared_ptr solver) { solver_ = solver; } virtual ~OptimizationAlgorithm(); DISALLOW_COPY(OptimizationAlgorithm) /** * initialize the Solver, called once before the first call to Solve() */ virtual bool Init(); /** * Solve one iteration. The SparseOptimizer running on-top will call this * for the given number of iterations. * @param iteration indicates the current iteration */ virtual SolverResult Solve(int iteration) = 0; void SetOptimizer(Optimizer* optimizer); protected: Optimizer* optimizer_ = nullptr; ///< the optimizer the Solver is working on std::shared_ptr solver_; ///< Solver }; } // namespace lightning::miao #endif // MIAO_OPTIMIZATION_ALGORITHM_H ================================================ FILE: src/core/miao/core/robust_kernel/cauchy.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_CAUCHY_H #define MIAO_CAUCHY_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Cauchy cost function * * 2 e * d log(-- + 1) * 2 * d */ class RobustKernelCauchy : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { double dsqr = delta_ * delta_; double dsqrReci = 1. / dsqr; double aux = dsqrReci * e2 + 1.0; rho[0] = dsqr * log(aux); rho[1] = 1. / aux; rho[2] = -dsqrReci * std::pow(rho[1], 2); } }; } // namespace lightning::miao #endif // MIAO_CAUCHY_H ================================================ FILE: src/core/miao/core/robust_kernel/dcs.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_DCS_H #define MIAO_DCS_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Dynamic covariance scaling - DCS * * See paper Robust Map Optimization from Agarwal et al. ICRA 2013 * * Delta is used as $phi$ */ class RobustKernelDCS : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { const double& phi = delta_; double scale = (2.0 * phi) / (phi + e2); if (scale >= 1.0) { // limit scale to max of 1 and return this rho[0] = e2; rho[1] = 1.; rho[2] = 0; } else { double phi_sqr = phi * phi; rho[0] = scale * e2 * scale; rho[1] = (4 * phi_sqr * (phi - e2)) / std::pow(phi + e2, 3); rho[2] = -(8 * phi_sqr * (2 * phi - e2)) / std::pow(phi + e2, 4); } }; }; } // namespace lightning::miao #endif // MIAO_DCS_H ================================================ FILE: src/core/miao/core/robust_kernel/fair.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_FAIR_H #define MIAO_FAIR_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Fair cost function * * See * http://research.microsoft.com/en-us/um/people/zhang/Papers/ZhangIVC-97-01.pdf * * 2 * d^2 [e2 / d - log (1 + e2 / d)] * */ class RobustKernelFair : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { const double sqrte = sqrt(e2); const double dsqr = delta_ * delta_; const double aux = sqrte / delta_; rho[0] = 2. * dsqr * (aux - log1p(aux)); rho[1] = 1. / (1. + aux); const double drec = 1. / delta_; const double e_3_2 = 1. / (sqrte * e2); const double aux2 = drec * sqrte + 1; rho[2] = 2 * dsqr * (1 / (4 * dsqr * aux2 * aux2 * e2) + (drec * e_3_2) / (4 * aux2) - (drec * e_3_2) / 4); } }; } // namespace lightning::miao #endif // MIAO_FAIR_H ================================================ FILE: src/core/miao/core/robust_kernel/huber.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_HUBER_H #define MIAO_HUBER_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Huber Cost Function * * Loss function as described by Huber * See http://en.wikipedia.org/wiki/Huber_loss_function * * If e^(1/2) < d * rho(e) = e * * else * * 1/2 2 * rho(e) = 2 d e - d */ class RobustKernelHuber : public RobustKernel { public: void Robustify(double e, Vector3& rho) const override { double dsqr = delta_ * delta_; if (e <= dsqr) { // inlier rho[0] = e; rho[1] = 1.; rho[2] = 0.; } else { // outlier double sqrte = sqrt(e); // absolute value of the error rho[0] = 2 * sqrte * delta_ - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2 rho[1] = delta_ / sqrte; // rho'(e) = delta / sqrt(e) rho[2] = -0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e } } }; } // namespace lightning::miao #endif // MIAO_HUBER_H ================================================ FILE: src/core/miao/core/robust_kernel/mc_clure.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_MC_CLURE_H #define MIAO_MC_CLURE_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Geman-McClure cost function * * See * http://research.microsoft.com/en-us/um/people/zhang/Papers/ZhangIVC-97-01.pdf * and * http://www2.informatik.uni-freiburg.de/~agarwal/resources/agarwal-thesis.pdf * e2 * ----- * e2 + 1 */ class RobustKernelGemanMcClure : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { const double aux = 1. / (delta_ + e2); rho[0] = delta_ * e2 * aux; rho[1] = delta_ * delta_ * aux * aux; rho[2] = -2. * rho[1] * aux; } }; } // namespace lightning::miao #endif // MIAO_MC_CLURE_H ================================================ FILE: src/core/miao/core/robust_kernel/pseudo_huber.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_PSEUDO_HUBER_H #define MIAO_PSEUDO_HUBER_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Pseudo Huber Cost Function * * The smooth pseudo huber cost function: * See http://en.wikipedia.org/wiki/Huber_loss_function * * 2 e * 2 d (sqrt(-- + 1) - 1) * 2 * d */ class RobustKernelPseudoHuber : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { double dsqr = delta_ * delta_; double dsqrReci = 1. / dsqr; double aux1 = dsqrReci * e2 + 1.0; double aux2 = sqrt(aux1); rho[0] = 2 * dsqr * (aux2 - 1); rho[1] = 1. / aux2; rho[2] = -0.5 * dsqrReci * rho[1] / aux1; } }; } // namespace lightning::miao #endif // MIAO_PSEUDO_HUBER_H ================================================ FILE: src/core/miao/core/robust_kernel/robust_kernel.h ================================================ // // Created by xiang on 24-4-25. // #ifndef MIAO_ROBUST_KERNEL_H #define MIAO_ROBUST_KERNEL_H #include "common/eigen_types.h" #include namespace lightning::miao { /** * \brief base for all robust cost functions * * Note in all the implementations for the other cost functions the e in the * functions corresponds to the squared errors, i.e., the robustification * functions gets passed the squared error. * * e.g. the robustified least squares function is * * chi^2 = sum_{e} rho( e^T Omega e ) */ class RobustKernel { public: RobustKernel() {} explicit RobustKernel(double delta) : delta_(delta) {} virtual ~RobustKernel() = default; /** * compute the scaling factor for a error: * The error is e^T Omega e * The output rho is * rho[0]: The actual scaled error value * rho[1]: First derivative of the scaling function * rho[2]: Second derivative of the scaling function */ virtual void Robustify(double squaredError, lightning::Vec3d& rho) const = 0; /** * set the window size of the error. A squared error above Delta^2 is * considered as outlier in the data. */ virtual void SetDelta(double delta) { delta_ = delta; } double Delta() const { return delta_; } protected: double delta_ = 1.0; }; typedef std::shared_ptr RobustKernelPtr; } // namespace lightning::miao #endif // MIAO_ROBUST_KERNEL_H ================================================ FILE: src/core/miao/core/robust_kernel/robust_kernel_all.h ================================================ // // Created by xiang on 24-5-22. // #ifndef MIAO_ROBUST_KERNEL_ALL_H #define MIAO_ROBUST_KERNEL_ALL_H #include "robust_kernel.h" #include "cauchy.h" #include "dcs.h" #include "fair.h" #include "huber.h" #include "mc_clure.h" #include "pseudo_huber.h" #include "saturated.h" #include "tukey.h" #include "welsch.h" #endif // MIAO_ROBUST_KERNEL_ALL_H ================================================ FILE: src/core/miao/core/robust_kernel/saturated.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_SATURATED_H #define MIAO_SATURATED_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Saturated cost function. * * The error is at most Delta^2 */ class RobustKernelSaturated : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { double dsqr = delta_ * delta_; if (e2 <= dsqr) { // inlier rho[0] = e2; rho[1] = 1.; rho[2] = 0.; } else { // outlier rho[0] = dsqr; rho[1] = 0.; rho[2] = 0.; } } }; } // namespace lightning::miao #endif // MIAO_SATURATED_H ================================================ FILE: src/core/miao/core/robust_kernel/tukey.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_TUKEY_H #define MIAO_TUKEY_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Tukey Cost Function * * See * http://research.microsoft.com/en-us/um/people/zhang/Papers/ZhangIVC-97-01.pdf * * If e2^(1/2) <= d * rho(e) = d^2 * (1 - ( 1 - e2 / d^2)^3) / 3 * * else * * rho(e) = d^2 / 3 */ class RobustKernelTukey : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { const double delta2 = delta_ * delta_; if (e2 <= delta2) { const double aux = e2 / delta2; rho[0] = delta2 * (1. - std::pow((1. - aux), 3)) / 3.; rho[1] = std::pow((1. - aux), 2); rho[2] = -2. * (1. - aux) / delta2; } else { rho[0] = delta2 / 3.; rho[1] = 0; rho[2] = 0; } } }; } // namespace lightning::miao #endif // MIAO_TUKEY_H ================================================ FILE: src/core/miao/core/robust_kernel/welsch.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_WELSCH_H #define MIAO_WELSCH_H #include "robust_kernel.h" namespace lightning::miao { /** * \brief Welsch cost function * * See * http://research.microsoft.com/en-us/um/people/zhang/Papers/ZhangIVC-97-01.pdf * * d^2 [1 - exp(- e2/d^2)] * */ class RobustKernelWelsch : public RobustKernel { public: virtual void Robustify(double e2, Vector3& rho) const override { const double dsqr = delta_ * delta_; const double aux = e2 / dsqr; const double aux2 = exp(-aux); rho[0] = dsqr * (1. - aux2); rho[1] = aux2; rho[2] = -aux2 / dsqr; } }; } // namespace lightning::miao #endif // MIAO_WELSCH_H ================================================ FILE: src/core/miao/core/solver/block_solver.h ================================================ // // Created by xiang on 24-4-26. // #ifndef MIAO_BLOCK_SOLVER_H #define MIAO_BLOCK_SOLVER_H /// 模板化的solver /// Block solver 是 Solver 的子类，实际完成solver中的各种计算 /// 同时持有linear solver的指针，用linear solver计算Ax=GetB /// NOTE: 在没有pose, landmark概念的问题中，尽量不要定义pose, landmark的概念 #include "core/common/config.h" #include "core/sparse/sparse_block_matrix_diagonal.h" #include "linear_solver.h" #include "solver.h" #include namespace lightning::miao { /** * \brief traits to summarize the properties of the fixed size optimization * problem */ template struct BlockSolverTraits { static const int PoseDim = _PoseDim; static const int LandmarkDim = _LandmarkDim; typedef Eigen::Matrix PoseMatrixType; typedef Eigen::Matrix LandmarkMatrixType; typedef Eigen::Matrix PoseLandmarkMatrixType; typedef Eigen::Matrix PoseVectorType; typedef Eigen::Matrix LandmarkVectorType; typedef SparseBlockMatrix PoseHessianType; typedef SparseBlockMatrix LandmarkHessianType; typedef SparseBlockMatrix PoseLandmarkHessianType; typedef LinearSolver LinearSolverType; }; /// Block Solver X 的特化 /** * \brief traits to summarize the properties of the dynamic size optimization * problem */ template <> struct BlockSolverTraits { static const int PoseDim = Eigen::Dynamic; static const int LandmarkDim = Eigen::Dynamic; typedef lightning::MatrixX PoseMatrixType; typedef lightning::MatrixX LandmarkMatrixType; typedef lightning::MatrixX PoseLandmarkMatrixType; typedef lightning::VectorX PoseVectorType; typedef lightning::VectorX LandmarkVectorType; typedef SparseBlockMatrix PoseHessianType; typedef SparseBlockMatrix LandmarkHessianType; typedef SparseBlockMatrix PoseLandmarkHessianType; typedef LinearSolver LinearSolverType; }; /** * \brief Implementation of a Solver operating on the blocks of the Hessian * * 带有pose和landmark的概念，默认Landmark会被marg，而pose不会 * 在没有pose, landmark概念的问题中，可以简单地以是否marg来区分pose或landmark */ template class BlockSolver : public Solver { public: static const int PoseDim = Traits::PoseDim; static const int LandmarkDim = Traits::LandmarkDim; typedef typename Traits::PoseMatrixType PoseMatrixType; typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType; typedef typename Traits::PoseVectorType PoseVectorType; typedef typename Traits::LandmarkVectorType LandmarkVectorType; typedef typename Traits::PoseHessianType PoseHessianType; typedef typename Traits::LandmarkHessianType LandmarkHessianType; typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType; typedef typename Traits::LinearSolverType LinearSolverType; public: /** * allocate a Block solver ontop of the underlying linear Solver. * NOTE: The BlockSolver assumes exclusive access to the linear Solver and * will therefore free the pointer in its destructor. */ explicit BlockSolver(std::unique_ptr linearSolver); ~BlockSolver() override; bool Init(Optimizer* optimizer) override; /// 初始化时调用，建立各矩阵块的数据 bool BuildStructure(bool zeroBlocks) override; /// 原生模式下建立问题结构 bool BuildStructureFromRaw(bool zero_blocks); /// 增量模式下建立问题结构 bool BuildStructureInc(bool zero_blocks); /// 每次迭代时调用的函数,计算当前这次迭代中的jacobian bool BuildSystem() override; /// 建立H, b矩阵并交给linear solver bool Solve() override; bool SetLambda(double lambda, bool backup) override; void RestoreDiagonal() override; bool SupportsSchur() override { return true; } bool Schur() override { return do_schur_; } void SetSchur(bool s) override { do_schur_ = s; } void MultiplyHessian(double* dest, const double* src) const override { Hpp_->MultiplySymmetricUpperTriangle(dest, src); } protected: void Resize(const std::vector& blockPoseIndices, const std::vector& blockLandmarkIndices, int totalDim); void Deallocate(); OptimizerConfig config_; /// pose to pose 的矩阵块，解稠密问题，只求解这一部分的计算 std::unique_ptr> Hpp_; // pose to pose std::unique_ptr> Hll_; // landmark to landmark std::unique_ptr> Hpl_; // pose to landmark std::unique_ptr> Hschur_; std::unique_ptr> D_inv_schur_; std::unique_ptr> Hpl_CCS_; std::unique_ptr> H_schur_transpose_CCS_; std::unique_ptr linear_solver_; std::vector diagonal_backup_pose_; std::vector diagonal_backup_landmark_; bool do_schur_ = true; lightning::VecXd coeffs_; lightning::VecXd b_schur_; int num_poses_ = 0, num_landmarks_ = 0; int size_poses_ = 0, size_landmarks_ = 0; }; template using BlockSolverPL = BlockSolver>; } // namespace lightning::miao #include "block_solver.hpp" #endif // MIAO_BLOCK_SOLVER_H ================================================ FILE: src/core/miao/core/solver/block_solver.hpp ================================================ // // Created by xiang on 24-4-26. // #include #include #include "core/graph/edge.h" #include "core/graph/vertex.h" #include "utils/timer.h" namespace lightning::miao { template BlockSolver::BlockSolver(std::unique_ptr linearSolver) : Solver(), linear_solver_(std::move(linearSolver)) { // workspace x_size_ = 0; num_poses_ = 0; num_landmarks_ = 0; size_poses_ = 0; size_landmarks_ = 0; do_schur_ = true; } template void BlockSolver::Resize(const std::vector& blockPoseIndices, const std::vector& blockLandmarkIndices, int s) { Deallocate(); ResizeVector(s); if (do_schur_) { // the following two are only used in schur assert(size_poses_ > 0 && "allocating with wrong size"); coeffs_.setZero(s); b_schur_.setZero(size_poses_); } Hpp_ = std::make_unique(blockPoseIndices, blockPoseIndices); if (do_schur_) { Hschur_ = std::make_unique(blockPoseIndices, blockPoseIndices); Hll_ = std::make_unique(blockLandmarkIndices, blockLandmarkIndices); D_inv_schur_ = std::make_unique>(Hll_->ColBlockIndices()); Hpl_ = std::make_unique(blockPoseIndices, blockLandmarkIndices); Hpl_CCS_ = std::make_unique>(Hpl_->RowBlockIndices(), Hpl_->ColBlockIndices()); H_schur_transpose_CCS_ = std::make_unique>(Hschur_->ColBlockIndices(), Hschur_->RowBlockIndices()); } } template void BlockSolver::Deallocate() { Hpp_.reset(); Hll_.reset(); Hpl_.reset(); Hschur_.reset(); D_inv_schur_.reset(); coeffs_.setZero(); b_schur_.setZero(); Hpl_CCS_.reset(); H_schur_transpose_CCS_.reset(); } template BlockSolver::~BlockSolver() = default; template bool BlockSolver::BuildStructure(bool zeroBlocks) { assert(optimizer_); if (optimizer_ == nullptr) { return false; } if (config_.incremental_mode_) { do_schur_ = false; BuildStructureInc(zeroBlocks); } else { BuildStructureFromRaw(zeroBlocks); } return true; } template bool BlockSolver::BuildStructureFromRaw(bool zero_blocks) { size_t sparseDim = 0; num_poses_ = 0; num_landmarks_ = 0; size_poses_ = 0; size_landmarks_ = 0; int num_all_vertex = optimizer_->IndexMapping().size(); std::vector blockPoseIndices; std::vector blockLandmarkIndices; blockPoseIndices.reserve(num_all_vertex); blockLandmarkIndices.reserve(num_all_vertex); for (const auto& v : optimizer_->IndexMapping()) { int dim = v->Dimension(); if (!v->Marginalized()) { v->SetColInHessian(size_poses_); size_poses_ += dim; blockPoseIndices.emplace_back(size_poses_); ++num_poses_; } else { v->SetColInHessian(size_landmarks_); size_landmarks_ += dim; blockLandmarkIndices.emplace_back(size_landmarks_); ++num_landmarks_; } sparseDim += dim; } Resize(blockPoseIndices, blockLandmarkIndices, sparseDim); // allocate the diagonal on Hpp and Hll int poseIdx = 0; int landmarkIdx = 0; for (const auto& v : optimizer_->IndexMapping()) { if (!v->Marginalized()) { PoseMatrixType* m = Hpp_->Block(poseIdx, poseIdx, true); if (zero_blocks) { m->setZero(); } v->MapHessianMemory(m->data()); ++poseIdx; } else { LandmarkMatrixType* m = Hll_->Block(landmarkIdx, landmarkIdx, true); if (zero_blocks) { m->setZero(); } v->MapHessianMemory(m->data()); ++landmarkIdx; } } assert(poseIdx == num_poses_ && landmarkIdx == num_landmarks_); // temporary structures for building the pattern of the Schur complement SparseBlockMatrixHashMap* schurMatrixLookup = nullptr; if (do_schur_) { schurMatrixLookup = new SparseBlockMatrixHashMap(Hschur_->RowBlockIndices(), Hschur_->ColBlockIndices()); schurMatrixLookup->BlockCols().resize(Hschur_->BlockCols().size()); } // here we assume that the landmark indices start after the pose ones // create the structure in Hpp, Hll and in Hpl for (const auto& e : optimizer_->ActiveEdges()) { for (size_t viIdx = 0; viIdx < e->GetVertices().size(); ++viIdx) { auto v1 = e->GetVertex(viIdx); int ind1 = v1->HessianIndex(); if (ind1 == -1) { continue; } int indexV1Bak = ind1; for (size_t vjIdx = viIdx + 1; vjIdx < e->GetVertices().size(); ++vjIdx) { auto v2 = e->GetVertex(vjIdx); int ind2 = v2->HessianIndex(); if (ind2 == -1) { continue; } ind1 = indexV1Bak; bool transposedBlock = ind1 > ind2; if (transposedBlock) { // make sure, we allocate the upper triangle // block std::swap(ind1, ind2); } if (!v1->Marginalized() && !v2->Marginalized()) { PoseMatrixType* m = Hpp_->Block(ind1, ind2, true); if (zero_blocks) { m->setZero(); } e->MapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock); if (Hschur_) { // assume this is only needed in case we solve with // the schur complement schurMatrixLookup->AddBlock(ind1, ind2); } } else if (v1->Marginalized() && v2->Marginalized()) { // RAINER hmm.... should we ever reach this here???? LandmarkMatrixType* m = Hll_->Block(ind1 - num_poses_, ind2 - num_poses_, true); if (zero_blocks) { m->setZero(); } e->MapHessianMemory(m->data(), viIdx, vjIdx, false); } else { if (v1->Marginalized()) { PoseLandmarkMatrixType* m = Hpl_->Block(v2->HessianIndex(), v1->HessianIndex() - num_poses_, true); if (zero_blocks) { m->setZero(); } e->MapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it } else { PoseLandmarkMatrixType* m = Hpl_->Block(v1->HessianIndex(), v2->HessianIndex() - num_poses_, true); if (zero_blocks) { m->setZero(); } e->MapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block } } } } } if (!do_schur_) { delete schurMatrixLookup; return true; } D_inv_schur_->Diagonal().resize(landmarkIdx); Hpl_->FillSparseBlockMatrixCCS(*Hpl_CCS_); /// 啧啧 for (const auto& v : optimizer_->IndexMapping()) { if (!v->Marginalized()) { continue; } const auto& vedges = v->GetEdges(); for (auto& e : vedges) { for (size_t i = 0; i < e->GetVertices().size(); ++i) { auto v1 = e->GetVertex(i); if (v1->HessianIndex() == -1 || v1 == v) { continue; } for (auto& e2 : vedges) { for (size_t j = 0; j < e2->GetVertices().size(); ++j) { auto v2 = e2->GetVertex(j); if (v2->HessianIndex() == -1 || v2 == v) { continue; } int i1 = v1->HessianIndex(); int i2 = v2->HessianIndex(); if (i1 <= i2) { schurMatrixLookup->AddBlock(i1, i2); } } } } } } Hschur_->TakePatternFromHash(*schurMatrixLookup); delete schurMatrixLookup; Hschur_->FillSparseBlockMatrixCCSTransposed(*H_schur_transpose_CCS_); return true; } template bool BlockSolver::BuildStructureInc(bool zero_blocks) { /// 增量模式下没有landmark，且省略历史过程中的index building size_t sparseDim = size_poses_; int num_all_vertex = optimizer_->IndexMapping().size(); /// for inc std::vector new_block_pose_indices; new_block_pose_indices.reserve(num_all_vertex); int last_num_poses = num_poses_; for (const auto& v : optimizer_->NewVertices()) { int dim = v->Dimension(); assert(v->Marginalized() == false); if (v->Fixed()) { continue; } v->SetColInHessian(size_poses_); size_poses_ += dim; new_block_pose_indices.emplace_back(size_poses_); ++num_poses_; sparseDim += dim; } ResizeVector(sparseDim); if (Hpp_ == nullptr) { Hpp_ = std::make_unique(new_block_pose_indices, new_block_pose_indices); } else { Hpp_->SetBlockIndexInc(new_block_pose_indices, new_block_pose_indices); if (config_.incremental_mode_ && config_.max_vertex_size_ >= 0) { /// Hpp 不清空的话影响lambda之类的求解 Hpp_->Clear(); } } // allocate the diagonal on Hpp and Hll int poseIdx = last_num_poses; for (const auto& v : optimizer_->NewVertices()) { if (v->Fixed()) { continue; } PoseMatrixType* m = Hpp_->Block(poseIdx, poseIdx, true); if (zero_blocks) { m->setZero(); } v->MapHessianMemory(m->data()); ++poseIdx; } assert(poseIdx == num_poses_); for (const auto& e : optimizer_->NewEdges()) { for (size_t vi_idx = 0; vi_idx < e->GetVertices().size(); ++vi_idx) { auto v1 = e->GetVertex(vi_idx); int ind1 = v1->HessianIndex(); if (ind1 == -1) { continue; } int indexV1Bak = ind1; for (size_t vj_idx = vi_idx + 1; vj_idx < e->GetVertices().size(); ++vj_idx) { auto v2 = e->GetVertex(vj_idx); int ind2 = v2->HessianIndex(); if (ind2 == -1) { continue; } ind1 = indexV1Bak; bool transposedBlock = ind1 > ind2; if (transposedBlock) { // make sure, we allocate the upper triangle // block std::swap(ind1, ind2); } PoseMatrixType* m = Hpp_->Block(ind1, ind2, true); if (zero_blocks) { m->setZero(); } e->MapHessianMemory(m->data(), vi_idx, vj_idx, transposedBlock); } } } optimizer_->ClearNewElements(); return true; } template bool BlockSolver::Solve() { if (!do_schur_) { return linear_solver_->Solve(*Hpp_, x_.data(), b_.data()); } // schur thing Hschur_->Clear(); Hpp_->Add(*Hschur_); coeffs_.setZero(); std::vector landmark_idx_vec(Hll_->BlockCols().size()); std::iota(landmark_idx_vec.begin(), landmark_idx_vec.end(), 0); std::vector mutex_b(num_landmarks_); std::vector mutex_H(num_poses_ * num_poses_); std::for_each(std::execution::par_unseq, landmark_idx_vec.begin(), landmark_idx_vec.end(), [&](int landmarkIndex) { const typename SparseBlockMatrix::IntBlockMap& marginalizeColumn = Hll_->BlockCols()[landmarkIndex]; assert(marginalizeColumn.size() == 1 && "more than one Block in _Hll column"); // calculate inverse block for the landmark const LandmarkMatrixType* D = marginalizeColumn.begin()->second; assert(D && D->rows() == D->cols() && "Error in landmark matrix"); LandmarkMatrixType& Dinv = D_inv_schur_->Diagonal()[landmarkIndex]; Dinv = D->inverse(); LandmarkVectorType db(D->rows()); for (int j = 0; j < D->rows(); ++j) { db[j] = b_[Hll_->RowBaseOfBlock(landmarkIndex) + size_poses_ + j]; } db = Dinv * db; assert((size_t)landmarkIndex < Hpl_CCS_->BlockCols().size() && "Index out of bounds"); const typename SparseBlockMatrixCCS::SparseColumn& landmarkColumn = Hpl_CCS_->BlockCols()[landmarkIndex]; for (auto it_outer = landmarkColumn.begin(); it_outer != landmarkColumn.end(); ++it_outer) { int i1 = it_outer->row; const PoseLandmarkMatrixType* Bi = it_outer->block; assert(Bi); PoseLandmarkMatrixType BDinv = (*Bi) * (Dinv); // 右侧b部分 { int b_index = Hpl_CCS_->RowBaseOfBlock(i1); assert(b_index >= 0 && b_index < num_landmarks_); lightning::UL lock(mutex_b[b_index]); typename PoseVectorType::MapType Bb(&coeffs_[b_index], Bi->rows()); Bb.noalias() += (*Bi) * db; } assert(i1 >= 0 && i1 < static_cast(H_schur_transpose_CCS_->BlockCols().size()) && "Index out of bounds"); typename SparseBlockMatrixCCS::SparseColumn::iterator targetColumnIt = H_schur_transpose_CCS_->BlockCols()[i1].begin(); typename SparseBlockMatrixCCS::RowBlock aux(i1, 0); typename SparseBlockMatrixCCS::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux); for (; it_inner != landmarkColumn.end(); ++it_inner) { int i2 = it_inner->row; const PoseLandmarkMatrixType* Bj = it_inner->block; assert(Bj); while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/) { ++targetColumnIt; } assert(targetColumnIt != H_schur_transpose_CCS_->BlockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure"); lightning::UL lock(mutex_H[i1 * num_poses_ + i2]); PoseMatrixType* Hi1i2 = targetColumnIt->block; assert(Hi1i2); (*Hi1i2).noalias() -= BDinv * Bj->transpose(); } } }); memcpy(b_schur_.data(), b_.data(), size_poses_ * sizeof(double)); for (int i = 0; i < size_poses_; ++i) { b_schur_[i] -= coeffs_[i]; } /// linear solver 只解pose部分 bool solvedPoses = true; solvedPoses = linear_solver_->Solve(*Hschur_, x_.data(), b_schur_.data()); if (!solvedPoses) { return false; } // _x contains the solution for the poses, now applying it to the landmarks to // get the new part of the solution; double* xp = x_.data(); double* cp = coeffs_.data(); double* xl = x_.data() + size_poses_; double* cl = coeffs_.data() + size_poses_; double* bl = b_.data() + size_poses_; // cp = -xp for (int i = 0; i < size_poses_; ++i) { cp[i] = -xp[i]; } // cl = bl memcpy(cl, bl, size_landmarks_ * sizeof(double)); // cl = bl - Bt * xp // Bt->multiply(cl, cp); Hpl_CCS_->RightMultiply(cl, cp); // xl = Dinv * cl memset(xl, 0, size_landmarks_ * sizeof(double)); D_inv_schur_->Multiply(xl, cl); return true; } template bool BlockSolver::BuildSystem() { Hpp_->Clear(); if (config_.parallel_) { // clear b vector std::for_each(std::execution::par_unseq, optimizer_->IndexMapping().begin(), optimizer_->IndexMapping().end(), [](const auto& v) { v->ClearQuadraticForm(); }); std::for_each(std::execution::par_unseq, optimizer_->ActiveEdges().begin(), optimizer_->ActiveEdges().end(), [](const auto& e) { /// 线性化，计算jacobian，可以并行 e->LinearizeOplus(); // 求AtA 和 AtB ，这个也可以并行，但需要锁每个block e->ConstructQuadraticForm(); // 将边的Hessian拷回大的hessian阵 e->CopyHessianToSolver(); }); // 将顶点的hessian拷回大的hessian阵 std::for_each(std::execution::par_unseq, optimizer_->ActiveVertices().begin(), optimizer_->ActiveVertices().end(), [](const auto& v) { v->CopyHessianToSolver(); }); } else { // clear b vector std::for_each(std::execution::seq, optimizer_->IndexMapping().begin(), optimizer_->IndexMapping().end(), [](const auto& v) { v->ClearQuadraticForm(); }); std::for_each(std::execution::seq, optimizer_->ActiveEdges().begin(), optimizer_->ActiveEdges().end(), [](const auto& e) { /// 线性化，计算jacobian，可以并行 e->LinearizeOplus(); // 求AtA 和 AtB ，这个也可以并行，但需要锁每个block e->ConstructQuadraticForm(); // 将边的Hessian拷回大的hessian阵 e->CopyHessianToSolver(); }); // 将顶点的hessian拷回大的hessian阵 std::for_each(std::execution::seq, optimizer_->ActiveVertices().begin(), optimizer_->ActiveVertices().end(), [](const auto& v) { v->CopyHessianToSolver(); }); } // flush the current system in a sparse block matrix // 把每个vertex部分中的b拷至整个b中 for (const auto& v : optimizer_->IndexMapping()) { int iBase = v->ColInHessian(); if (v->Marginalized()) { iBase += size_poses_; } v->CopyB(b_.data() + iBase); } return true; } template bool BlockSolver::SetLambda(double lambda, bool backup) { if (backup) { diagonal_backup_pose_.resize(num_poses_); diagonal_backup_landmark_.resize(num_landmarks_); } for (int i = 0; i < num_poses_; ++i) { PoseMatrixType* b = Hpp_->Block(i, i); assert(b != nullptr); if (backup) { diagonal_backup_pose_[i] = b->diagonal(); } b->diagonal().array() += lambda; } for (int i = 0; i < num_landmarks_; ++i) { LandmarkMatrixType* b = Hll_->Block(i, i); if (backup) { diagonal_backup_landmark_[i] = b->diagonal(); } b->diagonal().array() += lambda; } return true; } template void BlockSolver::RestoreDiagonal() { assert((int)diagonal_backup_pose_.size() == num_poses_ && "Mismatch in dimensions"); assert((int)diagonal_backup_landmark_.size() == num_landmarks_ && "Mismatch in dimensions"); for (int i = 0; i < num_poses_; ++i) { PoseMatrixType* b = Hpp_->Block(i, i); b->diagonal() = diagonal_backup_pose_[i]; } for (int i = 0; i < num_landmarks_; ++i) { LandmarkMatrixType* b = Hll_->Block(i, i); b->diagonal() = diagonal_backup_landmark_[i]; } } template bool BlockSolver::Init(Optimizer* optimizer) { optimizer_ = optimizer; config_ = optimizer->GetConfig(); if (!config_.incremental_mode_) { /// 不在增量模式下，就清空整个Hessian if (Hpp_) { Hpp_->Clear(); } if (Hpl_) { Hpl_->Clear(); } if (Hll_) { Hll_->Clear(); } } linear_solver_->Init(); return true; } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/solver/linear_solver.h ================================================ // // Created by xiang on 24-4-26. // #ifndef MIAO_LINEAR_SOLVER_H #define MIAO_LINEAR_SOLVER_H #include "core/sparse/sparse_block_matrix.h" namespace lightning::miao { /// 线性方程求解器的基类 /// 默认就是稀疏矩阵 /** * \brief basic Solver for Ax = b * * basic Solver for Ax = b which has to reimplemented for different linear * algebra libraries. A is assumed to be symmetric (only upper triangular Block * is stored) and positive-semi-definite. */ /// TODO 对于dense的问题，默认的linear solver多一步拷贝 template class LinearSolver { public: LinearSolver() = default; virtual ~LinearSolver() = default; /** * Init for operating on matrices with a different non-zero pattern like * before */ virtual bool Init() = 0; /** * Assumes that A is the same matrix for several calls. * Among other assumptions, the non-zero pattern does not change! * If the matrix changes call Init() before. * Solve system Ax = b, x and b have to allocated beforehand!! */ virtual bool Solve(const SparseBlockMatrix& A, double* x, double* b) = 0; /** * Dense solve version * @param A Dense matrix A * @param x * @param b * @return */ virtual bool Solve(const lightning::MatXd& A, double* x, double* b) { return true; } /** * Convert a Block permutation matrix to a scalar permutation */ template static void BlockToScalarPermutation(const SparseBlockMatrix& A, const Eigen::MatrixBase& p, const Eigen::MatrixBase& scalar /* output */) { int n = A.Cols(); auto& scalarPermutation = const_cast&>(scalar); if (scalarPermutation.size() == 0) { scalarPermutation.derived().resize(n); } if (scalarPermutation.size() < n) { scalarPermutation.derived().resize(2 * n); } size_t scalarIdx = 0; for (size_t i = 0; i < A.ColBlockIndices().size(); ++i) { int base = A.ColBaseOfBlock(p(i)); int nCols = A.ColsOfBlock(p(i)); for (int j = 0; j < nCols; ++j) { scalarPermutation(scalarIdx++) = base++; } } assert((int)scalarIdx == n); } }; } // namespace lightning::miao #endif // MIAO_LINEAR_SOLVER_H ================================================ FILE: src/core/miao/core/solver/linear_solver_ccs.h ================================================ // // Created by xiang on 24-4-26. // #ifndef MIAO_LINEAR_SOLVER_CCS_H #define MIAO_LINEAR_SOLVER_CCS_H #include "core/math/marginal_covariance_cholesky.h" #include "linear_solver.h" #include namespace lightning::miao { /** * \brief Solver with faster iterating structure for the linear matrix */ template class LinearSolverCCS : public LinearSolver { public: LinearSolverCCS() : LinearSolver() {} ~LinearSolverCCS() {} //! do the AMD ordering on the blocks or on the scalar matrix bool BlockOrdering() const { return block_ordering_; } void SetBlockOrdering(bool blockOrdering) { block_ordering_ = blockOrdering; } protected: void InitMatrixStructure(const SparseBlockMatrix& A) { ccs_matrix_ = std::make_shared>(A.RowBlockIndices(), A.ColBlockIndices()); A.FillSparseBlockMatrixCCS(*ccs_matrix_); } std::shared_ptr> ccs_matrix_ = nullptr; bool block_ordering_ = true; }; } // namespace lightning::miao #endif // MIAO_LINEAR_SOLVER_CCS_H ================================================ FILE: src/core/miao/core/solver/linear_solver_dense.h ================================================ // // Created by xiang on 24-4-28. // #ifndef MIAO_LINEAR_SOLVER_DENSE_H #define MIAO_LINEAR_SOLVER_DENSE_H #include "linear_solver.h" namespace lightning::miao { /** * \brief linear Solver using dense cholesky decomposition */ template class LinearSolverDense : public LinearSolver { public: LinearSolverDense() : LinearSolver() {} virtual ~LinearSolverDense() = default; bool Init() override { reset_ = true; return true; } bool Solve(const SparseBlockMatrix& A, double* x, double* b) override { int n = A.Cols(); int m = A.Cols(); MatrixX& H = H_; if (H.cols() != n) { H.resize(n, m); reset_ = true; } if (reset_) { reset_ = false; H.setZero(); } // copy the sparse block matrix into a dense matrix int c_idx = 0; for (size_t i = 0; i < A.BlockCols().size(); ++i) { int c_size = A.ColsOfBlock(i); assert(c_idx == A.ColBaseOfBlock(i) && "mismatch in Block indices"); const typename SparseBlockMatrix::IntBlockMap& col = A.BlockCols()[i]; if (col.size() > 0) { typename SparseBlockMatrix::IntBlockMap::const_iterator it; for (it = col.begin(); it != col.end(); ++it) { int r_idx = A.RowBaseOfBlock(it->first); // only the upper triangular block is processed if (it->first <= (int)i) { int r_size = A.RowsOfBlock(it->first); H.block(r_idx, c_idx, r_size, c_size) = *(it->second); if (r_idx != c_idx) { // write the lower triangular block H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose(); } } } } c_idx += c_size; } // solving via Cholesky decomposition VectorX::MapType xvec(x, m); VectorX::ConstMapType bvec(b, n); cholesky_.compute(H); if (cholesky_.isPositive()) { xvec = cholesky_.solve(bvec); return true; } return false; } protected: bool reset_ = true; MatrixX H_; Eigen::LDLT cholesky_; }; } // namespace lightning::miao #endif // MIAO_LINEAR_SOLVER_DENSE_H ================================================ FILE: src/core/miao/core/solver/linear_solver_eigen.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_LINEAR_SOLVER_EIGEN_H #define MIAO_LINEAR_SOLVER_EIGEN_H #include #include #include "linear_solver_ccs.h" #include "utils/timer.h" namespace lightning::miao { /** * \brief linear solver which uses the sparse Cholesky Solver from Eigen * * Has no dependencies except Eigen. Hence, should compile almost everywhere * without to much issues. Performance should be similar to CSparse. */ template class LinearSolverEigen : public LinearSolverCCS { public: typedef Eigen::SparseMatrix SparseMatrix; typedef Eigen::Triplet Triplet; typedef Eigen::PermutationMatrix PermutationMatrix; using CholeskyDecompositionBase = Eigen::SimplicialLLT; /** * \brief Sub-classing Eigen's SimplicialLLT to perform ordering with a given * ordering */ class CholeskyDecomposition : public CholeskyDecompositionBase { public: CholeskyDecomposition() : CholeskyDecompositionBase() {} //! use a given permutation for analyzing the pattern of the sparse matrix void AnalyzePatternWithPermutation(SparseMatrix &a, const PermutationMatrix &permutation) { m_Pinv = permutation; m_P = permutation.inverse(); int size = a.cols(); SparseMatrix ap(size, size); ap.selfadjointView() = a.selfadjointView().twistedBy(m_P); analyzePattern_preordered(ap, false); } protected: using CholeskyDecompositionBase::analyzePattern_preordered; }; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW LinearSolverEigen() : LinearSolverCCS() {} bool Init() override { init_ = true; return true; } bool Solve(const SparseBlockMatrix &A, double *x, double *b) override { bool cholState = false; cholState = ComputeCholesky(A); if (!cholState) { return false; } // Solving the system VectorX::MapType xx(x, sparse_matrix_.cols()); VectorX::ConstMapType bb(b, sparse_matrix_.cols()); xx = cholesky_.solve(bb); return true; } protected: bool init_ = true; SparseMatrix sparse_matrix_; CholeskyDecomposition cholesky_; // compute the cholesky factor bool ComputeCholesky(const SparseBlockMatrix &A) { // perform some operations only once upon init if (init_) { sparse_matrix_.resize(A.Rows(), A.Cols()); } FillSparseMatrix(A, !init_); if (init_) { ComputeSymbolicDecomposition(A); } init_ = false; cholesky_.factorize(sparse_matrix_); if (cholesky_.info() != Eigen::Success) { // the matrix is not positive definite LOG(ERROR) << "error : Cholesky failure, solve failed---------------"; return false; } return true; } /** * compute the symbolic decomposition of the matrix only once. * Since A has the same pattern in all the iterations, we only * compute the fill-in reducing ordering once and re-use for all * the following iterations. */ void ComputeSymbolicDecomposition(const SparseBlockMatrix &A) { if (!this->BlockOrdering()) { cholesky_.analyzePattern(sparse_matrix_); } else { assert(A.Rows() == A.Cols() && "Matrix A is not square"); // block ordering with the Eigen Interface Eigen::PermutationMatrix blockP; { SparseMatrix auxBlockMatrix(A.BlockCols().size(), A.BlockCols().size()); auxBlockMatrix.resizeNonZeros(A.NonZeroBlocks()); // fill the CCS structure of the Eigen SparseMatrix A.FillBlockStructure(auxBlockMatrix.outerIndexPtr(), auxBlockMatrix.innerIndexPtr()); // determine ordering by AMD using Ordering = Eigen::AMDOrdering; Ordering ordering; ordering(auxBlockMatrix, blockP); } // Adapt the block permutation to the scalar matrix PermutationMatrix scalarP(A.Rows()); this->BlockToScalarPermutation(A, blockP.indices(), scalarP.indices()); // analyze with the scalar permutation cholesky_.AnalyzePatternWithPermutation(sparse_matrix_, scalarP); } } void FillSparseMatrix(const SparseBlockMatrix &A, bool onlyValues) { if (onlyValues) { this->ccs_matrix_->FillCCS(sparse_matrix_.valuePtr(), true); return; } this->InitMatrixStructure(A); sparse_matrix_.resizeNonZeros(A.NonZeros()); int nz = this->ccs_matrix_->FillCCS(sparse_matrix_.outerIndexPtr(), sparse_matrix_.innerIndexPtr(), sparse_matrix_.valuePtr(), true); assert(nz <= static_cast(sparse_matrix_.data().size())); } }; } // namespace lightning::miao #endif // MIAO_LINEAR_SOLVER_EIGEN_H ================================================ FILE: src/core/miao/core/solver/linear_solver_pcg.h ================================================ // // Created by xiang on 24-5-13. // #ifndef MIAO_LINEAR_SOLVER_PCG_H #define MIAO_LINEAR_SOLVER_PCG_H #include "core/math/misc.h" #include "linear_solver.h" namespace lightning::miao { /** * \brief linear Solver using PCG, pre-conditioner is Block Jacobi */ template class LinearSolverPCG : public LinearSolver { public: LinearSolverPCG() : LinearSolver() {} virtual ~LinearSolverPCG(); virtual bool Init() { residual_ = -1.0; indices_by_row_.clear(); indices_by_col_.clear(); return true; } bool Solve(const SparseBlockMatrix& A, double* x, double* b); protected: using MatrixVector = std::vector; using MatrixPtrVector = std::vector; double tolerance_ = cst(1e-6); double residual_ = -1.0; bool abs_tolerance_ = true; int max_iter_ = 100; /// A的对角线部分 MatrixPtrVector diag_; // A阵的对角线部分 MatrixVector J_; // A阵对角线部分之逆 /// A的非对角线部分 struct MatAndIdx { int idx_ = 0; // 索引为行时，此处填列的id,索引为列时，此处填行 MatrixType* mat_; }; std::map> indices_by_row_; // 以行为索引的稀疏矩阵块 std::map> indices_by_col_; // 以列为索引的稀疏矩阵块 void MultDiag(const std::vector& colBlockIndices, MatrixVector& A, const VectorX& src, VectorX& dest); void MultDiag(const std::vector& colBlockIndices, MatrixPtrVector& A, const VectorX& src, VectorX& dest); void Mult(const std::vector& colBlockIndices, const VectorX& src, VectorX& dest); }; template LinearSolverPCG::~LinearSolverPCG() {} } // namespace lightning::miao #include "linear_solver_pcg.hpp" #endif // MIAO_LINEAR_SOLVER_PCG_H ================================================ FILE: src/core/miao/core/solver/linear_solver_pcg.hpp ================================================ #include namespace lightning::miao { namespace internal { /** * y=Ax */ template inline void pcg_axy(const MatrixType& A, const VectorX& x, int xoff, VectorX& y, int yoff) { y.segment(yoff) = A * x.segment(xoff); } template <> inline void pcg_axy(const MatrixX& A, const VectorX& x, int xoff, VectorX& y, int yoff) { y.segment(yoff, A.rows()) = A * x.segment(xoff, A.cols()); } template inline void pcg_axpy(const MatrixType& A, const VectorX& x, int xoff, VectorX& y, int yoff) { y.segment(yoff) += A * x.segment(xoff); } template <> inline void pcg_axpy(const MatrixX& A, const VectorX& x, int xoff, VectorX& y, int yoff) { y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols()); } template inline void pcg_atxpy(const MatrixType& A, const VectorX& x, int xoff, VectorX& y, int yoff) { y.segment(yoff) += A.transpose() * x.segment(xoff); } template <> inline void pcg_atxpy(const MatrixX& A, const VectorX& x, int xoff, VectorX& y, int yoff) { y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows()); } } // namespace internal // helpers end template bool LinearSolverPCG::Solve(const SparseBlockMatrix& A, double* x, double* b) { const bool indexRequired = indices_by_row_.size() == 0; diag_.clear(); J_.clear(); // put the block matrix once in a linear structure, makes mult faster int colIdx = 0; for (size_t i = 0; i < A.BlockCols().size(); ++i) { auto& col = A.BlockCols()[i]; if (col.size() > 0) { for (auto it = col.begin(); it != col.end(); ++it) { if (it->first == (int)i) { // only the upper triangular block is needed diag_.push_back(it->second); J_.push_back(it->second->inverse()); break; } if (indexRequired) { int row_idx = it->first > 0 ? A.RowBlockIndices()[it->first - 1] : 0; MatAndIdx m; m.idx_ = colIdx; m.mat_ = it->second; auto iter = indices_by_row_.find(row_idx); if (iter == indices_by_row_.end()) { std::vector data{m}; indices_by_row_.emplace(row_idx, data); } else { iter->second.emplace_back(m); } iter = indices_by_col_.find(colIdx); m.idx_ = row_idx; if (iter == indices_by_col_.end()) { std::vector data{m}; indices_by_col_.emplace(colIdx, data); } else { iter->second.emplace_back(m); } } } } colIdx = A.ColBlockIndices()[i]; } int n = A.Rows(); assert(n > 0 && "Hessian has 0 Rows/Cols"); Eigen::Map xvec(x, A.Cols()); const Eigen::Map bvec(b, n); xvec.setZero(); VectorX r, pk, q, s; pk.setZero(n); q.setZero(n); s.setZero(n); r = bvec; MultDiag(A.ColBlockIndices(), J_, r, pk); double dn = r.dot(pk); double d0 = tolerance_ * dn; if (abs_tolerance_) { if (residual_ > 0.0 && residual_ > d0) { d0 = residual_; } } int maxIter = max_iter_ < 0 ? A.Rows() : max_iter_; int iteration; std::vector dns; double last_dn = 0.0; for (iteration = 0; iteration < maxIter; ++iteration) { if (dn <= d0 || dn < 1e-8) { break; // done } /// qk = A pk Mult(A.ColBlockIndices(), pk, q); /// alpha = rk^T pk / (pk^T A pk) double alpha = dn / pk.dot(q); /// x_k+1 = x_k + alpha pk xvec += alpha * pk; // TODO: reset residual here every 50 iterations /// r_{k+1} = r_k + alpha p_k r -= alpha * q; /// s = Lambda pk MultDiag(A.ColBlockIndices(), J_, r, s); double dold = dn; /// dn = r_{k+1}^T A p_k dn = r.dot(s); if (iteration >= 0.1 * maxIter && dn >= 1.1 * last_dn) { break; } /// beta = r_{k+1}^T A p_k / (pk^T A p_k) double beta = dn / dold; if (iteration % 50 == 0) { beta = 0; } /// pk+1 = -r_k+1 + beta pk pk = s + beta * pk; dns.emplace_back(dn); last_dn = dn; } residual_ = 0.5 * dn; return true; } template void LinearSolverPCG::MultDiag(const std::vector& colBlockIndices, MatrixVector& A, const VectorX& src, VectorX& dest) { int row = 0; for (size_t i = 0; i < A.size(); ++i) { internal::pcg_axy(A[i], src, row, dest, row); row = colBlockIndices[i]; } } template void LinearSolverPCG::MultDiag(const std::vector& colBlockIndices, MatrixPtrVector& A, const VectorX& src, VectorX& dest) { int row = 0; for (size_t i = 0; i < A.size(); ++i) { internal::pcg_axy(*A[i], src, row, dest, row); row = colBlockIndices[i]; } } template void LinearSolverPCG::Mult(const std::vector& colBlockIndices, const VectorX& src, VectorX& dest) { // first multiply with the diagonal // A的主对角线部分已经乘完了 MultDiag(colBlockIndices, diag_, src, dest); // now multiply with the upper triangular block // NOTE: 这个维度太小的时候，多线程反而不划算 if (indices_by_row_.size() >= 200) { /// 上半角部分 std::for_each(std::execution::par_unseq, indices_by_row_.begin(), indices_by_row_.end(), [&](const auto& d) { const int& destOffset = d.first; for (const MatAndIdx& m : d.second) { const int& srcOffset = m.idx_; const auto& a = m.mat_; internal::pcg_axpy(*a, src, srcOffset, dest, destOffset); } }); /// 下半角部分 std::for_each(std::execution::par_unseq, indices_by_col_.begin(), indices_by_col_.end(), [&](const auto& d) { const int& destOffsetT = d.first; for (const MatAndIdx& m : d.second) { const int& srcOffsetT = m.idx_; const auto& a = m.mat_; internal::pcg_atxpy(*a, src, srcOffsetT, dest, destOffsetT); } }); } else { /// 串行计算时不需要再区分上下半区 std::for_each(std::execution::seq, indices_by_row_.begin(), indices_by_row_.end(), [&](const auto& d) { for (const MatAndIdx& m : d.second) { const int& srcOffset = m.idx_; const int& destOffsetT = srcOffset; const int& destOffset = d.first; const int& srcOffsetT = destOffset; const auto& a = m.mat_; // destVec += *a * srcVec (according to the sub-vector parts) internal::pcg_axpy(*a, src, srcOffset, dest, destOffset); // destVec += *a.transpose() * srcVec (according to the sub-vector parts) internal::pcg_atxpy(*a, src, srcOffsetT, dest, destOffsetT); } }); } } } // namespace lightning::miao ================================================ FILE: src/core/miao/core/solver/solver.cc ================================================ // // Created by xiang on 24-4-26. // #include "solver.h" #include #include namespace lightning::miao { Solver::Solver() {} void Solver::ResizeVector(size_t sx) { x_size_ = sx; x_.conservativeResize(x_size_, 1); b_.conservativeResize(x_size_, 1); } Solver::~Solver() {} } // namespace lightning::miao ================================================ FILE: src/core/miao/core/solver/solver.h ================================================ // // Created by xiang on 24-4-25. // #ifndef MIAO_SOLVER_H #define MIAO_SOLVER_H #include "common/eigen_types.h" #include "core/graph/graph.h" #include "core/sparse/sparse_block_matrix.h" namespace lightning::miao { class Optimizer; /** * \brief Generic interface for a sparse Solver operating on a graph which * solves one iteration of the linearized objective function * * Solver 接口类 * Solver 的主要继承类是block solver,由block solver解出具体的H, b的矩阵块 * 由optimization Algorithm 来调用各种成员函数 * * Block Solver 矩阵块大小在编译期已知 * Block Solver 进一步持有linear solver的指针，从而求解Hx = GetB */ class Solver { public: Solver(); virtual ~Solver(); DISALLOW_COPY(Solver) public: /** * initialize the Solver, called once before the first iteration */ virtual bool Init(Optimizer* optimizer) = 0; /** * build the structure of the system */ virtual bool BuildStructure(bool zeroBlocks = false) = 0; /** * build the current system */ virtual bool BuildSystem() = 0; /** * Solve Ax = GetB * 实际由持有的linear solver执行 */ virtual bool Solve() = 0; /** * compute dest = H * src */ virtual void MultiplyHessian(double* dest, const double* src) const = 0; /** * Update the system while performing Levenberg, i.e., modifying the Diagonal * components of A by doing += lambda along the main Diagonal of the Matrix. * Note that this function may be called with a positive and a negative * lambda. The latter is used to undo a former modification. If backup is * true, then the Solver should store a backup of the Diagonal, which can be * restored by RestoreDiagonal() */ virtual bool SetLambda(double lambda, bool backup = false) = 0; /** * restore a previously made backup of the Diagonal */ virtual void RestoreDiagonal() = 0; //! return x, the solution vector double* GetX() { return x_.data(); } lightning::VecXd GetXVec() { return x_; } const double* GetX() const { return x_.data(); } //! return GetB, the right hand side of the system double* GetB() { return b_.data(); } const double* GetB() const { return b_.data(); } //! return the size of the solution vector (GetX) and GetB size_t VectorSize() const { return x_size_; } /** * does this Solver support the Schur complement for solving a system * consisting of poses and landmarks. Re-implement in a derived Solver, if * your Solver supports it. */ virtual bool SupportsSchur() { return false; } //! should the Solver perform the Schur complement or not virtual bool Schur() = 0; virtual void SetSchur(bool s) = 0; protected: Optimizer* optimizer_ = nullptr; lightning::VecXd x_; // 求解的x lightning::VecXd b_; // 求解的b size_t x_size_ = 0, max_x_size_ = 0; // x的size bool is_levenberg_ = false; ///< the system we gonna Solve is a Levenberg-Marquardt system size_t additional_vector_space_ = 0; void ResizeVector(size_t sx); }; } // namespace lightning::miao #endif // MIAO_SOLVER_H ================================================ FILE: src/core/miao/core/sparse/sparse_block_matrix.h ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_SPARSE_BLOCK_MATRIX_H #define MIAO_SPARSE_BLOCK_MATRIX_H #include "common/eigen_types.h" #include "sparse_block_matrix_ccs.h" #include "sparse_block_matrix_hashmap.h" #include #include namespace lightning::miao { /** * \brief Sparse matrix which uses blocks * * Template class that specifies a sparse Block matrix. A Block * matrix is a sparse matrix made of dense blocks. These blocks * cannot have a random pattern, but follow a (variable) grid * structure. This structure is specified by a partition of the Rows * and the columns of the matrix. The blocks are represented by the * Eigen::Matrix structure, thus they can be statically or dynamically * allocated. For efficiency reasons it is convenient to allocate them * statically, when possible. A static Block matrix has all blocks of * the same size, and the size of the Block is specified by the * template argument. If this is not the case, and you have different * Block sizes than you have to use a dynamic-Block matrix (default * template argument). */ template class SparseBlockMatrix { public: //! this is the type of the elementary Block, it is an Eigen::Matrix. using SparseMatrixBlock = MatrixType; //! columns of the matrix inline int Cols() const { return col_block_indices_.size() ? col_block_indices_.back() : 0; } //! Rows of the matrix inline int Rows() const { return row_block_indices_.size() ? row_block_indices_.back() : 0; } using IntBlockMap = std::map; /** * constructs a sparse Block matrix having a specific layout * @param rbi: array of int containing the row layout of the blocks. * the component i of the array should contain the index of the first row of * the Block i+1. * @param cbi: array of int containing the column layout of the blocks. * the component i of the array should contain the index of the first col of * the Block i+1. * @param rb: number of row blocks * @param cb: number of col blocks * @param hasStorage: set it to true if the matrix "owns" the blocks, thus it * deletes it on destruction. if false the matrix is only a "view" over an * existing structure. */ SparseBlockMatrix(const int* rbi, const int* cbi, int rb, int cb, bool hasStorage = true); SparseBlockMatrix(const std::vector& row_block_index, const std::vector& col_block_index, bool hasStorage = true); SparseBlockMatrix(); ~SparseBlockMatrix(); /** * 设定新增的blocks */ void SetBlockIndexInc(const std::vector& row_block_index, const std::vector& col_block_index); //! this zeroes all the blocks. If dealloc=true the blocks are removed from //! memory /** * 置零整个稀疏矩阵 * @param dealloc 为true时，释放所有内存 */ void Clear(bool dealloc = false); //! returns the Block at location r,c. if alloc=true he Block is created if it //! does not exist SparseMatrixBlock* Block(int r, int c, bool alloc = false); //! returns the Block at location r,c const SparseMatrixBlock* Block(int r, int c) const; //! how many Rows does the Block at Block-row r has? inline int RowsOfBlock(int r) const { return r ? row_block_indices_[r] - row_block_indices_[r - 1] : row_block_indices_[0]; } //! how many Cols does the Block at Block-col c has? inline int ColsOfBlock(int c) const { return c ? col_block_indices_[c] - col_block_indices_[c - 1] : col_block_indices_[0]; } //! where does the row at Block-row r starts? inline int RowBaseOfBlock(int r) const { return r ? row_block_indices_[r - 1] : 0; } //! where does the col at Block-col r starts? inline int ColBaseOfBlock(int c) const { return c ? col_block_indices_[c - 1] : 0; } //! number of non-zero elements size_t NonZeros() const; //! number of allocated blocks size_t NonZeroBlocks() const; //! transposes a Block matrix, The transposed type should match the argument //! false on failure template bool transpose(SparseBlockMatrix& dest) const; //! adds the current matrix to the destination bool Add(SparseBlockMatrix& dest) const; /** * compute dest = (*this) * src * However, assuming that this is a symmetric matrix where only the upper * triangle is stored */ void MultiplySymmetricUpperTriangle(double*& dest, const double* src) const; //! exports the non zero blocks into a column compressed structure void FillBlockStructure(int* Cp, int* Ci) const; //! the Block matrices per Block-column const std::vector& BlockCols() const { return block_cols_; } std::vector& BlockCols() { return block_cols_; } //! indices of the row blocks const std::vector& RowBlockIndices() const { return row_block_indices_; } std::vector& RowBlockIndices() { return row_block_indices_; } //! indices of the column blocks const std::vector& ColBlockIndices() const { return col_block_indices_; } std::vector& ColBlockIndices() { return col_block_indices_; } /** * copy into CCS structure * @return number of processed blocks, -1 on error */ int FillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const; /** * copy as transposed into a CCS structure * @return number of processed blocks, -1 on error */ int FillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS& blockCCS) const; /** * take over the memory and matrix pattern from a hash matrix. * The structure of the hash matrix will be cleared. */ void TakePatternFromHash(SparseBlockMatrixHashMap& hashMatrix); bool WriteOctave(const std::string& filename, bool upperTriangle = true) const; protected: std::vector row_block_indices_; ///< vector of the indices of the blocks ///< along the Rows. std::vector col_block_indices_; ///< vector of the indices of the blocks ///< along the Cols //! array of maps of blocks. The index of the array represent a Block column //! of the matrix and the Block column is stored as a map row_block -> //! matrix_block_ptr. std::vector block_cols_; // vector 索引为列，内部map索引为行 bool has_storage_; private: template void TransposeInternal(SparseBlockMatrix& dest) const; void AddInternal(SparseBlockMatrix& dest) const; }; template std::ostream& operator<<(std::ostream&, const SparseBlockMatrix& m); using SparseBlockMatrixX = SparseBlockMatrix; } // namespace lightning::miao #include "sparse_block_matrix.hpp" #endif // MIAO_SPARSE_BLOCK_MATRIX_H ================================================ FILE: src/core/miao/core/sparse/sparse_block_matrix.hpp ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_SPARSE_BLCOK_MATRIX_HPP #define MIAO_SPARSE_BLCOK_MATRIX_HPP #include "sparse_block_matrix.h" #include #include namespace lightning::miao { template SparseBlockMatrix::SparseBlockMatrix(const int* rbi, const int* cbi, int rb, int cb, bool hasStorage) : row_block_indices_(rbi, rbi + rb), col_block_indices_(cbi, cbi + cb), block_cols_(cb), has_storage_(hasStorage) {} template SparseBlockMatrix::SparseBlockMatrix(const std::vector& row_block_index, const std::vector& col_block_index, bool hasStorage) : row_block_indices_(row_block_index), col_block_indices_(col_block_index), block_cols_(col_block_index.size()), has_storage_(hasStorage) {} template void SparseBlockMatrix::SetBlockIndexInc(const std::vector& row_block_index, const std::vector& col_block_index) { for (const auto& r : row_block_index) { row_block_indices_.emplace_back(r); } for (const auto& c : col_block_index) { col_block_indices_.emplace_back(c); block_cols_.emplace_back(std::map()); } } template SparseBlockMatrix::SparseBlockMatrix() : block_cols_(0), has_storage_(true) {} template void SparseBlockMatrix::Clear(bool dealloc) { for (int i = 0; i < static_cast(block_cols_.size()); ++i) { for (auto it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { typename SparseBlockMatrix::SparseMatrixBlock* b = it->second; if (has_storage_ && dealloc) { delete b; } else { b->setZero(); } } if (has_storage_ && dealloc) { block_cols_[i].clear(); } } } template SparseBlockMatrix::~SparseBlockMatrix() { if (has_storage_) { Clear(true); } } template typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::Block(int r, int c, bool alloc) { typename SparseBlockMatrix::IntBlockMap::iterator it = block_cols_[c].find(r); typename SparseBlockMatrix::SparseMatrixBlock* _block = 0; if (it == block_cols_[c].end()) { if (!has_storage_ && !alloc) { return nullptr; } else { int rb = RowsOfBlock(r); int cb = ColsOfBlock(c); _block = new typename SparseBlockMatrix::SparseMatrixBlock(rb, cb); _block->setZero(); block_cols_[c].insert(std::make_pair(r, _block)); } } else { _block = it->second; } return _block; } template const typename SparseBlockMatrix::SparseMatrixBlock* SparseBlockMatrix::Block(int r, int c) const { auto it = block_cols_[c].find(r); if (it == block_cols_[c].end()) { return nullptr; } return it->second_; } template template void SparseBlockMatrix::TransposeInternal(SparseBlockMatrix& dest) const { for (size_t i = 0; i < block_cols_.size(); ++i) { for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { typename SparseBlockMatrix::SparseMatrixBlock* s = it->second; typename SparseBlockMatrix::SparseMatrixBlock* d = dest.Block(i, it->first, true); *d = s->transpose(); } } } template template bool SparseBlockMatrix::transpose(SparseBlockMatrix& dest) const { if (!dest.has_storage_) { return false; } if (row_block_indices_.size() != dest.col_block_indices_.size()) { return false; } if (col_block_indices_.size() != dest.row_block_indices_.size()) { return false; } for (size_t i = 0; i < row_block_indices_.size(); ++i) { if (row_block_indices_[i] != dest.col_block_indices_[i]) { return false; } } for (size_t i = 0; i < col_block_indices_.size(); ++i) { if (col_block_indices_[i] != dest.row_block_indices_[i]) { return false; } } TransposeInternal(dest); return true; } template void SparseBlockMatrix::AddInternal(SparseBlockMatrix& dest) const { for (size_t i = 0; i < block_cols_.size(); ++i) { for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { typename SparseBlockMatrix::SparseMatrixBlock* s = it->second; typename SparseBlockMatrix::SparseMatrixBlock* d = dest.Block(it->first, i, true); (*d) += *s; } } } template bool SparseBlockMatrix::Add(SparseBlockMatrix& dest) const { if (!dest.has_storage_) { return false; } if (row_block_indices_.size() != dest.row_block_indices_.size()) { return false; } if (col_block_indices_.size() != dest.col_block_indices_.size()) { return false; } for (size_t i = 0; i < row_block_indices_.size(); ++i) { if (row_block_indices_[i] != dest.row_block_indices_[i]) { return false; } } for (size_t i = 0; i < col_block_indices_.size(); ++i) { if (col_block_indices_[i] != dest.col_block_indices_[i]) { return false; } } AddInternal(dest); return true; } template void SparseBlockMatrix::MultiplySymmetricUpperTriangle(double*& dest, const double* src) const { if (!dest) { dest = new double[row_block_indices_[row_block_indices_.size() - 1]]; memset(dest, 0, row_block_indices_[row_block_indices_.size() - 1] * sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, Rows()); const Eigen::Map srcVec(src, Cols()); for (size_t i = 0; i < block_cols_.size(); ++i) { int srcOffset = ColBaseOfBlock(i); for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { const typename SparseBlockMatrix::SparseMatrixBlock* a = it->second; int destOffset = RowBaseOfBlock(it->first); if (destOffset > srcOffset) // only upper triangle break; // destVec += *a * srcVec (according to the sub-vector parts) internal::template axpy::SparseMatrixBlock>(*a, srcVec, srcOffset, destVec, destOffset); if (destOffset < srcOffset) internal::template atxpy::SparseMatrixBlock>( *a, srcVec, destOffset, destVec, srcOffset); } } } template size_t SparseBlockMatrix::NonZeroBlocks() const { size_t count = 0; for (size_t i = 0; i < block_cols_.size(); ++i) { count += block_cols_[i].size(); } return count; } template size_t SparseBlockMatrix::NonZeros() const { if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) { size_t nnz = NonZeroBlocks() * MatrixType::SizeAtCompileTime; return nnz; } else { size_t count = 0; for (size_t i = 0; i < block_cols_.size(); ++i) { for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { const typename SparseBlockMatrix::SparseMatrixBlock* a = it->second; count += a->cols() * a->rows(); } } return count; } } template std::ostream& operator<<(std::ostream& os, const SparseBlockMatrix& m) { os << "RBI: " << m.RowBlockIndices().size(); for (size_t i = 0; i < m.RowBlockIndices().size(); ++i) os << " " << m.RowBlockIndices()[i]; os << std::endl; os << "CBI: " << m.ColBlockIndices().size(); for (size_t i = 0; i < m.ColBlockIndices().size(); ++i) os << " " << m.ColBlockIndices()[i]; os << std::endl; for (size_t i = 0; i < m.BlockCols().size(); ++i) { for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = m.BlockCols()[i].begin(); it != m.BlockCols()[i].end(); ++it) { const typename SparseBlockMatrix::SparseMatrixBlock* b = it->second; os << "BLOCK: " << it->first << " " << i << std::endl; os << *b << std::endl; } } return os; } template void SparseBlockMatrix::FillBlockStructure(int* Cp, int* Ci) const { int nz = 0; for (int c = 0; c < static_cast(block_cols_.size()); ++c) { *Cp = nz; for (auto it = block_cols_[c].begin(); it != block_cols_[c].end(); ++it) { const int& r = it->first; if (r <= c) { *Ci++ = r; ++nz; } } Cp++; } *Cp = nz; assert(nz <= static_cast(NonZeroBlocks())); } template int SparseBlockMatrix::FillSparseBlockMatrixCCS(SparseBlockMatrixCCS& blockCCS) const { auto& b = blockCCS.BlockCols(); b.resize(block_cols_.size()); int numblocks = 0; for (size_t i = 0; i < block_cols_.size(); ++i) { const IntBlockMap& row = block_cols_[i]; auto& dest = b[i]; dest.clear(); dest.reserve(row.size()); for (auto it = row.begin(); it != row.end(); ++it) { dest.push_back(typename SparseBlockMatrixCCS::RowBlock(it->first, it->second)); ++numblocks; } } return numblocks; } template int SparseBlockMatrix::FillSparseBlockMatrixCCSTransposed( SparseBlockMatrixCCS& blockCCS) const { blockCCS.BlockCols().clear(); blockCCS.BlockCols().resize(row_block_indices_.size()); int numblocks = 0; for (size_t i = 0; i < BlockCols().size(); ++i) { const IntBlockMap& row = BlockCols()[i]; for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) { typename SparseBlockMatrixCCS::SparseColumn& dest = blockCCS.BlockCols()[it->first]; dest.push_back(typename SparseBlockMatrixCCS::RowBlock(i, it->second)); ++numblocks; } } return numblocks; } template void SparseBlockMatrix::TakePatternFromHash(SparseBlockMatrixHashMap& hashMatrix) { // sort the sparse columns and add them to the map structures by // exploiting that we are inserting a sorted structure typedef std::pair SparseColumnPair; typedef typename SparseBlockMatrixHashMap::SparseColumn HashSparseColumn; for (size_t i = 0; i < hashMatrix.BlockCols().size(); ++i) { // prepare a temporary vector for sorting HashSparseColumn& column = hashMatrix.BlockCols()[i]; if (column.size() == 0) continue; std::vector sparseRowSorted; // temporary structure sparseRowSorted.reserve(column.size()); for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it) sparseRowSorted.push_back(*it); std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }); // try to free some memory early HashSparseColumn aux; std::swap(aux, column); // now insert sorted vector to the std::map structure IntBlockMap& destColumnMap = BlockCols()[i]; destColumnMap.insert(sparseRowSorted[0]); for (size_t j = 1; j < sparseRowSorted.size(); ++j) { typename SparseBlockMatrix::IntBlockMap::iterator hint = destColumnMap.end(); --hint; // cppreference says the element goes after the hint (until // C++11) destColumnMap.insert(hint, sparseRowSorted[j]); } } } template bool SparseBlockMatrix::WriteOctave(const std::string& filename, bool upperTriangle) const { std::string name = filename; std::string::size_type lastDot = name.find_last_of('.'); if (lastDot != std::string::npos) { name.resize(lastDot); } struct TripletEntry { int r, c; double x; TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {} }; struct TripletColSort { bool operator()(const TripletEntry& e1, const TripletEntry& e2) const { return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r); } }; std::vector entries; for (size_t i = 0; i < block_cols_.size(); ++i) { const int& c = i; for (typename SparseBlockMatrix::IntBlockMap::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { const int& r = it->first; const MatrixType& m = *(it->second); for (int cc = 0; cc < m.cols(); ++cc) for (int rr = 0; rr < m.rows(); ++rr) { int aux_r = RowBaseOfBlock(r) + rr; int aux_c = ColBaseOfBlock(c) + cc; entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc))); if (upperTriangle && r != c) { entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc))); } } } } int nz = entries.size(); std::sort(entries.begin(), entries.end(), TripletColSort()); std::ofstream fout(filename); fout << "# name: " << name << std::endl; fout << "# type: sparse matrix" << std::endl; fout << "# nnz: " << nz << std::endl; fout << "# rows: " << Rows() << std::endl; fout << "# columns: " << Cols() << std::endl; fout << std::setprecision(9) << std::fixed << std::endl; for (auto it = entries.begin(); it != entries.end(); ++it) { const TripletEntry& entry = *it; fout << entry.r + 1 << " " << entry.c + 1 << " " << entry.x << std::endl; } return fout.good(); } } // namespace lightning::miao #endif // MIAO_SPARSE_BLCOK_MATRIX_HPP ================================================ FILE: src/core/miao/core/sparse/sparse_block_matrix_ccs.h ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_SPARSE_BLOCK_MATRIX_CCS_H #define MIAO_SPARSE_BLOCK_MATRIX_CCS_H #include #include #include #include "common/eigen_types.h" #include "core/math/matrix_operations.h" namespace lightning::miao { /** * \brief Sparse matrix which uses blocks * * This class is used as a const view on a SparseBlockMatrix * which allows a faster iteration over the elements of the * matrix. */ template class SparseBlockMatrixCCS { public: //! this is the type of the elementary Block, it is an Eigen::Matrix. typedef MatrixType SparseMatrixBlock; //! columns of the matrix int Cols() const { return col_block_indices_.size() ? col_block_indices_.back() : 0; } //! Rows of the matrix int Rows() const { return row_block_indices_.size() ? row_block_indices_.back() : 0; } /** * \brief A Block within a column */ struct RowBlock { int row; ///< row of the Block MatrixType* block = nullptr; ///< matrix pointer for the Block RowBlock() : row(-1), block(nullptr) {} RowBlock(int r, MatrixType* b) : row(r), block(b) {} bool operator<(const RowBlock& other) const { return row < other.row; } }; typedef std::vector SparseColumn; SparseBlockMatrixCCS(const std::vector& rowIndices, const std::vector& colIndices) : row_block_indices_(rowIndices), col_block_indices_(colIndices) {} //! how many Rows does the Block at Block-row r has? int RowsOfBlock(int r) const { return r ? row_block_indices_[r] - row_block_indices_[r - 1] : row_block_indices_[0]; } //! how many Cols does the Block at Block-col c has? int ColsOfBlock(int c) const { return c ? col_block_indices_[c] - col_block_indices_[c - 1] : col_block_indices_[0]; } //! where does the row at Block-row r start? int RowBaseOfBlock(int r) const { return r ? row_block_indices_[r - 1] : 0; } //! where does the col at Block-col r start? int ColBaseOfBlock(int c) const { return c ? col_block_indices_[c - 1] : 0; } //! the Block matrices per Block-column const std::vector& BlockCols() const { return block_cols_; } std::vector& BlockCols() { return block_cols_; } //! indices of the row blocks const std::vector& RowBlockIndices() const { return row_block_indices_; } //! indices of the column blocks const std::vector& ColBlockIndices() const { return col_block_indices_; } void RightMultiply(double*& dest, const double* src) const { int destSize = Cols(); if (!dest) { dest = new double[destSize]; memset(dest, 0, destSize * sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, destSize); Eigen::Map srcVec(src, Rows()); for (int i = 0; i < static_cast(block_cols_.size()); ++i) { int destOffset = ColBaseOfBlock(i); for (typename SparseColumn::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { const SparseMatrixBlock* a = it->block; int srcOffset = RowBaseOfBlock(it->row); // destVec += *a.transpose() * srcVec (according to the sub-vector // parts) internal::template atxpy(*a, srcVec, srcOffset, destVec, destOffset); } } } /** * sort the blocks in each column */ void SortColumns() { for (int i = 0; i < static_cast(block_cols_.size()); ++i) { std::sort(block_cols_[i].begin(), block_cols_[i].end()); } } /** * fill the CCS arrays of a matrix, arrays have to be allocated beforehand */ int FillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const { assert(Cp && Ci && Cx && "Target destination is NULL"); int nz = 0; for (size_t i = 0; i < block_cols_.size(); ++i) { int cstart = i ? col_block_indices_[i - 1] : 0; int csize = ColsOfBlock(i); for (int c = 0; c < csize; ++c) { *Cp = nz; for (auto it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { const SparseMatrixBlock* b = it->block; int rstart = it->row ? row_block_indices_[it->row - 1] : 0; int elemsToCopy = b->rows(); if (upperTriangle && rstart == cstart) elemsToCopy = c + 1; for (int r = 0; r < elemsToCopy; ++r) { *Cx++ = (*b)(r, c); *Ci++ = rstart++; ++nz; } } ++Cp; } } *Cp = nz; return nz; } /** * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. * This function only writes the values and assumes that column and row * structures have already been written. */ int FillCCS(double* Cx, bool upperTriangle = false) const { assert(Cx && "Target destination is NULL"); double* CxStart = Cx; int cstart = 0; for (size_t i = 0; i < block_cols_.size(); ++i) { int csize = col_block_indices_[i] - cstart; for (int c = 0; c < csize; ++c) { for (typename SparseColumn::const_iterator it = block_cols_[i].begin(); it != block_cols_[i].end(); ++it) { const SparseMatrixBlock* b = it->block; int rstart = it->row ? row_block_indices_[it->row - 1] : 0; int elemsToCopy = b->rows(); if (upperTriangle && rstart == cstart) { elemsToCopy = c + 1; } memcpy(Cx, b->data() + c * b->rows(), elemsToCopy * sizeof(double)); Cx += elemsToCopy; } } cstart = col_block_indices_[i]; } return Cx - CxStart; } protected: const std::vector& row_block_indices_; ///< vector of the indices of the ///< blocks along the Rows. const std::vector& col_block_indices_; ///< vector of the indices of the blocks along the Cols std::vector block_cols_; ///< the matrices stored in CCS order }; } // namespace lightning::miao #endif // MIAO_SPARSE_BLOCK_MATRIX_CCS_H ================================================ FILE: src/core/miao/core/sparse/sparse_block_matrix_diagonal.h ================================================ // // Created by xiang on 24-4-26. // #ifndef MIAO_SPARSE_BLOCK_MATRIX_DIAGONAL_H #define MIAO_SPARSE_BLOCK_MATRIX_DIAGONAL_H #include "core/math/matrix_operations.h" namespace lightning::miao { /** * 对角线的稀疏矩阵块 * @tparam MatrixType */ template class SparseBlockMatrixDiagonal { public: //! this is the type of the elementary Block, it is an Eigen::Matrix. typedef MatrixType SparseMatrixBlock; //! columns of the matrix int Cols() const { return block_indices_.size() ? block_indices_.back() : 0; } //! Rows of the matrix int Rows() const { return block_indices_.size() ? block_indices_.back() : 0; } using DiagonalVector = std::vector; explicit SparseBlockMatrixDiagonal(const std::vector& blockIndices) : block_indices_(blockIndices) {} //! how many Rows/Cols does the block at Block-row / Block-column r has? inline int DimOfBlock(int r) const { return r ? block_indices_[r] - block_indices_[r - 1] : block_indices_[0]; } //! where does the row /col at Block-row / Block-column r starts? inline int BaseOfBlock(int r) const { return r ? block_indices_[r - 1] : 0; } //! the Block matrices per Block-column const DiagonalVector& Diagonal() const { return diagonal_; } DiagonalVector& Diagonal() { return diagonal_; } //! indices of the row blocks const std::vector& BlockIndices() const { return block_indices_; } void Multiply(double*& dest, const double* src) const { int destSize = Cols(); if (!dest) { dest = new double[destSize]; memset(dest, 0, destSize * sizeof(double)); } // map the memory by Eigen Eigen::Map destVec(dest, destSize); Eigen::Map srcVec(src, Rows()); for (int i = 0; i < static_cast(diagonal_.size()); ++i) { int destOffset = BaseOfBlock(i); int srcOffset = destOffset; const SparseMatrixBlock& A = diagonal_[i]; // destVec += *A.transpose() * srcVec (according to the sub-vector parts) internal::template axpy(A, srcVec, srcOffset, destVec, destOffset); } } protected: const std::vector& block_indices_; ///< vector of the indices of the ///< blocks along the Diagonal DiagonalVector diagonal_; }; } // namespace lightning::miao #endif // MIAO_SPARSE_BLOCK_MATRIX_DIAGONAL_H ================================================ FILE: src/core/miao/core/sparse/sparse_block_matrix_hashmap.h ================================================ // // Created by xiang on 24-4-24. // #ifndef MIAO_SPARSE_BLOCK_HASHMAP_H #define MIAO_SPARSE_BLOCK_HASHMAP_H #include "common/eigen_types.h" namespace lightning::miao { /** * \brief Sparse matrix which uses blocks based on hash structures * * This class is used to construct the pattern of a sparse Block matrix * * hash map版本的sparse matrix */ template class SparseBlockMatrixHashMap { public: //! this is the type of the elementary Block, it is an Eigen::Matrix. using SparseMatrixBlock = MatrixType; //! columns of the matrix int Cols() const { return col_block_indices_.size() ? col_block_indices_.back() : 0; } //! Rows of the matrix int Rows() const { return row_block_indices_.size() ? row_block_indices_.back() : 0; } using SparseColumn = std::unordered_map; SparseBlockMatrixHashMap(const std::vector& rowIndices, const std::vector& colIndices) : row_block_indices_(rowIndices), col_block_indices_(colIndices) {} //! how many Rows does the Block at Block-row r has? int RowsOfBlock(int r) const { return r ? row_block_indices_[r] - row_block_indices_[r - 1] : row_block_indices_[0]; } //! how many Cols does the Block at Block-col c has? int ColsOfBlock(int c) const { return c ? col_block_indices_[c] - col_block_indices_[c - 1] : col_block_indices_[0]; } //! where does the row at Block-row r start? int RowBaseOfBlock(int r) const { return r ? row_block_indices_[r - 1] : 0; } //! where does the col at Block-col r start? int ColBaseOfBlock(int c) const { return c ? col_block_indices_[c - 1] : 0; } //! the Block matrices per Block-column const std::vector& BlockCols() const { return block_cols_; } std::vector& BlockCols() { return block_cols_; } //! indices of the row blocks const std::vector& RowBlockIndices() const { return row_block_indices_; } //! indices of the column blocks const std::vector& ColBlockIndices() const { return col_block_indices_; } /** * Add a Block to the pattern, return a pointer to the added Block */ MatrixType* AddBlock(int r, int c, bool zeroBlock = false) { assert(c < (int)block_cols_.size() && "accessing column which is not available"); SparseColumn& sparseColumn = block_cols_[c]; typename SparseColumn::iterator foundIt = sparseColumn.find(r); if (foundIt == sparseColumn.end()) { int rb = RowsOfBlock(r); int cb = ColsOfBlock(c); MatrixType* m = new MatrixType(rb, cb); if (zeroBlock) m->setZero(); sparseColumn[r] = m; return m; } return foundIt->second; } protected: const std::vector& row_block_indices_; ///< vector of the indices of the ///< blocks along the Rows. const std::vector& col_block_indices_; ///< vector of the indices of the blocks along the Cols std::vector block_cols_; ///< the matrices stored in CCS order }; } // namespace lightning::miao #endif // MIAO_SPARSE_BLOCK_HASHMAP_H ================================================ FILE: src/core/miao/core/types/edge_se2.h ================================================ // // Created by xiang on 24-6-17. // #ifndef MIAO_EDGE_SE2_H #define MIAO_EDGE_SE2_H #include "core/graph/base_binary_edge.h" #include "core/types/vertex_se2.h" namespace lightning::miao { class EdgeSE2 : public BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2> { public: void ComputeError() override { auto* v1 = (VertexSE2*)(vertices_[0]); auto* v2 = (VertexSE2*)(vertices_[1]); SE2 delta = measurement_.inverse() * (v1->Estimate().inverse() * v2->Estimate()); error_ = Vec3d(delta.translation().x(), delta.translation().y(), delta.so2().log()); } void LinearizeOplus() override { auto* vi = (VertexSE2*)(vertices_[0]); auto* vj = (VertexSE2*)(vertices_[1]); double thetai = vi->Estimate().so2().log(); Vec2d dt = vj->Estimate().translation() - vi->Estimate().translation(); double si = std::sin(thetai), ci = std::cos(thetai); jacobian_oplus_xi_ << -ci, -si, -si * dt.x() + ci * dt.y(), si, -ci, -ci * dt.x() - si * dt.y(), 0, 0, -1; jacobian_oplus_xj_ << ci, si, 0, -si, ci, 0, 0, 0, 1; const SE2& rmean = measurement_.inverse(); Mat3d z; z.block<2, 2>(0, 0) = rmean.so2().matrix(); z.col(2) << cst(0.), cst(0.), cst(1.); z.row(2).head<2>() << cst(0.), cst(0.); jacobian_oplus_xi_ = z * jacobian_oplus_xi_; jacobian_oplus_xj_ = z * jacobian_oplus_xj_; } }; } // namespace lightning::miao #endif // MIAO_EDGE_SE2_H ================================================ FILE: src/core/miao/core/types/edge_se2_prior.h ================================================ // // Created by xiang on 24-7-1. // #ifndef MIAO_EDGE_SE2_PRIOR_H #define MIAO_EDGE_SE2_PRIOR_H #include "core/graph/base_unary_edge.h" #include "core/types/vertex_se2.h" namespace lightning::miao { class EdgeSE2Prior : public BaseUnaryEdge<3, SE2, VertexSE2> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW void ComputeError() override { const VertexSE2* v1 = static_cast(vertices_[0]); SE2 delta = measurement_.inverse() * v1->Estimate(); error_ = Vec3d(delta.translation().x(), delta.translation().y(), delta.so2().log()); } void LinearizeOplus() override { jacobian_oplus_xi_.setZero(); jacobian_oplus_xi_.block<2, 2>(0, 0) = measurement_.inverse().so2().matrix(); jacobian_oplus_xi_(2, 2) = 1; } }; } // namespace lightning::miao #endif // MIAO_EDGE_SE2_PRIOR_H ================================================ FILE: src/core/miao/core/types/edge_se3.h ================================================ // // Created by xiang on 24-6-4. // #ifndef MIAO_EDGE_SE3_H #define MIAO_EDGE_SE3_H #include "core/common/math.h" #include "core/graph/base_binary_edge.h" #include "core/types/vertex_se3.h" namespace lightning::miao { /** * 两条SE3边之间的约束 * 观测 = Tw1.inv * Tw2 = T12 = * = [R1^T R2 R1^T t2 - R1^T t1 ] * = [0^T 1 ] * -> [obs_R, obs_t] * [0^T , 1 ] * * 误差平移在前（与g2o保持一致，否则pose graph的info就不对） * 误差(旋转) = obs_R.inv * T12.rot * 误差(平移) = obs_t.inv - (R1^T t2 - R1^T t1) * */ class EdgeSE3 : public BaseBinaryEdge<6, SE3, VertexSE3, VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// 为了读g2o文件，这里的算法必须与g2o一致。 void ComputeError() override { SE3 from = ((VertexSE3*)(vertices_[0]))->Estimate(); SE3 to = ((VertexSE3*)(vertices_[1]))->Estimate(); SE3 T12 = from.inverse() * to; SE3 delta = measurement_.inverse() * T12; error_.head<3>() = delta.translation(); error_.tail<3>() = delta.so3().unit_quaternion().coeffs().head<3>(); } }; } // namespace lightning::miao #endif // MIAO_EDGE_SE3_H ================================================ FILE: src/core/miao/core/types/edge_se3_height_prior.h ================================================ // // Created by xiang on 25-11-13. // #ifndef LIGHTNING_EDGE_SE3_HEIGHT_PRIOR_H #define LIGHTNING_EDGE_SE3_HEIGHT_PRIOR_H #include "core/common/math.h" #include "core/graph/base_unary_edge.h" #include "core/types/vertex_se3.h" namespace lightning::miao { /** * 高度约束 */ class EdgeHeightPrior : public BaseUnaryEdge<1, double, VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW void ComputeError() override { SE3 pose = ((VertexSE3 *)(vertices_[0]))->Estimate(); error_[0] = pose.translation()[2] - measurement_; } void LinearizeOplus() override { jacobian_oplus_xi_.setZero(); jacobian_oplus_xi_[2] = 1; } }; } // namespace lightning::miao #endif // LIGHTNING_EDGE_SE3_HEIGHT_PRIOR_H ================================================ FILE: src/core/miao/core/types/edge_se3_prior.h ================================================ // // Created by xiang on 25-7-14. // #ifndef MIAO_EDGE_SE3_PRIOR_H #define MIAO_EDGE_SE3_PRIOR_H #include "core/common/math.h" #include "core/graph/base_unary_edge.h" #include "core/types/vertex_se3.h" namespace lightning::miao { /** * 先验的SE3观测 * * 误差平移在前（与g2o保持一致，否则pose graph的info就不对） * 误差(旋转) = T.trans - obs.trans * 误差(平移) = T.R^T * obs.R * */ class EdgeSE3Prior : public BaseUnaryEdge<6, SE3, VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// 为了读g2o文件，这里的算法必须与g2o一致。 void ComputeError() override { SE3 pose = ((VertexSE3*)(vertices_[0]))->Estimate(); error_.head<3>() = pose.translation() - measurement_.translation(); error_.tail<3>() = (pose.so3().inverse() * measurement_.so3()).log(); } // TODO: jacobian计算 }; } // namespace lightning::miao #endif // LIGHTNING_EDGE_SE3_PRIOR_H ================================================ FILE: src/core/miao/core/types/vertex_pointxyz.h ================================================ // // Created by xiang on 24-5-22. // #ifndef MIAO_VERTEX_POINTXYZ_H #define MIAO_VERTEX_POINTXYZ_H #include "core/graph/base_vec_vertex.h" namespace lightning::miao { class VertexPointXYZ : public BaseVecVertex<3> {}; } // namespace lightning::miao #endif // MIAO_VERTEX_POINTXYZ_H ================================================ FILE: src/core/miao/core/types/vertex_se2.h ================================================ // // Created by xiang on 24-6-17. // #ifndef MIAO_VERTEX_SE2_H #define MIAO_VERTEX_SE2_H #include "core/graph/base_vertex.h" namespace lightning::miao { class VertexSE2 : public BaseVertex<3, SE2> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// 覆盖oplus void OplusImpl(const double *update) override { Eigen::Map v(update); estimate_.translation() += Vec2d(v[0], v[1]); estimate_.so2() = SO2(estimate_.so2().log() + v[2]); } }; } // namespace lightning::miao #endif // MIAO_VERTEX_SE2_H ================================================ FILE: src/core/miao/core/types/vertex_se3.h ================================================ // // Created by xiang on 24-5-20. // #ifndef MIAO_VERTEX_SE3_H #define MIAO_VERTEX_SE3_H #include "core/graph/base_vertex.h" namespace lightning::miao { /** * \brief 3D pose Vertex, represented as an SE3 * * 更新量为6维：t, w，平移在前 * 更新方程： * t = t + Delta t * R = R Exp(w) */ class VertexSE3 : public BaseVertex<6, SE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// 覆盖oplus void OplusImpl(const double *update) override { Eigen::Map v(update); estimate_.translation() += Vec3d(v[0], v[1], v[2]); estimate_.so3() = estimate_.so3() * SO3::exp(Vec3d(v[3], v[4], v[5])); } }; } // namespace lightning::miao #endif // MIAO_VERTEX_SE3_H ================================================ FILE: src/core/miao/examples/CMakeLists.txt ================================================ add_subdirectory(data_fitting) add_subdirectory(icp) add_subdirectory(sba) add_subdirectory(bal) add_subdirectory(sphere) add_subdirectory(incremental) add_subdirectory(pose_graph) ================================================ FILE: src/core/miao/examples/bal/CMakeLists.txt ================================================ add_executable(bal_example bal_example.cc) target_link_libraries(bal_example ${PROJECT_NAME}.core ${PROJECT_NAME}.utils glog gflags ) ================================================ FILE: src/core/miao/examples/bal/bal_example.cc ================================================ // // Created by xiang on 24-6-3. // #include #include #include #include "core/common/eigen_types.h" #include "core/graph/base_binary_edge.h" #include "core/graph/base_vec_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "utils/sampler.h" #include "utils/timer.h" #include /** * \brief camera vertex which stores the parameters for a pinhole camera * * The parameters of the camera are * - rx,ry,rz representing the rotation axis, whereas the angle is given by * ||(rx,ry,rz)|| * - tx,ty,tz the translation of the camera * - f the focal length of the camera * - k1, k2 two radial distortion parameters */ struct BALCam { SE3 Twc_; Vec3d f_k1_k2_ = Vec3d ::Zero(); }; class VertexCameraBAL : public miao::BaseVertex<9, BALCam> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexCameraBAL() = default; void OplusImpl(const double* update) override { Eigen::Map upd(update); estimate_.Twc_ = SE3::exp(upd) * estimate_.Twc_; Eigen::Map upd_f(update + 6); estimate_.f_k1_k2_ += upd_f; } }; /** * \brief 3D world feature * * A 3D point feature in the world */ class VertexPointBAL : public miao::BaseVecVertex<3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexPointBAL() = default; }; /** * \brief edge representing the observation of a world feature by a camera * * see: http://grail.cs.washington.edu/projects/bal/ * We use a pinhole camera model; the parameters we estimate for each camera * area rotation R, a translation t, a focal length f and two radial distortion * parameters k1 and k2. The formula for projecting a 3D point X into a camera * R,t,f,k1,k2 is: * P = R * X + t (conversion from world to camera coordinates) * p = -P / P.z (perspective division) * p' = f * r(p) * p (conversion to pixel coordinates) where P.z is the third * (z) coordinate of P. * * In the last equation, r(p) is a function that computes a scaling factor to * undo the radial distortion: r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4. * * This gives a projection in pixels, where the origin of the image is the * center of the image, the positive x-axis points right, and the positive * y-axis points up (in addition, in the camera coordinate system, the positive * z-axis points backwards, so the camera is looking down the negative z-axis, * as in OpenGL). */ class EdgeObservationBAL : public miao::BaseBinaryEdge<2, Vec2d, VertexCameraBAL, VertexPointBAL> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeObservationBAL() {} void ComputeError() override { auto v_camera = (VertexCameraBAL*)(vertices_[0]); auto v_point = (VertexPointBAL*)(vertices_[1]); BALCam camera = v_camera->Estimate(); Vec3d point = v_point->Estimate(); Vec3d camPt = camera.Twc_ * point; if (camPt.z() > 0.01) { error_ = Vec2d(25.0, 0.0); } Vec2d projPt(-camPt.x() / camPt.z(), -camPt.y() / camPt.z()); double f = camera.f_k1_k2_[0]; double k1 = camera.f_k1_k2_[1]; double k2 = camera.f_k1_k2_[2]; double projPtSqN = projPt.squaredNorm(); double r = 1.0 + (k1 + k2 * projPtSqN) * projPtSqN; Vec2d calibPt = (f * r) * projPt; error_ = calibPt - measurement_; } void LinearizeOplus() override { auto v_camera = (VertexCameraBAL*)(vertices_[0]); auto v_point = (VertexPointBAL*)(vertices_[1]); BALCam camera = v_camera->Estimate(); Vec3d worldPt = v_point->Estimate(); SE3 T_W_C = camera.Twc_; Vec3d camPt = T_W_C * worldPt; if (camPt.z() > 0.01) { jacobian_oplus_xi_.setZero(); jacobian_oplus_xj_.setZero(); return; } Vec2d projPt(-camPt.x() / camPt.z(), -camPt.y() / camPt.z()); double f = camera.f_k1_k2_[0]; double k1 = camera.f_k1_k2_[1]; double k2 = camera.f_k1_k2_[2]; double projPtSqN = projPt.squaredNorm(); double r = 1.0 + (k1 + k2 * projPtSqN) * projPtSqN; Vec2d calibPt = (f * r) * projPt; { const Eigen::Matrix& R = T_W_C.so3().matrix(); double denum = -1.0 / (camPt[2] * camPt[2]); // the final values of the non-calibrated case Eigen::Matrix DprojPt; DprojPt.row(0) = (camPt[2] * R.row(0) - camPt[0] * R.row(2)) * denum; DprojPt.row(1) = (camPt[2] * R.row(1) - camPt[1] * R.row(2)) * denum; Eigen::Matrix DpPtSqN = 2.0 * projPt.transpose() * DprojPt; jacobian_oplus_xj_ = (f * r) * DprojPt + projPt * DpPtSqN * (f * (k1 + k2 * 2.0 * projPtSqN)); } { // camera pose Eigen::Matrix J; double dz = 1 / camPt[2]; double xdz = camPt[0] * dz; double ydz = camPt[1] * dz; double xydz2 = xdz * ydz; // the final values of the non-calibrated case Eigen::Matrix DprojPt; DprojPt(0, 0) = -dz; DprojPt(0, 1) = 0; DprojPt(0, 2) = xdz * dz; DprojPt(0, 3) = xydz2; DprojPt(0, 4) = -1 - xdz * xdz; DprojPt(0, 5) = ydz; DprojPt(1, 0) = 0; DprojPt(1, 1) = -dz; DprojPt(1, 2) = ydz * dz; DprojPt(1, 3) = 1 + ydz * ydz; DprojPt(1, 4) = -xydz2; DprojPt(1, 5) = -xdz; Eigen::Matrix DpPtSqN = 2.0 * projPt.transpose() * DprojPt; J = (f * r) * DprojPt + projPt * DpPtSqN * (f * (k1 + k2 * 2.0 * projPtSqN)); jacobian_oplus_xi_.block<2, 6>(0, 0) = J; } { /// jacobian calib Eigen::Matrix J; J.col(0) = r * projPt; // d(calibPt) / d(f) J.col(1) = (f * projPtSqN) * projPt; // d(calibPt) / d(k1) J.col(2) = (f * projPtSqN * projPtSqN) * projPt; // d(calibPt) / d(k2) jacobian_oplus_xi_.block<2, 3>(0, 6) = J; } } }; DEFINE_int32(max_iterations, 20, "max iterations"); DEFINE_bool(use_PCG, false, "use PCG instead of the Cholesky"); DEFINE_bool(verbose, true, "verbose output"); DEFINE_string(graph_input, "./dataset/bal/problem-49-7776-pre.txt", "file which will be processed"); int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning::miao; Eigen::setNbThreads(8); LOG(INFO) << "use_pcg " << FLAGS_use_PCG; auto optimizer = SetupOptimizer<9, 3>(OptimizerConfig( AlgorithmType::LEVENBERG_MARQUARDT, FLAGS_use_PCG ? LinearSolverType::LINEAR_SOLVER_PCG : LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN, false)); std::vector> points; std::vector> cameras; // parse BAL dataset LOG(INFO) << "Loading BAL dataset " << FLAGS_graph_input; std::ifstream ifs(FLAGS_graph_input); int numCameras, numPoints, numObservations; ifs >> numCameras >> numPoints >> numObservations; LOG(INFO) << "cam: " << numCameras << ", pts: " << numPoints << ", obs: " << numObservations; int id = 0; cameras.reserve(numCameras); for (int i = 0; i < numCameras; ++i, ++id) { auto cam = std::make_shared(); cam->SetId(id); optimizer->AddVertex(cam); cameras.emplace_back(cam); } points.reserve(numPoints); for (int i = 0; i < numPoints; ++i, ++id) { auto p = std::make_shared(); p->SetId(id); p->SetMarginalized(true); bool addedVertex = optimizer->AddVertex(p); if (!addedVertex) { LOG(ERROR) << "failing adding vertex"; } points.emplace_back(p); } std::vector> edges; // read in the observation for (int i = 0; i < numObservations; ++i) { int camIndex, pointIndex; double obsX, obsY; ifs >> camIndex >> pointIndex >> obsX >> obsY; assert(camIndex >= 0 && (size_t)camIndex < cameras.size() && "Index out of bounds"); auto cam = cameras[camIndex]; assert(pointIndex >= 0 && (size_t)pointIndex < points.size() && "Index out of bounds"); auto point = points[pointIndex]; auto e = std::make_shared(); e->SetVertex(0, cam); e->SetVertex(1, point); e->SetInformation(Mat2d::Identity()); e->SetMeasurement(Vec2d(obsX, obsY)); bool addedEdge = optimizer->AddEdge(e); if (!addedEdge) { LOG(ERROR) << "error adding edge"; } edges.emplace_back(e); } /// 罗德里格斯参数转四元数 auto rodriguezToQuat = [](const Vec3d& rRot) { double angle = rRot.norm(); if (angle < 1e-12) { return Quat(1, 0, 0, 0); } Vec3d v = rRot / angle; double c = cos(angle / 2.0), s = sin(angle / 2.0); return Quat(c, s * v.x(), s * v.y(), s * v.z()); }; // read in the camera params for (int i = 0; i < numCameras; ++i) { Vec9d data; for (int j = 0; j < 9; ++j) { ifs >> data(j); } auto cam = cameras[i]; BALCam c; c.Twc_ = SE3(SO3(rodriguezToQuat(data.head<3>())), Vec3d(data[3], data[4], data[5])); c.f_k1_k2_ = data.tail<3>(); cam->SetEstimate(c); } // read in the points for (int i = 0; i < numPoints; ++i) { Vec3d p; ifs >> p(0) >> p(1) >> p(2); auto point = points[i]; point->SetEstimate(p); } optimizer->InitializeOptimization(); optimizer->SetVerbose(FLAGS_verbose); Timer::Evaluate([&]() { optimizer->Optimize(FLAGS_max_iterations); }, "optimize"); miao::Timer::DumpIntoFile("./miao_bal.txt"); Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/examples/data_fitting/CMakeLists.txt ================================================ add_executable(circle_fit circle_fit.cc) set_target_properties(circle_fit PROPERTIES OUTPUT_NAME circle_fit) target_link_libraries(circle_fit ${PROJECT_NAME}.core gflags glog ${PROJECT_NAME}.core ${PROJECT_NAME}.utils ) add_executable(curve_fit curve_fit.cc) set_target_properties(curve_fit PROPERTIES OUTPUT_NAME curve_fit) target_link_libraries(curve_fit ${PROJECT_NAME}.core gflags glog ${PROJECT_NAME}.core ${PROJECT_NAME}.utils ) ================================================ FILE: src/core/miao/examples/data_fitting/circle_fit.cc ================================================ // // Created by xiang on 24-4-26. // #include #include #include "core/graph/base_unary_edge.h" #include "core/graph/base_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "utils/sampler.h" #include "utils/timer.h" /** * \brief a circle located at x,y with radius r */ class VertexCircle : public miao::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; VertexCircle() {} void OplusImpl(const double *update) override { Eigen::Vector3d::ConstMapType v(update); estimate_ += v; } }; /** * \brief measurement for a point on the circle * * Here the measurement is the point which is on the circle. * The error function computes the distance of the point to * the center minus the radius of the circle. */ class EdgePointOnCircle : public miao::BaseUnaryEdge<1, Eigen::Vector2d, VertexCircle> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePointOnCircle() {} /// 计算误差 void ComputeError() override { auto v = (VertexCircle *)(vertices_[0]); auto circle = v->Estimate(); const double radius = circle[2]; const Vec2d center = circle.head<2>(); // err = sqrt((mx-x)^2 + (my-y)^2) - r error_[0] = (measurement_ - center).norm() - radius; } /// 雅可比使用数值导数 void LinearizeOplus() override { auto v = (VertexCircle *)(vertices_[0]); auto circle = v->Estimate(); const Vec2d center = circle.head<2>(); double e0 = (measurement_ - center).norm(); jacobian_oplus_xi_[0] = (circle[0] - measurement_[0]) / e0; jacobian_oplus_xi_[1] = (circle[1] - measurement_[1]) / e0; jacobian_oplus_xi_[2] = -1; } }; double errorOfSolution(int numPoints, const std::vector &points, const Eigen::Vector3d &circle) { Eigen::Vector2d center = circle.head<2>(); double radius = circle(2); double error = 0.; for (int i = 0; i < numPoints; ++i) { double d = (points[i] - center).norm() - radius; error += d * d; } return error; } DEFINE_int32(num_points, 100, "number of points"); DEFINE_int32(max_iterations, 10, "number of iterations"); DEFINE_bool(verbose, true, "Verbose output"); int main(int argc, char **argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); Vec2d center(4.0, 2.0); double radius = 2.0; std::vector points(FLAGS_num_points); miao::Sampler::seedRand(); for (int i = 0; i < FLAGS_num_points; ++i) { double r = miao::Sampler::gaussRand(radius, 0.05); double angle = miao::Sampler::uniformRand(0.0, 2.0 * M_PI); points[i].x() = center.x() + r * cos(angle); points[i].y() = center.y() + r * sin(angle); } auto optimizer = miao::SetupOptimizer<3, -1>(miao::OptimizerConfig(miao::AlgorithmType::GAUSS_NEWTON)); // 1. add the circle vertex auto circle = std::make_shared(); circle->SetId(0); circle->SetEstimate(Eigen::Vector3d(3.0, 3.0, 3.0)); // some initial value for the circle optimizer->AddVertex(circle); // 2. add the points we measured for (int i = 0; i < FLAGS_num_points; ++i) { auto e = std::make_shared(); e->SetInformation(Eigen::Matrix::Identity()); e->SetVertex(0, circle); e->SetMeasurement(points[i]); optimizer->AddEdge(e); } // perform the optimization miao::Timer::Evaluate( [&]() { optimizer->InitializeOptimization(); optimizer->SetVerbose(FLAGS_verbose); optimizer->Optimize(FLAGS_max_iterations); }, "Miao optimizer"); // print out the result LOG(INFO) << "Iterative least squares solution"; LOG(INFO) << "center of the circle " << circle->Estimate().head<2>().transpose(); LOG(INFO) << "radius of the cirlce " << circle->Estimate()(2); LOG(INFO) << "error " << errorOfSolution(FLAGS_num_points, points, circle->Estimate()); miao::Timer::Evaluate( [&]() { Eigen::MatrixXd A(FLAGS_num_points, 3); Eigen::VectorXd b(FLAGS_num_points); /// NOTE: 这里的定义貌似不完全一样 for (int i = 0; i < FLAGS_num_points; ++i) { A(i, 0) = -2 * points[i].x(); A(i, 1) = -2 * points[i].y(); A(i, 2) = 1; b(i) = -pow(points[i].x(), 2) - pow(points[i].y(), 2); } Vec3d solution = (A.transpose() * A).ldlt().solve(A.transpose() * b); // calculate the radius of the circle given the solution so far solution(2) = sqrt(pow(solution(0), 2) + pow(solution(1), 2) - solution(2)); LOG(INFO) << "Linear least squares solution"; LOG(INFO) << "center of the circle " << solution.head<2>().transpose(); LOG(INFO) << "radius of the cirlce " << solution(2); LOG(INFO) << "error " << errorOfSolution(FLAGS_num_points, points, solution); }, "GN Solver"); miao::Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/examples/data_fitting/curve_fit.cc ================================================ // // Created by xiang on 24-5-14. // #include #include #include "core/graph/base_unary_edge.h" #include "core/graph/base_vec_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "utils/sampler.h" #include "utils/timer.h" /** * \brief the params, a, GetB, and lambda for a * exp(-lambda * t) + GetB */ class VertexParams : public miao::BaseVecVertex<3> {}; /** * \brief measurement for a point on the curve * * Here the measurement is the point which is lies on the curve. * The error function computes the difference between the curve * and the point. */ class EdgePointOnCurve : public miao::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePointOnCurve() = default; /// 计算误差 void ComputeError() override { auto v = (VertexParams*)(GetVertex(0)); Vec3d est = v->Estimate(); double fval = est[0] * exp(-est[2] * measurement_[0]) + est[1]; error_[0] = fval - measurement_[1]; } void LinearizeOplus() override { auto v = (VertexParams*)(GetVertex(0)); Vec3d est = v->Estimate(); double exp_c0 = exp(-est[2] * measurement_[0]); jacobian_oplus_xi_[0] = exp_c0; jacobian_oplus_xi_[1] = 1; jacobian_oplus_xi_[2] = est[0] * (-est[2]) * exp_c0; } }; DEFINE_int32(num_points, 100, "number of points"); DEFINE_int32(max_iterations, 10, "number of iterations"); DEFINE_bool(verbose, true, "Verbose output"); int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); // generate random data miao::Sampler::seedRand(); double a = 2.; double b = 0.4; double lambda = 0.2; std::vector points(FLAGS_num_points); for (int i = 0; i < FLAGS_num_points; ++i) { double x = miao::Sampler::uniformRand(0, 10); double y = a * exp(-lambda * x) + b; // add Gaussian noise y += miao::Sampler::gaussRand(0, 0.02); points[i].x() = x; points[i].y() = y; } // setup the solver miao::Optimizer optimizer; optimizer.SetVerbose(false); miao::SetupOptimizer<3, -1>(optimizer, miao::OptimizerConfig(miao::AlgorithmType::LEVENBERG_MARQUARDT)); // build the optimization problem given the points // 1. add the parameter vertex auto params = std::make_shared(); params->SetId(0); params->SetEstimate(Eigen::Vector3d(1, 1, 1)); // some initial value for the params optimizer.AddVertex(params); // 2. add the points we measured to be on the curve for (int i = 0; i < FLAGS_num_points; ++i) { auto e = std::make_shared(); e->SetInformation(Eigen::Matrix::Identity()); e->SetVertex(0, params); e->SetMeasurement(points[i]); optimizer.AddEdge(e); } // perform the optimization miao::Timer::Evaluate( [&]() { optimizer.InitializeOptimization(); optimizer.SetVerbose(FLAGS_verbose); optimizer.Optimize(FLAGS_max_iterations); }, "optimization"); // print out the result LOG(INFO) << "Target curve"; LOG(INFO) << "a * exp(-lambda * x) + b"; LOG(INFO) << "Iterative least squares solution"; LOG(INFO) << "a = " << params->Estimate()(0); LOG(INFO) << "b = " << params->Estimate()(1); LOG(INFO) << "lambda = " << params->Estimate()(2); LOG(INFO) << "real abc: " << a << ", " << b << ", " << lambda; miao::Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/examples/icp/CMakeLists.txt ================================================ add_executable(gicp_demo gicp_demo.cc) target_link_libraries(gicp_demo ${PROJECT_NAME}.core ${PROJECT_NAME}.utils glog gflags ) ================================================ FILE: src/core/miao/examples/icp/gicp_demo.cc ================================================ // // Created by xiang on 24-5-20. // #include #include #include #include #include #include "core/common/eigen_types.h" #include "core/graph/base_binary_edge.h" #include "core/graph/base_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "utils/sampler.h" #include "utils/timer.h" #include "core/types/vertex_se3.h" class EdgeGICP { EIGEN_MAKE_ALIGNED_OPERATOR_NEW; public: // point positions Vec3d pos0_ = Vec3d::Zero(), pos1_ = Vec3d::Zero(); // unit normals Vec3d normal0_, normal1_; // rotation matrix for normal Mat3d R0_, R1_; // initialize an object EdgeGICP() { normal0_ << 0, 0, 1; normal1_ << 0, 0, 1; // makeRot(); R0_.setIdentity(); R1_.setIdentity(); } // set up rotation matrix for pos0 void makeRot0() { Vector3 y; y << 0, 1, 0; R0_.row(2) = normal0_; y = y - normal0_(1) * normal0_; y.normalize(); // need to check if y is close to 0 R0_.row(1) = y; R0_.row(0) = normal0_.cross(R0_.row(1)); } // set up rotation matrix for pos1 void makeRot1() { Vector3 y; y << 0, 1, 0; R1_.row(2) = normal1_; y = y - normal1_(1) * normal1_; y.normalize(); // need to check if y is close to 0 R1_.row(1) = y; R1_.row(0) = normal1_.cross(R1_.row(1)); } // returns a precision matrix for point-plane Matrix3 prec0(double e) { makeRot0(); Matrix3 prec; prec << e, 0, 0, 0, e, 0, 0, 0, 1; return R0_.transpose() * prec * R0_; } // returns a precision matrix for point-plane Matrix3 prec1(double e) { makeRot1(); Matrix3 prec; prec << e, 0, 0, 0, e, 0, 0, 0, 1; return R1_.transpose() * prec * R1_; } // return a covariance matrix for plane-plane Matrix3 cov0(double e) { makeRot0(); Matrix3 cov; cov << 1, 0, 0, 0, 1, 0, 0, 0, e; return R0_.transpose() * cov * R0_; } // return a covariance matrix for plane-plane Matrix3 cov1(double e) { makeRot1(); Matrix3 cov; cov << 1, 0, 0, 0, 1, 0, 0, 0, e; return R1_.transpose() * cov * R1_; } }; // 3D rigid constraint // 3 values for position wrt frame // 3 values for normal wrt frame, not used here // first two args are the measurement type, second two the connection classes class Edge_V_V_GICP : public miao::BaseBinaryEdge<3, EdgeGICP, miao::VertexSE3, miao::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; Edge_V_V_GICP() : pl_pl(false) {} // switch to go between point-plane and plane-plane bool pl_pl; Matrix3 cov0, cov1; // return the error estimate as a 3-vector void ComputeError() override { // from to const auto vp0 = (miao::VertexSE3*)(vertices_[0]); const auto vp1 = (miao::VertexSE3*)(vertices_[1]); // get vp1 point into vp0 frame // could be more efficient if we computed this transform just once Vector3 p1; p1 = vp1->Estimate() * measurement_.pos1_; p1 = vp0->Estimate().inverse() * p1; // get their difference // this is simple Euclidean distance, for now error_ = p1 - measurement_.pos0_; if (!pl_pl) { return; } // re-define the information matrix // topLeftCorner<3,3>() is the rotation() const Matrix3 transform = (vp0->Estimate().inverse() * vp1->Estimate()).matrix().topLeftCorner<3, 3>(); information_ = (cov0 + transform * cov1 * transform.transpose()).inverse(); } }; int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning::miao; double euc_noise = 0.01; // noise in position, m // double outlier_ratio = 0.1; Optimizer optimizer; optimizer.SetVerbose(false); SetupOptimizer(optimizer, OptimizerConfig(AlgorithmType::LEVENBERG_MARQUARDT)); std::vector true_points; for (size_t i = 0; i < 1000; ++i) { true_points.emplace_back((Sampler::uniformRand(0., 1.) - 0.5) * 3, Sampler::uniformRand(0., 1.) - 0.5, Sampler::uniformRand(0., 1.) + 10); } // set up two poses int vertex_id = 0; std::shared_ptr vp0, vp1; for (size_t i = 0; i < 2; ++i) { // set up rotation and translation for this node Vec3d t(0, 0, i); Quat q; q.setIdentity(); SE3 cam(q, t); // camera pose // set up node auto vc = std::make_shared(); vc->SetEstimate(cam); vc->SetId(vertex_id); // vertex id // set first cam pose fixed if (i == 0) { vc->SetFixed(true); vp0 = vc; } else { vp1 = vc; } // add to optimizer optimizer.AddVertex(vc); vertex_id++; } // set up point matches for (size_t i = 0; i < true_points.size(); ++i) { // calculate the relative 3D position of the point Vec3d pt0, pt1; pt0 = vp0->Estimate().inverse() * true_points[i]; pt1 = vp1->Estimate().inverse() * true_points[i]; // add in noise pt0 += Vec3d(Sampler::gaussRand(0., euc_noise), Sampler::gaussRand(0., euc_noise), Sampler::gaussRand(0., euc_noise)); pt1 += Vec3d(Sampler::gaussRand(0., euc_noise), Sampler::gaussRand(0., euc_noise), Sampler::gaussRand(0., euc_noise)); // form edge, with normals in varioius positions Vec3d nm0, nm1; nm0 << 0, i, 1; nm1 << 0, i, 1; nm0.normalize(); nm1.normalize(); // new edge with correct cohort for caching auto e = std::make_shared(); e->SetVertex(0, vp0); // first viewpoint e->SetVertex(1, vp1); // second viewpoint EdgeGICP meas; meas.pos0_ = pt0; meas.pos1_ = pt1; meas.normal0_ = nm0; meas.normal1_ = nm1; e->SetMeasurement(meas); meas = e->GetMeasurement(); // use this for point-plane e->Information() = meas.prec0(0.01); // use this for point-point // e->information().setIdentity(); // e->setRobustKernel(true); // e->setHuberWidth(0.01); optimizer.AddEdge(e); } // move second cam off of its true position auto vc = vp1; SE3 cam = vc->Estimate(); cam.translation() = Vec3d(0, 0, 0.2); vc->SetEstimate(cam); optimizer.InitializeOptimization(); optimizer.ComputeActiveErrors(); LOG(INFO) << "Initial chi2 = " << optimizer.Chi2(); optimizer.SetVerbose(true); optimizer.Optimize(5); LOG(INFO) << "Second vertex should be near 0,0,1"; LOG(INFO) << vp0->Estimate().translation().transpose() << ", " << vp0->Estimate().unit_quaternion().coeffs().transpose(); LOG(INFO) << vp1->Estimate().translation().transpose() << ", " << vp1->Estimate().unit_quaternion().coeffs().transpose(); return 0; } ================================================ FILE: src/core/miao/examples/incremental/CMakeLists.txt ================================================ add_executable(incremental incremental.cc) target_link_libraries(incremental ${PROJECT_NAME}.core ${PROJECT_NAME}.utils glog gflags ) ================================================ FILE: src/core/miao/examples/incremental/incremental.cc ================================================ // // Created by xiang on 24-6-4. // #include #include #include #include #include #include "core/common/eigen_types.h" #include "core/graph/base_binary_edge.h" #include "core/graph/base_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "core/types/edge_se3.h" #include "core/types/vertex_se3.h" #include "utils/sampler.h" #include "utils/timer.h" DEFINE_int32(nodes_per_level, 100, "how many nodes per lap on the sphere"); DEFINE_int32(num_laps, 30, "how many times the robot travels around the sphere"); DEFINE_double(radius, 100.0, "radius of the sphere"); DEFINE_double(noise_translation, 0.1, "noise level for the translation"); DEFINE_double(noise_rotation, 0.001, "noise level for the rotation"); DEFINE_bool(incremental_mode, true, "should we set incremental mode"); DEFINE_int32(inc_vertex_size, -1, "max vertex size in inc mode, set -1 as unlimited size"); int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning::miao; // command line parsing int nodesPerLevel = FLAGS_nodes_per_level; double radius = FLAGS_radius; std::vector noiseTranslation; std::vector noiseRotation; for (int i = 0; i < 3; ++i) { noiseTranslation.emplace_back(FLAGS_noise_translation); noiseRotation.emplace_back(FLAGS_noise_rotation); } Mat3d transNoise = Mat3d::Zero(); for (int i = 0; i < 3; ++i) { transNoise(i, i) = std::pow(noiseTranslation[i], 2); } Mat3d rotNoise = Mat3d::Zero(); for (int i = 0; i < 3; ++i) { rotNoise(i, i) = std::pow(noiseRotation[i], 2); } Mat6d information = Mat6d::Zero(); information.block<3, 3>(0, 0) = transNoise.inverse(); information.block<3, 3>(3, 3) = rotNoise.inverse(); std::vector> vertices; std::vector> odometryEdges; std::vector> edges; OptimizerConfig config{AlgorithmType::LEVENBERG_MARQUARDT, miao::LinearSolverType::LINEAR_SOLVER_PCG, false}; if (FLAGS_incremental_mode) { LOG(INFO) << "setting incremental mode"; } else { LOG(INFO) << "not setting incremental mode"; } config.incremental_mode_ = FLAGS_incremental_mode; config.max_vertex_size_ = FLAGS_inc_vertex_size; auto optimizer = SetupOptimizer(config); // 增量形式的 optimizer->SetVerbose(true); GaussianSampler transSampler; transSampler.setDistribution(transNoise); GaussianSampler rotSampler; rotSampler.setDistribution(rotNoise); auto gen_noisy_meas = [&](const SE3& meas) { Eigen::Quaterniond gtQuat = meas.so3().unit_quaternion(); Vec3d gtTrans = meas.translation(); Vec3d quatXYZ = rotSampler.generateSample(); double qw = 1.0 - quatXYZ.norm(); if (qw < 0) { qw = 0.; } Eigen::Quaterniond rot(qw, quatXYZ.x(), quatXYZ.y(), quatXYZ.z()); rot.normalize(); Eigen::Vector3d trans = transSampler.generateSample(); rot = gtQuat * rot; trans = gtTrans + trans; return SE3(rot, trans); }; int id = 0; std::shared_ptr prev = nullptr; std::map motion_data; for (int f = 0; f < FLAGS_num_laps; ++f) { for (int n = 0; n < nodesPerLevel; ++n) { Eigen::AngleAxisd rotz(-M_PI + 2 * n * M_PI / nodesPerLevel, Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd roty(-0.5 * M_PI + id * M_PI / (FLAGS_num_laps * nodesPerLevel), Eigen::Vector3d::UnitY()); Eigen::Matrix3d rot = (rotz * roty).toRotationMatrix(); Eigen::Isometry3d t; t = rot; t.translation() = t.linear() * Eigen::Vector3d(radius, 0, 0); motion_data.emplace(id++, SE3(t.matrix())); } } id = 0; for (int f = 0; f < FLAGS_num_laps; ++f) { for (int n = 0; n < nodesPerLevel; ++n) { auto v = std::make_shared(); int idx = id; v->SetId(id++); v->SetEstimate(motion_data[idx]); vertices.push_back(v); optimizer->AddVertex(v); if (prev != nullptr) { SE3 motion = motion_data[prev->GetId()].inverse() * v->Estimate(); auto e = std::make_shared(); e->SetVertex(0, prev); e->SetVertex(1, v); e->SetMeasurement(gen_noisy_meas(motion)); e->SetInformation(information); odometryEdges.emplace_back(e); edges.emplace_back(e); optimizer->AddEdge(e); } prev = v; if (id % 10 == 0) { LOG(INFO) << "optimizing for " << id; Timer::Evaluate( [&]() { optimizer->InitializeOptimization(); optimizer->Optimize(5); }, "optimization", true); } /// loop closure if (f == 0 || n == 1) { continue; } for (int nn = -1; nn <= 1; ++nn) { int idx = (f - 1) * nodesPerLevel + nn + n; if (idx < 0 || idx >= vertices.size()) { continue; } if (FLAGS_inc_vertex_size > 0 && (id - idx) >= FLAGS_inc_vertex_size) { continue; } auto from = vertices[idx]; SE3 pose = motion_data[idx].inverse() * v->Estimate(); auto e = std::make_shared(); e->SetVertex(0, from); e->SetVertex(1, v); e->SetMeasurement(gen_noisy_meas(pose)); e->SetInformation(information); edges.emplace_back(e); optimizer->AddEdge(e); } } } Timer::Evaluate( [&]() { optimizer->InitializeOptimization(); optimizer->Optimize(5); }, "optimization", true); Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/examples/pose_graph/CMakeLists.txt ================================================ add_executable(pose_graph pose_graph.cc) target_link_libraries(pose_graph ${PROJECT_NAME}.core ${PROJECT_NAME}.utils glog gflags ) ================================================ FILE: src/core/miao/examples/pose_graph/pose_graph.cc ================================================ // // Created by xiang on 24-6-14. // #include #include #include #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "core/robust_kernel/robust_kernel_all.h" #include "core/types/edge_se2.h" #include "core/types/edge_se3.h" #include "core/types/vertex_se2.h" #include "core/types/vertex_se3.h" #include "utils/timer.h" DEFINE_string(g2o_file, "./dataset/pose_graph/sphere_bignoise_vertex3.g2o", "noise level for the rotation"); DEFINE_bool(is_3d_pose, true, "3d or 2d pose graph"); DEFINE_bool(with_rb, false, "add robust kernel"); DEFINE_bool(use_PCG, false, "use PCG solver"); DEFINE_int32(iterations, 100, "num of iterations"); int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning::miao; std::ifstream fin(FLAGS_g2o_file); if (!fin) { LOG(ERROR) << "cannot load file: " << FLAGS_g2o_file; return -1; } std::shared_ptr optimizer = nullptr; OptimizerConfig config( AlgorithmType::LEVENBERG_MARQUARDT, FLAGS_use_PCG ? LinearSolverType::LINEAR_SOLVER_PCG : LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN, false); if (FLAGS_is_3d_pose) { optimizer = SetupOptimizer<6, 3>(config); } else { optimizer = SetupOptimizer<3, 2>(config); } std::vector> vertices_3d; std::vector> edges_3d; std::vector> vertices_2d; std::vector> edges_2d; /// NOTE: g2o VertexSE3 与 miao::VertexSE3 /// 的误差计算方式并不完全一致，rotation部分的误差和更新量相差一半，因此对rotation部分的协方差要除以4 while (!fin.eof()) { std::string line; std::getline(fin, line); if (line.empty()) { continue; } std::stringstream ss; ss << line; std::string name; ss >> name; if (name == "VERTEX_SE3:QUAT") { if (FLAGS_is_3d_pose == false) { LOG(FATAL) << "get 3d pose, but you set a 2D problem."; } int id = 0; double data[7] = {0}; ss >> id; for (int i = 0; i < 7; ++i) { ss >> data[i]; } auto v = std::make_shared(); v->SetId(id); v->SetEstimate(SE3(Quatd(data[6], data[3], data[4], data[5]), Vec3d(data[0], data[1], data[2]))); optimizer->AddVertex(v); vertices_3d.emplace_back(v); } else if (name == "VERTEX_SE2") { if (FLAGS_is_3d_pose == true) { LOG(FATAL) << "get 2d pose, but you set a 3D problem."; } int id = 0; double data[3] = {0}; ss >> id; for (int i = 0; i < 3; ++i) { ss >> data[i]; } auto v = std::make_shared(); v->SetId(id); v->SetEstimate(SE2(SO2(data[2]), Vec2d(data[0], data[1]))); optimizer->AddVertex(v); vertices_2d.emplace_back(v); } else if (name == "EDGE_SE3:QUAT") { auto e = std::make_shared(); int v1 = 0, v2 = 0; ss >> v1 >> v2; e->SetVertex(0, optimizer->GetVertex(v1)); e->SetVertex(1, optimizer->GetVertex(v2)); double data[7] = {0}; for (int i = 0; i < 7; ++i) { ss >> data[i]; } SE3 meas(Quatd(data[6], data[3], data[4], data[5]), Vec3d(data[0], data[1], data[2])); e->SetMeasurement(meas); // read info Mat6d info = Mat6d::Zero(); for (int i = 0; i < info.rows(); ++i) { for (int j = i; j < info.cols(); ++j) { ss >> info(i, j); if (i != j) { info(j, i) = info(i, j); } } } info.block<3, 3>(3, 3) = 0.25 * info.block<3, 3>(3, 3); e->SetInformation(info); if (FLAGS_with_rb) { e->SetRobustKernel(std::make_shared()); } optimizer->AddEdge(e); edges_3d.emplace_back(e); } else if (name == "EDGE_SE2") { auto e = std::make_shared(); int v1 = 0, v2 = 0; ss >> v1 >> v2; e->SetVertex(0, optimizer->GetVertex(v1)); e->SetVertex(1, optimizer->GetVertex(v2)); double data[3] = {0}; for (int i = 0; i < 3; ++i) { ss >> data[i]; } SE2 meas(SO2(data[2]), Vec2d(data[0], data[1])); e->SetMeasurement(meas); // read info Mat3d info = Mat3d::Zero(); for (int i = 0; i < info.rows(); ++i) { for (int j = i; j < info.cols(); ++j) { ss >> info(i, j); if (i != j) { info(j, i) = info(i, j); } } } e->SetInformation(info); if (FLAGS_with_rb) { e->SetRobustKernel(std::make_shared()); } optimizer->AddEdge(e); edges_2d.emplace_back(e); } } LOG(INFO) << "vert: " << optimizer->GetVertices().size() << ", edges: " << optimizer->GetEdges().size(); optimizer->InitializeOptimization(); optimizer->SetVerbose(true); Timer::Evaluate([&]() { optimizer->Optimize(FLAGS_iterations); }, "optimize"); /// TODO: save LOG(INFO) << "saving graph to result.g2o"; std::ofstream fout("./result.g2o"); for (const auto& v : vertices_3d) { fout << "VERTEX_SE3:QUAT " << v->GetId() << " "; Vec3d t = v->Estimate().translation(); Quatd q = v->Estimate().unit_quaternion(); fout << t[0] << " " << t[1] << " " << t[2] << " " << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << std::endl; } for (const auto& v : vertices_2d) { fout << "VERTEX_SE2 " << v->GetId() << " "; Vec2d t = v->Estimate().translation(); double theta = v->Estimate().so2().log(); fout << t[0] << " " << t[1] << " " << theta << std::endl; } for (const auto& e : edges_3d) { fout << "EDGE_SE3:QUAT " << e->GetVertex(0)->GetId() << " " << e->GetVertex(1)->GetId() << " "; Vec3d t = e->GetMeasurement().translation(); Quatd q = e->GetMeasurement().unit_quaternion(); fout << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " "; Mat6d info = e->Information(); for (int i = 0; i < info.rows(); ++i) { for (int j = i; j < info.cols(); ++j) { fout << info(i, j) << " "; } } fout << std::endl; } for (const auto& e : edges_2d) { fout << "EDGE_SE2 " << e->GetVertex(0)->GetId() << " " << e->GetVertex(1)->GetId() << " "; Vec2d t = e->GetMeasurement().translation(); double theta = e->GetMeasurement().so2().log(); fout << t[0] << " " << t[1] << " " << theta << " "; auto info = e->Information(); for (int i = 0; i < info.rows(); ++i) { for (int j = i; j < info.cols(); ++j) { fout << info(i, j) << " "; } } fout << std::endl; } fout.close(); LOG(INFO) << "graph saved. "; miao::Timer::DumpIntoFile("./miao_pose.txt"); Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/examples/sba/CMakeLists.txt ================================================ add_executable(sba_demo sba_demo.cc) set_target_properties(sba_demo PROPERTIES OUTPUT_NAME sba_demo) target_link_libraries(sba_demo ${PROJECT_NAME}.core gflags glog ${PROJECT_NAME}.core ${PROJECT_NAME}.utils ) ================================================ FILE: src/core/miao/examples/sba/sba_demo.cc ================================================ // // Created by xiang on 24-5-21. // #include #include #include "core/graph/base_binary_edge.h" #include "core/graph/base_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "core/robust_kernel/robust_kernel_all.h" #include "core/types/vertex_pointxyz.h" #include "core/types/vertex_se3.h" #include "utils/sampler.h" #include "utils/timer.h" DEFINE_double(pixel_noise, 1.0, "pixel noise"); DEFINE_double(outlier_ratio, 0.0, "outlier ratio"); DEFINE_bool(robust_kernel, false, "use robust kernel"); DEFINE_bool(structure_only, false, "structure-only BA"); DEFINE_bool(dense, false, "use dense Solver"); /** * \brief Stereo camera vertex, derived from SE3 class. * Note that we use the actual pose of the vertex as its parameterization, * rather than the transform from RW to camera coords. Uses static vars for * camera params, so there is a single camera setup. */ class VertexSCam : public miao::VertexSE3 { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSCam() {} // capture the update function to reset aux transforms void OplusImpl(const double* update) override { miao::VertexSE3::OplusImpl(update); setAll(); } // camera matrix and stereo baseline inline static Mat3d Kcam; inline static double baseline; // transformations Eigen::Matrix w2n; // transform from world to node coordinates Eigen::Matrix w2i; // transform from world to image coordinates // Derivatives of the rotation matrix transpose wrt quaternion xyz, used for // calculating Jacobian wrt pose of a projection. Mat3d dRdx, dRdy, dRdz; // transforms static void transformW2F(Eigen::Matrix& m, const Vec3d& trans, const Quat& qrot) { m.block<3, 3>(0, 0) = qrot.toRotationMatrix().transpose(); m.col(3).setZero(); // make sure there's no translation Vec4d tt; tt.head(3) = trans; tt[3] = 1.0; m.col(3) = -m * tt; } static void transformF2W(Eigen::Matrix& m, const Vec3d& trans, const Quat& qrot) { m.block<3, 3>(0, 0) = qrot.toRotationMatrix(); m.col(3) = trans; } // set up camera matrix static void setKcam(double fx, double fy, double cx, double cy, double tx) { Kcam.setZero(); Kcam(0, 0) = fx; Kcam(1, 1) = fy; Kcam(0, 2) = cx; Kcam(1, 2) = cy; Kcam(2, 2) = 1.0; baseline = tx; } // set transform from world to cam coords void setTransform() { w2n = estimate_.inverse().matrix().block<3, 4>(0, 0); } // Set up world-to-image projection matrix (w2i), assumes camera parameters // are filled. void setProjection() { w2i = Kcam * w2n; } // sets angle derivatives void setDr() { // inefficient, just for testing // use simple multiplications and additions for production code in // calculating dRdx,y,z for dS'*R', with dS the incremental change dRdx = dRidx * w2n.block<3, 3>(0, 0); dRdy = dRidy * w2n.block<3, 3>(0, 0); dRdz = dRidz * w2n.block<3, 3>(0, 0); } // set all aux transforms void setAll() { setTransform(); setProjection(); setDr(); } // calculate stereo projection void mapPoint(Vec3d& res, const Vec3d& pt3) { Vec4d pt; pt.head<3>() = pt3; pt(3) = miao::cst(1.0); Vec3d p1 = w2i * pt; Vec3d p2 = w2n * pt; Vec3d pb(baseline, 0, 0); double invp1 = miao::cst(1.0) / p1(2); res.head<2>() = p1.head<2>() * invp1; // right camera px p2 = Kcam * (p2 - pb); res(2) = p2(0) / p2(2); } inline static Mat3d dRidx; inline static Mat3d dRidy; inline static Mat3d dRidz; }; // stereo projection // first two args are the measurement type, second two the connection classes class Edge_XYZ_VSC : public miao::BaseBinaryEdge<3, Vector3, miao::VertexPointXYZ, VertexSCam> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Edge_XYZ_VSC() {} // return the error estimate as a 2-vector void ComputeError() override { // from to auto point = (miao::VertexPointXYZ*)(vertices_[0]); auto cam = (VertexSCam*)(vertices_[1]); // calculate the projection Vector3 kp; cam->mapPoint(kp, point->Estimate()); // error, which is backwards from the normal observed - calculated // _measurement is the measured projection error_ = kp - measurement_; } }; class Sample { public: static int uniform(int from, int to) { return static_cast(miao::Sampler::uniformRand(from, to)); } }; int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning::miao; Optimizer optimizer; if (FLAGS_dense) { LOG(INFO) << "Use dense Solver"; SetupOptimizer<6, 3>(optimizer, OptimizerConfig(AlgorithmType::LEVENBERG_MARQUARDT, LinearSolverType::LINEAR_SOLVER_DENSE, true)); } else { LOG(INFO) << "Use eigen sparse Solver"; SetupOptimizer<6, 3>(optimizer, OptimizerConfig(AlgorithmType::LEVENBERG_MARQUARDT, LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN, false)); } // set up 500 points std::vector true_points; for (size_t i = 0; i < 500; ++i) { true_points.emplace_back((Sampler::uniformRand(0., 1.) - 0.5) * 3, Sampler::uniformRand(0., 1.) - 0.5, Sampler::uniformRand(0., 1.) + 10); } Vec2d focal_length(500, 500); // pixels Vec2d principal_point(320, 240); // 640x480 image double baseline = 0.075; // 7.5 cm baseline std::vector true_poses; VertexSCam::setKcam(focal_length[0], focal_length[1], principal_point[0], principal_point[1], baseline); // set up 5 vertices, first 2 fixed int vertex_id = 0; for (size_t i = 0; i < 5; ++i) { Vec3d trans(i * 0.04 - 1., 0, 0); Eigen::Quaterniond q; q.setIdentity(); SE3 pose(q, trans); auto v_se3 = std::make_shared(); v_se3->SetId(vertex_id); v_se3->SetEstimate(pose); v_se3->setAll(); // set aux transforms if (i < 2) { v_se3->SetFixed(true); } optimizer.AddVertex(v_se3); true_poses.push_back(pose); vertex_id++; } int point_id = vertex_id; double sum_diff2 = 0; std::unordered_map pointid_2_trueid; std::unordered_set inliers; // add point projections to this vertex for (size_t i = 0; i < true_points.size(); ++i) { auto v_p = std::make_shared(); v_p->SetId(point_id); v_p->SetMarginalized(true); v_p->SetEstimate(true_points.at(i) + Vec3d(Sampler::gaussRand(0., 1), Sampler::gaussRand(0., 1), Sampler::gaussRand(0., 1))); int num_obs = 0; for (size_t j = 0; j < true_poses.size(); ++j) { /// 计算是否落在相机范围内 Vec3d z; std::dynamic_pointer_cast(optimizer.GetVertex(j))->mapPoint(z, true_points.at(i)); if (z[0] >= 0 && z[1] >= 0 && z[0] < 640 && z[1] < 480) { ++num_obs; } } if (num_obs < 2) { continue; } optimizer.AddVertex(v_p); bool inlier = true; for (size_t j = 0; j < true_poses.size(); ++j) { Vec3d z; std::dynamic_pointer_cast(optimizer.GetVertex(j))->mapPoint(z, true_points.at(i)); if (z[0] >= 0 && z[1] >= 0 && z[0] < 640 && z[1] < 480) { double sam = Sampler::uniformRand(0., 1.); if (sam < FLAGS_outlier_ratio) { z = Vec3d(Sample::uniform(64, 640), Sample::uniform(0, 480), Sample::uniform(0, 64)); // disparity z(2) = z(0) - z(2); // px' now inlier = false; } z += Vec3d(Sampler::gaussRand(0., FLAGS_pixel_noise), Sampler::gaussRand(0., FLAGS_pixel_noise), Sampler::gaussRand(0., FLAGS_pixel_noise / 16.0)); auto e = std::make_shared(); e->SetVertex(0, v_p); e->SetVertex(1, optimizer.GetVertices()[j]); e->SetMeasurement(z); e->SetInformation(Mat3d::Identity()); if (FLAGS_robust_kernel) { e->SetRobustKernel(std::make_shared()); } optimizer.AddEdge(e); } } if (inlier) { inliers.insert(point_id); Vec3d diff = v_p->Estimate() - true_points[i]; sum_diff2 += diff.dot(diff); } pointid_2_trueid.insert(std::make_pair(point_id, i)); ++point_id; } Timer::Evaluate( [&]() { optimizer.InitializeOptimization(); optimizer.SetVerbose(true); optimizer.Optimize(20); }, "Optimize"); LOG(INFO) << "Point error before optimisation (inliers only): " << sqrt(sum_diff2 / inliers.size()); sum_diff2 = 0; for (auto it : pointid_2_trueid) { auto v_p = std::dynamic_pointer_cast(optimizer.GetVertex(it.first)); if (v_p == nullptr) { LOG(ERROR) << "Vertex " << it.first << "is not a PointXYZ!"; exit(-1); } Vec3d diff = v_p->Estimate() - true_points[it.second]; if (inliers.find(it.first) == inliers.end()) { continue; } sum_diff2 += diff.dot(diff); } LOG(INFO) << "Point error after optimisation (inliers only): " << sqrt(sum_diff2 / inliers.size()); Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/examples/sphere/CMakeLists.txt ================================================ add_executable(sphere sphere.cc) target_link_libraries(sphere ${PROJECT_NAME}.core ${PROJECT_NAME}.utils glog gflags ) ================================================ FILE: src/core/miao/examples/sphere/sphere.cc ================================================ // // Created by xiang on 24-6-4. // #include #include #include #include #include #include "core/common/eigen_types.h" #include "core/graph/base_binary_edge.h" #include "core/graph/base_vertex.h" #include "core/graph/optimizer.h" #include "core/opti_algo/algo_select.h" #include "core/types/edge_se3.h" #include "core/types/vertex_se3.h" #include "utils/sampler.h" #include "utils/timer.h" DEFINE_int32(nodes_per_level, 50, "how many nodes per lap on the sphere"); DEFINE_int32(num_laps, 50, "how many times the robot travels around the sphere"); DEFINE_double(radius, 100.0, "radius of the sphere"); DEFINE_double(noise_translation, 0.1, "noise level for the translation"); DEFINE_double(noise_rotation, 0.001, "noise level for the rotation"); DEFINE_bool(use_PCG, false, "use PCG instead of the Cholesky"); int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_colorlogtostderr = true; FLAGS_stderrthreshold = google::INFO; gflags::ParseCommandLineFlags(&argc, &argv, true); using namespace lightning::miao; // command line parsing int nodesPerLevel = FLAGS_nodes_per_level; double radius = FLAGS_radius; std::vector noiseTranslation; std::vector noiseRotation; for (int i = 0; i < 3; ++i) { noiseTranslation.emplace_back(FLAGS_noise_translation); noiseRotation.emplace_back(FLAGS_noise_rotation); } Mat3d transNoise = Mat3d::Zero(); for (int i = 0; i < 3; ++i) { transNoise(i, i) = std::pow(noiseTranslation[i], 2); } Mat3d rotNoise = Mat3d::Zero(); for (int i = 0; i < 3; ++i) { rotNoise(i, i) = std::pow(noiseRotation[i], 2); } Mat6d information = Mat6d::Zero(); information.block<3, 3>(0, 0) = transNoise.inverse(); information.block<3, 3>(3, 3) = rotNoise.inverse(); std::vector> vertices; std::vector> odometryEdges; std::vector> edges; auto optimizer = SetupOptimizer(OptimizerConfig( AlgorithmType::LEVENBERG_MARQUARDT, FLAGS_use_PCG ? miao::LinearSolverType::LINEAR_SOLVER_PCG : miao::LinearSolverType::LINEAR_SOLVER_SPARSE_EIGEN, false)); int id = 0; for (int f = 0; f < FLAGS_num_laps; ++f) { for (int n = 0; n < nodesPerLevel; ++n) { auto v = std::make_shared(); v->SetId(id++); Eigen::AngleAxisd rotz(-M_PI + 2 * n * M_PI / nodesPerLevel, Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd roty(-0.5 * M_PI + id * M_PI / (FLAGS_num_laps * nodesPerLevel), Eigen::Vector3d::UnitY()); Eigen::Matrix3d rot = (rotz * roty).toRotationMatrix(); Eigen::Isometry3d t; t = rot; t.translation() = t.linear() * Eigen::Vector3d(radius, 0, 0); v->SetEstimate(SE3(t.matrix())); vertices.push_back(v); optimizer->AddVertex(v); } } // generate odometry edges for (size_t i = 1; i < vertices.size(); ++i) { auto prev = vertices[i - 1]; auto cur = vertices[i]; SE3 t = prev->Estimate().inverse() * cur->Estimate(); auto e = std::make_shared(); e->SetVertex(0, prev); e->SetVertex(1, cur); e->SetMeasurement(t); e->SetInformation(information); odometryEdges.emplace_back(e); edges.emplace_back(e); optimizer->AddEdge(e); } // generate loop closure edges for (int f = 1; f < FLAGS_num_laps; ++f) { for (int nn = 0; nn < nodesPerLevel; ++nn) { auto from = vertices[(f - 1) * nodesPerLevel + nn]; for (int n = -1; n <= 1; ++n) { if (f == FLAGS_num_laps - 1 && n == 1) { continue; } auto to = vertices[f * nodesPerLevel + nn + n]; SE3 t = from->Estimate().inverse() * to->Estimate(); auto e = std::make_shared(); e->SetVertex(0, from); e->SetVertex(1, to); e->SetMeasurement(t); e->SetInformation(information); edges.emplace_back(e); optimizer->AddEdge(e); } } } GaussianSampler transSampler; transSampler.setDistribution(transNoise); GaussianSampler rotSampler; rotSampler.setDistribution(rotNoise); // noise for all the edges for (auto& e : edges) { Eigen::Quaterniond gtQuat = e->GetMeasurement().so3().unit_quaternion(); Vec3d gtTrans = e->GetMeasurement().translation(); Vec3d quatXYZ = rotSampler.generateSample(); double qw = 1.0 - quatXYZ.norm(); if (qw < 0) { qw = 0.; } Eigen::Quaterniond rot(qw, quatXYZ.x(), quatXYZ.y(), quatXYZ.z()); rot.normalize(); Eigen::Vector3d trans = transSampler.generateSample(); rot = gtQuat * rot; trans = gtTrans + trans; SE3 noisyMeasurement(rot, trans); e->SetMeasurement(noisyMeasurement); } Timer::Evaluate( [&]() { optimizer->InitializeOptimization(); optimizer->SetVerbose(true); optimizer->Optimize(10); }, "optimize"); Timer::PrintAll(); return 0; } ================================================ FILE: src/core/miao/utils/CMakeLists.txt ================================================ add_library(miao.utils SHARED sampler.cc ) ================================================ FILE: src/core/miao/utils/sampler.cc ================================================ // // Created by xiang on 24-5-8. // #include "sampler.h" namespace lightning::miao { static std::normal_distribution uni_variate_sampler_(0., 1.); static std::uniform_real_distribution uniform_real_; static std::mt19937 gen_real_; double sampleUniform(double min, double max, std::mt19937* generator) { if (generator) { return uniform_real_(*generator) * (max - min) + min; } return uniform_real_(gen_real_) * (max - min) + min; } double sampleGaussian(std::mt19937* generator) { if (generator) { return uni_variate_sampler_(*generator); } return uni_variate_sampler_(gen_real_); } } // namespace lightning::miao ================================================ FILE: src/core/miao/utils/sampler.h ================================================ // // Created by xiang on 24-5-8. // #ifndef SAMPLER_H #define SAMPLER_H #include #include #include "common/eigen_types.h" #include "core/common/macros.h" namespace lightning::miao { double sampleUniform(double min = 0, double max = 1, std::mt19937* generator = 0); double sampleGaussian(std::mt19937* generator = 0); template class GaussianSampler { public: DISALLOW_COPY(GaussianSampler) explicit GaussianSampler(bool hasGenerator = true) : generator_(hasGenerator ? new std::mt19937 : nullptr) {} void setDistribution(const CovarianceType& cov) { Eigen::LLT cholDecomp; cholDecomp.compute(cov); if (cholDecomp.info() == Eigen::NumericalIssue) { assert(false && "Cholesky decomposition on the covariance matrix failed"); return; } cholesky_ = cholDecomp.matrixL(); } //! return a sample of the Gaussian distribution SampleType generateSample() { SampleType s; for (int i = 0; i < s.size(); i++) { s(i) = (generator_) ? sampleGaussian(generator_.get()) : sampleGaussian(); } return cholesky_ * s; } //! seed the random number generator, returns false if not having an own //! generator. bool seed(unsigned int s) { if (!generator_) { return false; } generator_->seed(s); return true; } protected: CovarianceType cholesky_; std::unique_ptr generator_; }; class Sampler { public: /** * Gaussian random with a mean and standard deviation. Uses the * Polar method of Marsaglia. */ static double gaussRand(double mean, double sigma) { double y, r2; do { double x = -1.0 + 2.0 * uniformRand(0.0, 1.0); y = -1.0 + 2.0 * uniformRand(0.0, 1.0); r2 = x * x + y * y; } while (r2 > 1.0 || r2 == 0.0); return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2); } /** * sample a number from a uniform distribution */ static double uniformRand(double lowerBndr, double upperBndr) { return lowerBndr + ((double)std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr); } /** * default seed function using the current time in seconds */ static void seedRand() { seedRand(static_cast(std::time(nullptr))); } /** seed the random number generator */ static void seedRand(unsigned int seed) { std::srand(seed); } }; } // namespace lightning::miao #endif // SAMPLER_H ================================================ FILE: src/core/miao/utils/tuple_tools.h ================================================ // // Created by xiang on 24-5-11. // #ifndef MIAO_TUPLE_TOOLS_H #define MIAO_TUPLE_TOOLS_H #include namespace lightning::miao { template void tuple_apply_i_(F &&f, T &t, int i, std::index_sequence) { (..., (I == i ? f(std::get(t)) : void())); } template void tuple_apply_i(F &&f, T &t, int i) { tuple_apply_i_(f, t, i, std::make_index_sequence>>()); } } // namespace lightning::miao #endif // MIAO_TUPLE_TOOLS_H ================================================ FILE: src/core/system/async_message_process.h ================================================ // // Created by xiang on 2022/2/9. // #ifndef ASYNC_MESSAGE_PROCESS_H #define ASYNC_MESSAGE_PROCESS_H #include #include #include #include #include #include #include namespace lightning::sys { using UL = std::unique_lock; /** * 异步消息处理类 * 内部有线程和队列机制，保证回调是串行的 * @tparam T * * NOTE skip设为1的时候实际不会跳帧。。设为2的时候实际走一帧跳一帧 */ template class AsyncMessageProcess { public: using ProcFunc = std::function; // 消息回调函数 AsyncMessageProcess() = default; AsyncMessageProcess(ProcFunc proc_func, std::string name = ""); /// 设置处理函数 void SetProcFunc(ProcFunc proc_func) { custom_func_ = proc_func; } /// 设置队列最大长度 void SetMaxSize(size_t size) { max_size_ = size; } /// 开始处理消息 void Start(); /// 添加一条消息 void AddMessage(const T& msg); /// 退出 void Quit(); /// 清空跳帧计数器，下一个数据会立即执行 void CleanSkipCnt(); void SetName(std::string name) { name_ = std::move(name); } void SetSkipParam(bool enable_skip, int skip_num) { enable_skip_ = enable_skip, skip_num_ = skip_num; } AsyncMessageProcess(const AsyncMessageProcess&) = delete; void operator=(const AsyncMessageProcess&) = delete; private: void ProcLoop(); std::thread proc_; std::mutex mutex_; std::condition_variable cv_msg_; std::deque msg_buffer_; bool update_flag_ = false; bool exit_flag_ = false; size_t max_size_ = 100; // 40 std::string name_; /// 跳帧 bool enable_skip_ = false; int skip_num_ = 0; int skip_cnt_ = 0; ProcFunc custom_func_; }; template void AsyncMessageProcess::CleanSkipCnt() { UL lock(mutex_); skip_cnt_ = 0; } template AsyncMessageProcess::AsyncMessageProcess(AsyncMessageProcess::ProcFunc proc_func, std::string name) { custom_func_ = std::move(proc_func); name_ = name; } template void AsyncMessageProcess::Start() { exit_flag_ = false; update_flag_ = false; proc_ = std::thread([this]() { ProcLoop(); }); } template void AsyncMessageProcess::ProcLoop() { while (!exit_flag_) { UL lock(mutex_); cv_msg_.wait(lock, [this]() { return update_flag_; }); // take the message and process it auto buffer = msg_buffer_; msg_buffer_.clear(); update_flag_ = false; lock.unlock(); // 处理之 for (const auto& msg : buffer) { custom_func_(msg); } } } template void AsyncMessageProcess::AddMessage(const T& msg) { UL lock(mutex_); if (enable_skip_) { if (skip_cnt_ != 0) { skip_cnt_++; skip_cnt_ = skip_cnt_ % skip_num_; return; } skip_cnt_++; skip_cnt_ = skip_cnt_ % skip_num_; } msg_buffer_.push_back(msg); while (msg_buffer_.size() > max_size_) { LOG(INFO) << name_ << " exceeds largest size: " << max_size_; msg_buffer_.pop_front(); } update_flag_ = true; cv_msg_.notify_one(); } template void AsyncMessageProcess::Quit() { update_flag_ = true; exit_flag_ = true; cv_msg_.notify_one(); if (proc_.joinable()) { proc_.join(); } } } // namespace lightning::sys #endif ================================================ FILE: src/core/system/loc_system.cc ================================================ // // Created by xiang on 25-9-12. // #include "core/system/loc_system.h" #include "core/localization/localization.h" #include "io/yaml_io.h" #include "wrapper/ros_utils.h" namespace lightning { LocSystem::LocSystem(LocSystem::Options options) : options_(options) { /// handle ctrl-c signal(SIGINT, lightning::debug::SigHandle); } LocSystem::~LocSystem() { loc_->Finish(); } bool LocSystem::Init(const std::string &yaml_path) { loc::Localization::Options opt; opt.online_mode_ = true; loc_ = std::make_shared(opt); YAML_IO yaml(yaml_path); std::string map_path = yaml.GetValue("system", "map_path"); LOG(INFO) << "online mode, creating ros2 node ... "; /// subscribers node_ = std::make_shared("lightning_slam"); imu_topic_ = yaml.GetValue("common", "imu_topic"); cloud_topic_ = yaml.GetValue("common", "lidar_topic"); livox_topic_ = yaml.GetValue("common", "livox_lidar_topic"); rclcpp::QoS qos(10); imu_sub_ = node_->create_subscription( imu_topic_, qos, [this](sensor_msgs::msg::Imu::SharedPtr msg) { IMUPtr imu = std::make_shared(); imu->timestamp = ToSec(msg->header.stamp); imu->linear_acceleration = Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); imu->angular_velocity = Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); ProcessIMU(imu); }); cloud_sub_ = node_->create_subscription( cloud_topic_, qos, [this](sensor_msgs::msg::PointCloud2::SharedPtr cloud) { Timer::Evaluate([&]() { ProcessLidar(cloud); }, "Proc Lidar", true); }); livox_sub_ = node_->create_subscription( livox_topic_, qos, [this](livox_ros_driver2::msg::CustomMsg ::SharedPtr cloud) { Timer::Evaluate([&]() { ProcessLidar(cloud); }, "Proc Lidar", true); }); if (options_.pub_tf_) { tf_broadcaster_ = std::make_shared(node_); loc_->SetTFCallback( [this](const geometry_msgs::msg::TransformStamped &pose) { tf_broadcaster_->sendTransform(pose); }); } bool ret = loc_->Init(yaml_path, map_path); if (ret) { LOG(INFO) << "online loc node has been created."; } return ret; } void LocSystem::SetInitPose(const SE3 &pose) { LOG(INFO) << "set init pose: " << pose.translation().transpose() << ", " << pose.unit_quaternion().coeffs().transpose(); loc_->SetExternalPose(pose.unit_quaternion(), pose.translation()); loc_started_ = true; } void LocSystem::ProcessIMU(const IMUPtr &imu) { if (loc_started_) { loc_->ProcessIMUMsg(imu); } } void LocSystem::ProcessLidar(const sensor_msgs::msg::PointCloud2::SharedPtr &cloud) { if (loc_started_) { loc_->ProcessLidarMsg(cloud); } } void LocSystem::ProcessLidar(const livox_ros_driver2::msg::CustomMsg::SharedPtr &cloud) { if (loc_started_) { loc_->ProcessLivoxLidarMsg(cloud); } } void LocSystem::Spin() { if (node_ != nullptr) { spin(node_); } } } // namespace lightning ================================================ FILE: src/core/system/loc_system.h ================================================ // // Created by xiang on 25-9-8. // #ifndef LIGHTNING_LOC_SYSTEM_H #define LIGHTNING_LOC_SYSTEM_H #include #include #include #include #include "livox_ros_driver2/msg/custom_msg.hpp" #include "common/eigen_types.h" #include "common/imu.h" #include "common/keyframe.h" namespace lightning { namespace loc { class Localization; } class LocSystem { public: struct Options { bool pub_tf_ = true; // 是否发布tf }; explicit LocSystem(Options options); ~LocSystem(); /// 初始化，地图路径在yaml里配置 bool Init(const std::string& yaml_path); /// 设置初始化位姿 void SetInitPose(const SE3& pose); /// 处理IMU void ProcessIMU(const lightning::IMUPtr& imu); /// 处理点云 void ProcessLidar(const sensor_msgs::msg::PointCloud2::SharedPtr& cloud); void ProcessLidar(const livox_ros_driver2::msg::CustomMsg::SharedPtr& cloud); /// 实时模式下的spin void Spin(); private: Options options_; std::shared_ptr loc_ = nullptr; // 定位接口 std::atomic_bool loc_started_ = false; // 是否开启定位 std::atomic_bool map_loaded_ = false; // 地图是否已载入 /// 实时模式下的ros2 node, subscribers rclcpp::Node::SharedPtr node_; std::shared_ptr tf_broadcaster_ = nullptr; std::string imu_topic_; std::string cloud_topic_; std::string livox_topic_; rclcpp::Subscription::SharedPtr imu_sub_ = nullptr; rclcpp::Subscription::SharedPtr cloud_sub_ = nullptr; rclcpp::Subscription::SharedPtr livox_sub_ = nullptr; }; }; // namespace lightning #endif // LIGHTNING_LOC_SYSTEM_H ================================================ FILE: src/core/system/slam.cc ================================================ // // Created by xiang on 25-5-6. // #include "core/system/slam.h" #include "core/g2p5/g2p5.h" #include "core/lio/laser_mapping.h" #include "core/loop_closing/loop_closing.h" #include "core/maps/tiled_map.h" #include "ui/pangolin_window.h" #include "wrapper/ros_utils.h" #include #include #include namespace lightning { SlamSystem::SlamSystem(lightning::SlamSystem::Options options) : options_(options) { /// handle ctrl-c signal(SIGINT, lightning::debug::SigHandle); } bool SlamSystem::Init(const std::string& yaml_path) { lio_ = std::make_shared(); if (!lio_->Init(yaml_path)) { LOG(ERROR) << "failed to init lio module"; return false; } auto yaml = YAML::LoadFile(yaml_path); options_.with_loop_closing_ = yaml["system"]["with_loop_closing"].as(); options_.with_visualization_ = yaml["system"]["with_ui"].as(); options_.with_2dvisualization_ = yaml["system"]["with_2dui"].as(); options_.with_gridmap_ = yaml["system"]["with_g2p5"].as(); options_.step_on_kf_ = yaml["system"]["step_on_kf"].as(); if (options_.with_loop_closing_) { LOG(INFO) << "slam with loop closing"; LoopClosing::Options options; options.online_mode_ = options_.online_mode_; lc_ = std::make_shared(options); lc_->Init(yaml_path); } if (options_.with_visualization_) { LOG(INFO) << "slam with 3D UI"; ui_ = std::make_shared(); ui_->Init(); lio_->SetUI(ui_); } if (options_.with_gridmap_) { g2p5::G2P5::Options opt; opt.online_mode_ = options_.online_mode_; g2p5_ = std::make_shared(opt); g2p5_->Init(yaml_path); if (options_.with_loop_closing_) { /// 当发生回环时，触发一次重绘 lc_->SetLoopClosedCB([this]() { g2p5_->RedrawGlobalMap(); }); } if (options_.with_2dvisualization_) { g2p5_->SetMapUpdateCallback([this](g2p5::G2P5MapPtr map) { cv::Mat image = map->ToCV(); cv::imshow("map", image); if (options_.step_on_kf_) { cv::waitKey(0); } else { cv::waitKey(10); } }); } } if (options_.online_mode_) { LOG(INFO) << "online mode, creating ros2 node ... "; /// subscribers node_ = std::make_shared("lightning_slam"); imu_topic_ = yaml["common"]["imu_topic"].as(); cloud_topic_ = yaml["common"]["lidar_topic"].as(); livox_topic_ = yaml["common"]["livox_lidar_topic"].as(); rclcpp::QoS qos(10); // qos.best_effort(); imu_sub_ = node_->create_subscription( imu_topic_, qos, [this](sensor_msgs::msg::Imu::SharedPtr msg) { IMUPtr imu = std::make_shared(); imu->timestamp = ToSec(msg->header.stamp); imu->linear_acceleration = Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); imu->angular_velocity = Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); ProcessIMU(imu); }); cloud_sub_ = node_->create_subscription( cloud_topic_, qos, [this](sensor_msgs::msg::PointCloud2::SharedPtr cloud) { Timer::Evaluate([&]() { ProcessLidar(cloud); }, "Proc Lidar", true); }); livox_sub_ = node_->create_subscription( livox_topic_, qos, [this](livox_ros_driver2::msg::CustomMsg ::SharedPtr cloud) { Timer::Evaluate([&]() { ProcessLidar(cloud); }, "Proc Lidar", true); }); savemap_service_ = node_->create_service( "lightning/save_map", [this](const SaveMapService::Request::SharedPtr& req, SaveMapService::Response::SharedPtr res) { SaveMap(req, res); }); LOG(INFO) << "online slam node has been created."; } return true; } SlamSystem::~SlamSystem() { if (ui_) { ui_->Quit(); } } void SlamSystem::StartSLAM(std::string map_name) { map_name_ = map_name; running_ = true; } void SlamSystem::SaveMap(const SaveMapService::Request::SharedPtr request, SaveMapService::Response::SharedPtr response) { map_name_ = request->map_id; std::string save_path = "./data/" + map_name_ + "/"; SaveMap(save_path); response->response = 0; } void SlamSystem::SaveMap(const std::string& path) { std::string save_path = path; if (save_path.empty()) { save_path = "./data/" + map_name_ + "/"; } LOG(INFO) << "slam map saving to " << save_path; if (!std::filesystem::exists(save_path)) { std::filesystem::create_directories(save_path); } else { std::filesystem::remove_all(save_path); std::filesystem::create_directories(save_path); } // auto global_map_no_loop = lio_->GetGlobalMap(true); auto global_map = lio_->GetGlobalMap(!options_.with_loop_closing_); // auto global_map_raw = lio_->GetGlobalMap(!options_.with_loop_closing_, false, 0.1); TiledMap::Options tm_options; tm_options.map_path_ = save_path; TiledMap tm(tm_options); SE3 start_pose = lio_->GetAllKeyframes().front()->GetOptPose(); tm.ConvertFromFullPCD(global_map, start_pose, save_path); pcl::io::savePCDFileBinaryCompressed(save_path + "/global.pcd", *global_map); // pcl::io::savePCDFileBinaryCompressed(save_path + "/global_no_loop.pcd", *global_map_no_loop); // pcl::io::savePCDFileBinaryCompressed(save_path + "/global_raw.pcd", *global_map_raw); if (options_.with_gridmap_) { /// 存为ROS兼容的模式 auto map = g2p5_->GetNewestMap()->ToROS(); const int width = map.info.width; const int height = map.info.height; cv::Mat nav_image(height, width, CV_8UC1); for (int y = 0; y < height; ++y) { const int rowStartIndex = y * width; for (int x = 0; x < width; ++x) { const int index = rowStartIndex + x; int8_t data = map.data[index]; if (data == 0) { // Free nav_image.at(height - 1 - y, x) = 255; // White } else if (data == 100) { // Occupied nav_image.at(height - 1 - y, x) = 0; // Black } else { // Unknown nav_image.at(height - 1 - y, x) = 128; // Gray } } } cv::imwrite(save_path + "/map.pgm", nav_image); /// yaml std::ofstream yamlFile(save_path + "/map.yaml"); if (!yamlFile.is_open()) { LOG(ERROR) << "failed to write map.yaml"; return; // 文件打开失败 } try { YAML::Emitter emitter; emitter << YAML::BeginMap; emitter << YAML::Key << "image" << YAML::Value << "map.pgm"; emitter << YAML::Key << "mode" << YAML::Value << "trinary"; emitter << YAML::Key << "width" << YAML::Value << map.info.width; emitter << YAML::Key << "height" << YAML::Value << map.info.height; emitter << YAML::Key << "resolution" << YAML::Value << float(0.05); std::vector orig{map.info.origin.position.x, map.info.origin.position.y, 0}; emitter << YAML::Key << "origin" << YAML::Value << orig; emitter << YAML::Key << "negate" << YAML::Value << 0; emitter << YAML::Key << "occupied_thresh" << YAML::Value << 0.65; emitter << YAML::Key << "free_thresh" << YAML::Value << 0.25; emitter << YAML::EndMap; yamlFile << emitter.c_str(); yamlFile.close(); } catch (...) { yamlFile.close(); return; } } LOG(INFO) << "map saved"; } void SlamSystem::ProcessIMU(const lightning::IMUPtr& imu) { if (running_ == false) { return; } lio_->ProcessIMU(imu); } void SlamSystem::ProcessLidar(const sensor_msgs::msg::PointCloud2::SharedPtr& cloud) { if (running_ == false) { return; } lio_->ProcessPointCloud2(cloud); lio_->Run(); auto kf = lio_->GetKeyframe(); if (kf != cur_kf_) { cur_kf_ = kf; } else { return; } if (cur_kf_ == nullptr) { return; } if (options_.with_loop_closing_) { lc_->AddKF(cur_kf_); } if (options_.with_gridmap_) { g2p5_->PushKeyframe(cur_kf_); } if (ui_) { ui_->UpdateKF(cur_kf_); } } void SlamSystem::ProcessLidar(const livox_ros_driver2::msg::CustomMsg::SharedPtr& cloud) { if (running_ == false) { return; } lio_->ProcessPointCloud2(cloud); lio_->Run(); auto kf = lio_->GetKeyframe(); if (kf != cur_kf_) { cur_kf_ = kf; } else { return; } if (cur_kf_ == nullptr) { return; } if (options_.with_loop_closing_) { lc_->AddKF(cur_kf_); } if (options_.with_gridmap_) { g2p5_->PushKeyframe(cur_kf_); } if (ui_) { ui_->UpdateKF(cur_kf_); } } void SlamSystem::Spin() { if (options_.online_mode_ && node_ != nullptr) { spin(node_); } } } // namespace lightning ================================================ FILE: src/core/system/slam.h ================================================ // // Created by xiang on 25-5-6. // #ifndef LIGHTNING_SLAM_H #define LIGHTNING_SLAM_H #include #include #include #include #include "lightning/srv/save_map.hpp" #include "livox_ros_driver2/msg/custom_msg.hpp" #include "common/eigen_types.h" #include "common/imu.h" #include "common/keyframe.h" namespace lightning { class LaserMapping; // lio 前端 class LoopClosing; // 回环检测 namespace ui { class PangolinWindow; } namespace g2p5 { class G2P5; } /** * SLAM 系统调用接口 */ class SlamSystem { public: struct Options { Options() {} bool online_mode_ = true; // 在线模式，在线模式下会起一些子线程来做异步处理 bool with_cc_ = true; // 是否需要带交叉验证 bool with_gridmap_ = true; // 是否需要2D栅格 bool with_loop_closing_ = true; // 是否需要回环检测 bool with_visualization_ = true; // 是否需要可视化UI bool with_2dvisualization_ = true; // 是否需要2D可视化UI bool step_on_kf_ = true; // 是否在关键帧处暂停p }; using SaveMapService = srv::SaveMap; SlamSystem(Options options); ~SlamSystem(); /// 初始化 bool Init(const std::string& yaml_path); /// 对外部交互接口 /// 开始建图，输入地图名称 void StartSLAM(std::string map_name); /// 保存地图，默认保存至./data/地图名/ 下方 void SaveMap(const std::string& path = ""); /// 处理IMU void ProcessIMU(const lightning::IMUPtr& imu); /// 处理点云 void ProcessLidar(const sensor_msgs::msg::PointCloud2::SharedPtr& cloud); void ProcessLidar(const livox_ros_driver2::msg::CustomMsg::SharedPtr& cloud); /// 实时模式下的spin void Spin(); private: /// ros端保存地图的实现 void SaveMap(const SaveMapService::Request::SharedPtr request, SaveMapService::Response::SharedPtr response); Options options_; std::atomic_bool running_ = false; rclcpp::Service::SharedPtr savemap_service_ = nullptr; std::string map_name_; // 地图名 std::shared_ptr lio_ = nullptr; // lio 前端 std::shared_ptr lc_ = nullptr; // 回环检测 std::shared_ptr ui_ = nullptr; // ui std::shared_ptr g2p5_ = nullptr; // 栅格地图 Keyframe::Ptr cur_kf_ = nullptr; /// 实时模式下的ros2 node, subscribers rclcpp::Node::SharedPtr node_; std::string imu_topic_; std::string cloud_topic_; std::string livox_topic_; rclcpp::Subscription::SharedPtr imu_sub_ = nullptr; rclcpp::Subscription::SharedPtr cloud_sub_ = nullptr; rclcpp::Subscription::SharedPtr livox_sub_ = nullptr; }; } // namespace lightning #endif // LIGHTNING_SLAM_H ================================================ FILE: src/io/dataset_type.h ================================================ // // Created by xiang on 25-3-24. // #ifndef LIGHTNING_DATASET_TYPE_H #define LIGHTNING_DATASET_TYPE_H #include namespace lightning { enum class DatasetType { UNKNOWN = -1, NCLT = 0, // NCLT: http://robots.engin.umich.edu/nclt/ KITTI = 1, // Kitti: ULHK = 3, // https://github.com/weisongwen/UrbanLoco UTBM = 4, // https://epan-utbm.github.io/utbm_robocar_dataset/ AVIA = 5, // https://epan-utbm.github.io/utbm_robocar_dataset/ WXB_3D, // 3d wxb }; inline DatasetType Str2DatasetType(const std::string& name) { if (name == "NCLT") { return DatasetType::NCLT; } if (name == "KITTI") { return DatasetType::KITTI; } if (name == "ULHK") { return DatasetType::ULHK; } if (name == "UTBM") { return DatasetType::UTBM; } if (name == "WXB3D") { return DatasetType::WXB_3D; } if (name == "AVIA") { return DatasetType::AVIA; } return DatasetType::UNKNOWN; } /// 各种数据集里用的topic名称 const std::string nclt_rtk_topic = "gps_rtk_fix"; const std::string nclt_lidar_topic = "points_raw"; const std::string ulhk_lidar_topic = "/velodyne_points_0"; const std::string wxb_lidar_topic = "/velodyne_packets_1"; const std::string utbm_lidar_topic = "/velodyne_points"; const std::string avia_lidar_topic = "/livox/lidar"; const std::string ulhk_imu_topic = "/imu/data"; const std::string utbm_imu_topic = "/imu/data"; const std::string nclt_imu_topic = "imu_raw"; const std::string wxb_imu_topic = "/ivsensorimu"; const std::string avia_imu_topic = "/livox/imu"; } // namespace lightning #endif // LIGHTNING_DATASET_TYPE_H ================================================ FILE: src/io/file_io.cc ================================================ // // Created by xiang on 25-3-12. // #include "io/file_io.h" #include namespace lightning { bool PathExists(const std::string& file_path) { std::filesystem::path path(file_path); return std::filesystem::exists(path); } bool RemoveIfExist(const std::string& path) { if (PathExists(path)) { // LOG(INFO) << "remove " << path; system(("rm -f " + path).c_str()); return true; } return false; } bool IsDirectory(const std::string& path) { return std::filesystem::is_directory(path); } } // namespace lightning ================================================ FILE: src/io/file_io.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_FILE_IO_H #define LIGHTNING_FILE_IO_H #include "common/eigen_types.h" #include "common/std_types.h" namespace lightning { /** * 检查某个路径是否存在 * @param file_path 路径名 * @return true if exist */ bool PathExists(const std::string& file_path); /** * 若文件存在，则删除之 * @param path * @return */ bool RemoveIfExist(const std::string& path); /** * 判断某路径是否为目录 * @param path * @return */ bool IsDirectory(const std::string& path); } #endif // LIGHTNING_FILE_IO_H ================================================ FILE: src/io/yaml_io.cc ================================================ // // Created by gaoxiang on 2020/5/13. // #include "io/yaml_io.h" #include #include namespace lightning { YAML_IO::YAML_IO(const std::string &path) { path_ = path; yaml_node_ = YAML::LoadFile(path_); if (yaml_node_.IsNull()) { LOG(ERROR) << "Failed to open yaml: " << path_; } is_opened_ = true; } bool YAML_IO::Save(const std::string &path) { if (path.empty()) { std::ofstream fout(path_); fout << yaml_node_; } else { std::ofstream fout(path); fout << yaml_node_; } return true; } } // namespace lightning ================================================ FILE: src/io/yaml_io.h ================================================ // // Created by gaoxiang on 2020/5/13. // #ifndef LIGHTNING_YAML_IO_H #define LIGHTNING_YAML_IO_H #include #include #include namespace lightning { /// 读取yaml配置文件的相关IO class YAML_IO { public: explicit YAML_IO(const std::string &path); YAML_IO() = default; ~YAML_IO() = default; inline bool IsOpened() const { return is_opened_; } /// 保存文件，不指明路径时，覆盖原文件 bool Save(const std::string &path = ""); /// 获取类型为T的参数值 template T GetValue(const std::string &key) const { assert(is_opened_); return yaml_node_[key].as(); } /// 获取在NODE下的key值 // 读取两层yaml参数 template T GetValue(const std::string &node, const std::string &key) const { assert(is_opened_); T res = yaml_node_[node][key].as(); return res; } // 读取三层yaml参数 template T GetValue(const std::string &node_1, const std::string &node_2, const std::string &key) const { assert(is_opened_); T res = yaml_node_[node_1][node_2][key].as(); return res; } /// 设定类型为T的参数值 template void SetValue(const std::string &key, const T &value) { yaml_node_[key] = value; } /// 设定NODE下的key值 template void SetValue(const std::string &node, const std::string &key, const T &value) { yaml_node_[node][key] = value; } private: std::string path_; bool is_opened_ = false; YAML::Node yaml_node_; }; } // namespace lightning #endif ================================================ FILE: src/ui/pangolin_window.cc ================================================ #include "ui/pangolin_window_impl.h" namespace lightning::ui { PangolinWindow::PangolinWindow() { impl_ = std::make_shared(); } PangolinWindow::~PangolinWindow() { Quit(); } bool PangolinWindow::Init() { impl_->cloud_global_need_update_.store(false); impl_->kf_result_need_update_.store(false); impl_->lidarloc_need_update_.store(false); impl_->current_scan_need_update_.store(false); bool inited = impl_->Init(); // 创建渲染线程 if (inited) { impl_->render_thread_ = std::thread([this]() { impl_->Render(); }); } return inited; } void PangolinWindow::Reset(const std::vector& keyframes) { impl_->Reset(keyframes); } void PangolinWindow::Quit() { if (impl_->render_thread_.joinable()) { impl_->exit_flag_.store(true); impl_->render_thread_.join(); } impl_->DeInit(); } void PangolinWindow::UpdatePointCloudGlobal(const std::map& cloud) { std::lock_guard lock(impl_->mtx_map_cloud_); impl_->cloud_global_map_ = cloud; impl_->cloud_global_need_update_.store(true); } void PangolinWindow::UpdatePointCloudDynamic(const std::map& cloud) { std::unique_lock lock(impl_->mtx_map_cloud_); impl_->cloud_dynamic_map_.clear(); // need deep copy for (auto& cp : cloud) { CloudPtr c(new PointCloudType()); *c = *cp.second; impl_->cloud_dynamic_map_.emplace(cp.first, c); } for (auto iter = impl_->cloud_dynamic_map_.begin(); iter != impl_->cloud_dynamic_map_.end();) { if (cloud.find(iter->first) == cloud.end()) { iter = impl_->cloud_dynamic_map_.erase(iter); } else { iter++; } } impl_->cloud_dynamic_need_update_.store(true); } void PangolinWindow::UpdateNavState(const NavState& state) { std::unique_lock lock_lio_res(impl_->mtx_nav_state_); impl_->pose_ = state.GetPose(); impl_->vel_ = state.GetVel(); impl_->bias_acc_ = state.Getba(); impl_->bias_gyr_ = state.Getbg(); impl_->confidence_ = state.confidence_; impl_->kf_result_need_update_.store(true); } void PangolinWindow::UpdateRecentPose(const SE3& pose) { std::lock_guard lock(impl_->mtx_nav_state_); impl_->newest_frontend_pose_ = pose; } void PangolinWindow::UpdatePredictPose(const SE3& pose) { UL lock(impl_->mtx_nav_state_); impl_->predicted_pose_ = pose; } void PangolinWindow::UpdateScan(CloudPtr cloud, const SE3& pose) { std::lock_guard lock(impl_->mtx_current_scan_); std::lock_guard lock2(impl_->mtx_nav_state_); *impl_->current_scan_ = *cloud; // need deep copy impl_->current_scan_pose_ = pose; impl_->current_scan_need_update_.store(true); } void PangolinWindow::UpdateKF(std::shared_ptr kf) { UL lock(impl_->mtx_current_scan_); impl_->all_keyframes_.emplace_back(kf); } void PangolinWindow::SetCurrentScanSize(int current_scan_size) { impl_->max_size_of_current_scan_ = current_scan_size; } void PangolinWindow::SetTImuLidar(const SE3& T_imu_lidar) { impl_->T_imu_lidar_ = T_imu_lidar; } bool PangolinWindow::ShouldQuit() { return pangolin::ShouldQuit(); } } // namespace lightning::ui ================================================ FILE: src/ui/pangolin_window.h ================================================ #pragma once #include #include #include #include "common/eigen_types.h" #include "common/keyframe.h" #include "common/loop_candidate.h" #include "common/nav_state.h" #include "common/point_def.h" namespace lightning::ui { class PangolinWindowImpl; /** * @note 此类本身不直接涉及任何opengl和pangolin操作，应当放到PangolinWindowImpl中 */ class PangolinWindow { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW PangolinWindow(); ~PangolinWindow(); /// @brief 初始化窗口，后台启动render线程。 /// @note 与opengl/pangolin无关的初始化，尽量放到此函数体中; /// opengl/pangolin相关的内容，尽量放到PangolinWindowImpl::Init中。 bool Init(); void Reset(const std::vector& keyframes); /// 更新激光地图点云，在激光定位中的地图发生改变时，由fusion调用 void UpdatePointCloudGlobal(const std::map& cloud); /// 更新动态图层点云 void UpdatePointCloudDynamic(const std::map& cloud); /// 更新kalman滤波器状态 void UpdateNavState(const NavState& state); /// 更新最新的pose void UpdateRecentPose(const SE3& pose); void UpdatePredictPose(const SE3& pose); /// 更新一次scan和它对应的Pose void UpdateScan(CloudPtr cloud, const SE3& pose); void UpdateKF(std::shared_ptr kf); /// 等待显示线程结束，并释放资源 void Quit(); /// 用户是否已经退出UI bool ShouldQuit(); /// 设置IMU到雷达的外参 void SetTImuLidar(const SE3& T_imu_lidar); /// 设置需要保留多少个扫描数据 void SetCurrentScanSize(int current_scan_size); private: std::shared_ptr impl_ = nullptr; }; } // namespace lightning::ui ================================================ FILE: src/ui/pangolin_window_impl.cc ================================================ #include #include #include #include #include #include "common/options.h" #include "common/std_types.h" #include "core/lightning_math.hpp" #include "ui/pangolin_window_impl.h" namespace lightning::ui { bool PangolinWindowImpl::Init() { // create a window and bind its context to the main thread pangolin::CreateWindowAndBind(win_name_, win_width_, win_height_); // 3D mouse handler requires depth testing to be enabled glEnable(GL_DEPTH_TEST); // opengl buffer AllocateBuffer(); // unset the current context from the main thread pangolin::GetBoundWindow()->RemoveCurrent(); // 雷达定位轨迹opengl设置 traj_newest_state_.reset(new ui::UiTrajectory(Vec3f(1.0, 0.0, 0.0))); // 红色 traj_scans_.reset(new ui::UiTrajectory(Vec3f(0.0, 1.0, 0.0))); // 绿色 current_scan_.reset(new PointCloudType); // 重置pcl点云指针 current_scan_ui_.reset(new ui::UiCloud); // 重置用于渲染的点云指针 /// data log log_vel_.SetLabels(std::vector{"vel_x", "vel_y", "vel_z"}); log_vel_baselink_.SetLabels(std::vector{"baselink_vel_x", "baselink_vel_y", "baselink_vel_z"}); log_bias_acc_.SetLabels(std::vector{"ba_x", "ba_y", "ba_z"}); log_confidence_.SetLabels(std::vector{"lidar loc confidence"}); log_error_.SetLabels(std::vector{"err v", "err h", "err eval v", "err eval h"}); return true; } void PangolinWindowImpl::Reset(const std::vector &keyframes) { UL lock(mtx_reset_); cloud_map_ui_.clear(); scans_.clear(); current_scan_ui_ = nullptr; traj_scans_->Clear(); for (const auto &keyframe : keyframes) { traj_scans_->AddPt(keyframe->GetOptPose()); } std::size_t i = keyframes.size() > max_size_of_current_scan_ ? keyframes.size() - max_size_of_current_scan_ : 0; for (; i < keyframes.size(); ++i) { const auto &keyframe = keyframes.at(i); current_scan_ui_ = std::make_shared(); CloudPtr tmp_cloud = std::make_shared(*(keyframe->GetCloud())); current_scan_ui_->SetCloud(math::VoxelGrid(tmp_cloud, 0.5), keyframe->GetOptPose()); current_scan_ui_->SetRenderColor(ui::UiCloud::UseColor::HEIGHT_COLOR); scans_.emplace_back(current_scan_ui_); } newest_backend_pose_ = keyframes.back()->GetOptPose(); } bool PangolinWindowImpl::DeInit() { ReleaseBuffer(); return true; } bool PangolinWindowImpl::UpdateGlobalMap() { if (!cloud_global_need_update_.load()) { return false; } std::lock_guard lock(mtx_map_cloud_); for (const auto &cp : cloud_global_map_) { if (cloud_map_ui_.find(cp.first) != cloud_map_ui_.end()) { continue; } std::shared_ptr ui_cloud(new ui::UiCloud); ui_cloud->SetCloud(cp.second, SE3()); ui_cloud->SetRenderColor(ui::UiCloud::UseColor::GRAY_COLOR); cloud_map_ui_.emplace(cp.first, ui_cloud); } for (auto iter = cloud_map_ui_.begin(); iter != cloud_map_ui_.end();) { if (cloud_global_map_.find(iter->first) == cloud_global_map_.end()) { iter = cloud_map_ui_.erase(iter); } else { iter++; } } cloud_global_need_update_.store(false); return true; } bool PangolinWindowImpl::UpdateDynamicMap() { if (!cloud_dynamic_need_update_.load()) { return false; } std::lock_guard lock(mtx_map_cloud_); for (const auto &cp : cloud_dynamic_map_) { auto it = cloud_dyn_ui_.find(cp.first); if (it != cloud_dyn_ui_.end()) { // 存在也要更新 it->second.reset(new ui::UiCloud); // it->second->SetRenderColor(ui::UiCloud::UseColor::PCL_COLOR); it->second->SetCustomColor(Vec4f(0.0, 0.2, 1.0, 1.0)); it->second->SetCloud(cp.second, SE3()); it->second->SetRenderColor(ui::UiCloud::UseColor::CUSTOM_COLOR); continue; } /// 不存在则创建一个 std::shared_ptr ui_cloud(new ui::UiCloud); ui_cloud->SetCustomColor(Vec4f(0.0, 0.2, 1.0, 1.0)); ui_cloud->SetCloud(cp.second, SE3()); ui_cloud->SetRenderColor(ui::UiCloud::UseColor::CUSTOM_COLOR); // ui_cloud->SetRenderColor(ui::UiCloud::UseColor::PCL_COLOR); cloud_dyn_ui_.emplace(cp.first, ui_cloud); } for (auto iter = cloud_dyn_ui_.begin(); iter != cloud_dyn_ui_.end();) { if (cloud_dynamic_map_.find(iter->first) == cloud_dynamic_map_.end()) { iter = cloud_dyn_ui_.erase(iter); } else { iter++; }; } cloud_dynamic_need_update_.store(false); return true; } bool PangolinWindowImpl::UpdateCurrentScan() { UL lock(mtx_current_scan_); if (current_scan_ != nullptr && !current_scan_->empty() && current_scan_need_update_) { if (current_scan_ui_) { current_scan_ui_->SetRenderColor(ui::UiCloud::UseColor::HEIGHT_COLOR); scans_.emplace_back(current_scan_ui_); } current_scan_ui_ = std::make_shared(); current_scan_ui_->SetCloud(current_scan_, current_scan_pose_); // current_scan_ui_->SetRenderColor(ui::UiCloud::UseColor::CUSTOM_COLOR); current_scan_ui_->SetRenderColor(ui::UiCloud::UseColor::HEIGHT_COLOR); // current_scan_ui_->SetCustomColor(Vec4f(1.0, 1.0, 1.0, 1.0)); // current_scan_ui_->SetPointSize(2.0); current_scan_need_update_.store(false); traj_scans_->AddPt(current_scan_pose_); newest_backend_pose_ = current_scan_pose_; } while (scans_.size() >= max_size_of_current_scan_) { scans_.pop_front(); } return true; } bool PangolinWindowImpl::UpdateState() { if (!kf_result_need_update_.load()) { return false; } std::lock_guard lock(mtx_nav_state_); Vec3d pos = pose_.translation().eval(); Vec3d vel_baselink = pose_.so3().inverse() * vel_; double roll = pose_.angleX(); double pitch = pose_.angleY(); double yaw = pose_.angleZ(); // 滤波器状态作曲线图 log_vel_.Log(vel_(0), vel_(1), vel_(2)); log_vel_baselink_.Log(vel_baselink(0), vel_baselink(1), vel_baselink(2)); log_bias_acc_.Log(bias_acc_(0), bias_acc_(1), bias_acc_(2)); log_confidence_.Log(confidence_); newest_frontend_pose_ = pose_; traj_newest_state_->AddPt(newest_frontend_pose_); std::ostringstream ss; ss << std::fixed << std::setprecision(4) << "ba: [" << bias_acc_(0) << ", " << bias_acc_(1) << ", " << bias_acc_(2) << "]"; gltext_label_state_ = pangolin::default_font().Text(ss.str()); kf_result_need_update_.store(false); return false; } void PangolinWindowImpl::DrawAll() { /// 地图 for (const auto &pc : cloud_map_ui_) { pc.second->Render(); } /// 动态地图 for (const auto &pc : cloud_dyn_ui_) { pc.second->Render(); } /// 缓存的scans for (const auto &s : scans_) { s->Render(); } current_scan_ui_->Render(); if (draw_frontend_traj_) { traj_newest_state_->Render(); // 车 frontend_car_.SetPose(newest_frontend_pose_); // 车在current pose上 frontend_car_.Render(); } if (draw_backend_traj_) { traj_scans_->Render(); // 车 backend_car_.SetPose(newest_backend_pose_); backend_car_.Render(); } // pred_car_.SetPose(predicted_pose_); // pred_car_.Render(); // 关键帧 { UL lock(mtx_current_scan_); if (all_keyframes_.size() > 1) { /// 闭环后的轨迹 glLineWidth(5.0); glBegin(GL_LINE_STRIP); glColor3f(0.5, 0.0, 0.5); for (int i = 0; i < all_keyframes_.size() - 1; ++i) { auto p1 = all_keyframes_[i]->GetOptPose().translation(); auto p2 = all_keyframes_[i + 1]->GetOptPose().translation(); glVertex3f(p1[0], p1[1], p1[2]); glVertex3f(p2[0], p2[1], p2[2]); } glEnd(); } } // 文字 RenderLabels(); } void PangolinWindowImpl::RenderClouds() { UL lock(mtx_reset_); // 更新各种推送过来的状态 UpdateGlobalMap(); UpdateDynamicMap(); UpdateState(); UpdateCurrentScan(); // 绘制 pangolin::Display(dis_3d_main_name_).Activate(s_cam_main_); DrawAll(); } void PangolinWindowImpl::RenderLabels() { // 定位状态标识，显示在3D窗口中 auto &d_cam3d_main = pangolin::Display(dis_3d_main_name_); d_cam3d_main.Activate(s_cam_main_); const auto cur_width = d_cam3d_main.v.w; const auto cur_height = d_cam3d_main.v.h; GLint view[4]; glGetIntegerv(GL_VIEWPORT, view); glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glOrtho(0, cur_width, 0, cur_height, -1, 1); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); glTranslatef(5, cur_height - 1.5 * gltext_label_global_.Height(), 1.0); glColor3ub(127, 127, 127); gltext_label_global_.Draw(); glTranslatef(0.0f, -1.5f * gltext_label_global_.Height(), 0.0f); glColor3ub(180, 220, 180); gltext_label_state_.Draw(); // Restore modelview / project matrices glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); glPopMatrix(); } void PangolinWindowImpl::CreateDisplayLayout() { // define camera render object (for view / scene browsing) // 定义视点的透视投影方式 auto proj_mat_main = pangolin::ProjectionMatrix(win_width_, win_width_, cam_focus_, cam_focus_, win_width_ / 2, win_width_ / 2, cam_z_near_, cam_z_far_); // 模型视图矩阵定义了视点的位置和朝向 auto model_view_main = pangolin::ModelViewLookAt(0, 0, 100, 0, 0, 0, pangolin::AxisY); s_cam_main_ = pangolin::OpenGlRenderState(std::move(proj_mat_main), std::move(model_view_main)); // Add named OpenGL viewport to window and provide 3D Handler pangolin::View &d_cam3d_main = pangolin::Display(dis_3d_main_name_) .SetBounds(0.0, 1.0, 0.0, 1.0) .SetHandler(new pangolin::Handler3D(s_cam_main_)); pangolin::View &d_cam3d = pangolin::Display(dis_3d_name_) .SetBounds(0.0, 1.0, 0.0, 0.75) .SetLayout(pangolin::LayoutOverlay) .AddDisplay(d_cam3d_main); // OpenGL 'view' of data. We might have many views of the same data. plotter_vel_ = std::make_unique(&log_vel_, -10, 600, -11, 11, 75, 2); plotter_vel_->SetBounds(0.02, 0.98, 0.0, 1.0); plotter_vel_->Track("$i"); plotter_vel_baselink_ = std::make_unique(&log_vel_baselink_, -10, 600, -11, 11, 75, 2); plotter_vel_baselink_->SetBounds(0.02, 0.98, 0.0, 1.0); plotter_vel_baselink_->Track("$i"); plotter_bias_acc_ = std::make_unique(&log_bias_acc_, -10, 600, -1, 1, 75, 0.1); plotter_bias_acc_->SetBounds(0.02, 0.98, 0.0, 1.0); plotter_bias_acc_->Track("$i"); plotter_confidence_ = std::make_unique(&log_confidence_, -10, 600, 0, 5.0, 100, 0.5); plotter_confidence_->SetBounds(0.02, 0.98, 0.0, 1.0); plotter_confidence_->Track("$i"); plotter_err_ = std::make_unique(&log_error_, -10, 600, 0, 1.0, 100, 0.1); plotter_err_->SetBounds(0.02, 0.98, 0.0, 1.0); plotter_err_->Track("$i"); pangolin::View &d_plot = pangolin::Display(dis_plot_name_) .SetBounds(0.0, 1.0, 0.75, 1.0) .SetLayout(pangolin::LayoutEqualVertical) .AddDisplay(*plotter_confidence_) .AddDisplay(*plotter_err_) .AddDisplay(*plotter_bias_acc_) .AddDisplay(*plotter_vel_) .AddDisplay(*plotter_vel_baselink_); pangolin::Display(dis_main_name_) .SetBounds(0.0, 1.0, pangolin::Attach::Pix(menu_width_), 1.0) .AddDisplay(d_cam3d) .AddDisplay(d_plot); } void PangolinWindowImpl::Render() { pangolin::BindToContext(win_name_); // Issue specific OpenGl we might need // 启用OpenGL深度测试和混合功能，以支持透明度等效果。 glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // menu pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(menu_width_)); pangolin::Var menu_follow_loc("menu.Follow", false, true); // 跟踪实时定位 pangolin::Var menu_draw_frontend_traj("menu.Draw Frontend Traj", true, true); // 前端实时轨迹 pangolin::Var menu_draw_backend_traj("menu.Draw Backend Traj", true, true); // 后端实时轨迹 pangolin::Var menu_reset_3d_view("menu.Reset 3D View", false, false); // 重置俯视视角 pangolin::Var menu_reset_front_view("menu.Set to front View", false, false); // 前视视角 pangolin::Var menu_step("menu.Step", false, false); // 单步调试 pangolin::Var menu_play_speed("menu.Play speed", 10.0, 0.1, 10.0); // 运行速度 pangolin::Var menu_intensity("menu.intensity", 0.5, 0.0, 1.0); // 亮度 // display layout CreateDisplayLayout(); exit_flag_.store(false); while (!pangolin::ShouldQuit() && !exit_flag_) { // Clear entire screen glClearColor(20.0 / 255.0, 20.0 / 255.0, 20.0 / 255.0, 1.0); // 清除了颜色缓冲区（GL_COLOR_BUFFER_BIT）和深度缓冲区（GL_DEPTH_BUFFER_BIT）。 // 通常在每一帧渲染之前执行的操作，以准备渲染新的内容。 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // menu control following_loc_ = menu_follow_loc; draw_frontend_traj_ = menu_draw_frontend_traj; draw_backend_traj_ = menu_draw_backend_traj; if (menu_reset_3d_view) { s_cam_main_.SetModelViewMatrix(pangolin::ModelViewLookAt(0, 0, 1000, 0, 0, 0, pangolin::AxisY)); menu_reset_3d_view = false; } if (menu_reset_front_view) { s_cam_main_.SetModelViewMatrix(pangolin::ModelViewLookAt(-50, 0, 10, 50, 0, 10, pangolin::AxisZ)); menu_reset_front_view = false; } if (menu_step) { debug::flg_next = true; } else { debug::flg_next = false; } debug::play_speed = menu_play_speed; ui::opacity = menu_intensity; // Render pointcloud RenderClouds(); /// 处理相机跟随问题 if (following_loc_) { Eigen::Vector3d translation = newest_frontend_pose_.translation(); Sophus::SE3d newest_frontend_pose_new(Eigen::Quaterniond::Identity(), Eigen::Vector3d(translation.x(), translation.y(), 0.0)); s_cam_main_.Follow(newest_frontend_pose_new.matrix()); } // Swap frames and Process Events // 完成当前帧的渲染并处理与窗口交互相关的事件 pangolin::FinishFrame(); std::this_thread::sleep_for(std::chrono::milliseconds(5)); } // unset the current context from the main thread pangolin::GetBoundWindow()->RemoveCurrent(); pangolin::DestroyWindow(GetWindowName()); } std::string PangolinWindowImpl::GetWindowName() const { return win_name_; } void PangolinWindowImpl::AllocateBuffer() { std::string global_text( "Welcome to SAD.UI. Open source code: https://github.com/gaoxiang12/slam_in_autonomous_driving. All right " "reserved.\n" "Red: newest IMU pose, yellow: lidar scan pose"); auto &font = pangolin::default_font(); gltext_label_global_ = font.Text(global_text); gltext_label_state_ = font.Text("ba: [0.0000, 0.0000, 0.0000]"); } void PangolinWindowImpl::ReleaseBuffer() {} } // namespace lightning::ui ================================================ FILE: src/ui/pangolin_window_impl.h ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include #include "common/keyframe.h" #include "common/loop_candidate.h" #include "ui/pangolin_window.h" #include "ui/ui_car.h" #include "ui/ui_cloud.h" #include "ui/ui_trajectory.h" namespace lightning::ui { /** */ class PangolinWindowImpl { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW PangolinWindowImpl() = default; ~PangolinWindowImpl() = default; PangolinWindowImpl(const PangolinWindowImpl &) = delete; PangolinWindowImpl &operator=(const PangolinWindowImpl &) = delete; PangolinWindowImpl(PangolinWindowImpl &&) = delete; PangolinWindowImpl &operator=(PangolinWindowImpl &&) = delete; /// 初始化，创建可用于渲染的点云和轨迹 bool Init(); void Reset(const std::vector &keyframes); /// 注销 bool DeInit(); /// 渲染所有信息 void Render(); /// 获取窗口名称 std::string GetWindowName() const; public: /// 后台渲染线程 std::thread render_thread_; /// 一些辅助的锁和原子变量 std::mutex mtx_map_cloud_; std::mutex mtx_current_scan_; std::mutex mtx_nav_state_; std::mutex mtx_gps_pose_; std::mutex mtx_loop_info_; std::mutex mtx_reset_; std::atomic exit_flag_; std::atomic cloud_global_need_update_; // 全局点云是否需要更新 std::atomic cloud_dynamic_need_update_; // 动态点云是否需要更新 std::atomic kf_result_need_update_; // 卡尔曼滤波结果 std::atomic current_scan_need_update_; // 更新当前扫描 std::atomic lidarloc_need_update_; // 雷达位置？ pcl::PointCloud::Ptr current_scan_ = nullptr; // 当前scan SE3 newest_frontend_pose_; // 最新pose SE3 predicted_pose_; SE3 newest_backend_pose_; // 最新pose SE3 current_scan_pose_; // 当前scan对应的pose or Twb/Twi std::deque> loop_info_; std::vector new_loop_candidate_; // 地图点云 std::map cloud_global_map_; std::map cloud_dynamic_map_; /// 滤波器状态 Sophus::SE3d pose_; double confidence_; Vec3d vel_; Vec3d bias_acc_; Vec3d bias_gyr_; Vec3d grav_; Sophus::SE3d T_imu_lidar_; int max_size_of_current_scan_ = 200; // 当前扫描数据保留多少个 std::vector> all_keyframes_; //////////////////////////////// 以下和render相关 /////////////////////////// private: /// 创建OpenGL Buffers void AllocateBuffer(); void ReleaseBuffer(); void CreateDisplayLayout(); void DrawAll(); // 作图：画定位窗口 /// 渲染点云，调用各种Update函数 void RenderClouds(); bool UpdateGlobalMap(); bool UpdateDynamicMap(); bool UpdateState(); bool UpdateCurrentScan(); void RenderLabels(); private: /// 窗口layout相关 int win_width_ = 1920; int win_height_ = 1080; static constexpr float cam_focus_ = 5000; static constexpr float cam_z_near_ = 1.0; static constexpr float cam_z_far_ = 1e10; static constexpr int menu_width_ = 210; const std::string win_name_ = "UI"; const std::string dis_main_name_ = "main"; const std::string dis_3d_name_ = "Cam 3D"; const std::string dis_3d_main_name_ = "Cam 3D Main"; // main const std::string dis_plot_name_ = "Plot"; const std::string dis_imgs_name = "Images"; bool following_loc_ = true; // 相机是否追踪定位结果 bool draw_frontend_traj_ = true; // 可视化前端轨迹 bool draw_backend_traj_ = true; // 可视化后端轨迹 // text pangolin::GlText gltext_label_global_; pangolin::GlText gltext_label_state_; // camera pangolin::OpenGlRenderState s_cam_main_; /// cloud rendering ui::UiCar backend_car_{Vec3f(0.2, 0.2, 0.8)}; // 白色车 ui::UiCar frontend_car_{Vec3f(0.2, 0.2, 0.8)}; // 白色车 ui::UiCar pred_car_{Vec3f(0.8, 0.5, 0.8)}; // 白色车 std::map> cloud_map_ui_; // 用来渲染的点云地图 std::map> cloud_dyn_ui_; // 用来渲染的点云地图 std::shared_ptr current_scan_ui_; // current scan std::deque> scans_; // current scan 保留的队列 std::deque> loop_info_ui_; // trajectory std::shared_ptr traj_scans_ = nullptr; // 激光扫描的轨迹 std::shared_ptr traj_newest_state_ = nullptr; // 最新state的轨迹 // 滤波器状态相关 Data logger object pangolin::DataLog log_vel_; // odom frame下的速度 pangolin::DataLog log_vel_baselink_; // baselink frame下的速度 pangolin::DataLog log_bias_acc_; // accelerometer bias pangolin::DataLog log_confidence_; // confidence pangolin::DataLog log_error_; // 误差 std::unique_ptr plotter_vel_ = nullptr; std::unique_ptr plotter_vel_baselink_ = nullptr; std::unique_ptr plotter_bias_acc_ = nullptr; std::unique_ptr plotter_confidence_ = nullptr; std::unique_ptr plotter_err_ = nullptr; std::unique_ptr plotter_err_eval_ = nullptr; }; } // namespace lightning::ui ================================================ FILE: src/ui/ui_car.cc ================================================ #include "ui/ui_car.h" #include namespace lightning::ui { std::vector UiCar::car_vertices_ = { // clang-format off { 0, 0, 0}, { 3.0, 0, 0}, { 0, 0, 0}, { 0, 3.0, 0}, { 0, 0, 0}, { 0, 0, 3.0}, // clang-format on }; void UiCar::SetPose(const SE3& pose) { pts_.clear(); for (auto& p : car_vertices_) { pts_.emplace_back(p); } // 转换到世界系 auto pose_f = pose.cast(); for (auto& pt : pts_) { pt = pose_f * pt; } } void UiCar::Render() { glLineWidth(5.0); glBegin(GL_LINES); /// x -红, y-绿 z-蓝 glColor3f(color_[0], color_[1], color_[2]); glVertex3f(pts_[0][0], pts_[0][1], pts_[0][2]); glVertex3f(pts_[1][0], pts_[1][1], pts_[1][2]); // glColor3f(0.0, 1.0, 0.0); glVertex3f(pts_[2][0], pts_[2][1], pts_[2][2]); glVertex3f(pts_[3][0], pts_[3][1], pts_[3][2]); // glColor3f(0.0, 0.0, 1.0); glVertex3f(pts_[4][0], pts_[4][1], pts_[4][2]); glVertex3f(pts_[5][0], pts_[5][1], pts_[5][2]); glEnd(); } } // namespace lightning::ui ================================================ FILE: src/ui/ui_car.h ================================================ #pragma once #include "common/eigen_types.h" namespace lightning::ui { /// 在UI里显示的小车 class UiCar { public: UiCar(const Vec3f& color) : color_(color) {} /// 设置小车 Pose，重设显存中的点 void SetPose(const SE3& pose); /// 渲染小车 void Render(); private: Vec3f color_; std::vector pts_; static std::vector car_vertices_; // 小车的顶点 }; } // namespace lightning::ui ================================================ FILE: src/ui/ui_cloud.cc ================================================ #include "ui/ui_cloud.h" #include "common/options.h" #include #include namespace lightning::ui { std::vector UiCloud::intensity_color_table_pcl_; UiCloud::UiCloud(CloudPtr cloud) { SetCloud(cloud, SE3()); } void UiCloud::SetCustomColor(Vec4f custom_color) { custom_color_ = custom_color; } // 把输入的点云映射为opengl可以渲染的点云 void UiCloud::SetCloud(CloudPtr cloud, const SE3& pose) { if (intensity_color_table_pcl_.empty()) { BuildIntensityTable(); } // assert(cloud != nullptr && cloud->empty() == false); xyz_data_.resize(cloud->size()); color_data_pcl_.resize(cloud->size()); color_data_intensity_.resize(cloud->size()); color_data_height_.resize(cloud->size()); color_data_gray_.resize(cloud->size()); std::vector idx(cloud->size()); std::iota(idx.begin(), idx.end(), 0); // 使用从0开始递增的整数填充idx SE3f pose_l = (pose).cast(); // 遍历所有点 for (auto iter = idx.begin(); iter != idx.end(); iter++) { const int& id = *iter; const auto& pt = cloud->points[id]; // 计算点的世界坐标 auto pt_world = pose_l * cloud->points[id].getVector3fMap(); xyz_data_[id] = Vec3f(pt_world.x(), pt_world.y(), pt_world.z()); // 把intensity映射为颜色 color_data_pcl_[id] = IntensityToRgbPCL(pt.intensity); color_data_gray_[id] = Vec4f(0.5, 0.5, 0.5, 1.0); // 根据高度映射颜色 color_data_height_[id] = IntensityToRgbPCL(pt.z * 10); color_data_intensity_[id] = Vec4f(pt.intensity / 255.0 * 3.0, pt.intensity / 255.0 * 3.0, pt.intensity / 255.0 * 3.0, 1.0); } } void UiCloud::Render() { // glPointSize(2.0); glBegin(GL_POINTS); glPointSize(point_size_); for (int i = 0; i < xyz_data_.size(); ++i) { if (use_color_ == UseColor::PCL_COLOR) { glColor4f(color_data_pcl_[i][0], color_data_pcl_[i][1], color_data_pcl_[i][2], ui::opacity); } else if (use_color_ == UseColor::INTENSITY_COLOR) { glColor4f(color_data_intensity_[i][0], color_data_intensity_[i][1], color_data_intensity_[i][2], ui::opacity); } else if (use_color_ == UseColor::HEIGHT_COLOR) { glColor4f(color_data_height_[i][0], color_data_height_[i][1], color_data_height_[i][2], ui::opacity); } else if (use_color_ == UseColor::GRAY_COLOR) { glColor4f(color_data_gray_[i][0], color_data_gray_[i][1], color_data_gray_[i][2], ui::opacity); } else if (use_color_ = UseColor::CUSTOM_COLOR) { glColor4f(custom_color_[0], custom_color_[1], custom_color_[2], ui::opacity); } glVertex3f(xyz_data_[i][0], xyz_data_[i][1], xyz_data_[i][2]); } glEnd(); } void UiCloud::BuildIntensityTable() { intensity_color_table_pcl_.reserve(255 * 3); // 接受rgb三个值，将它们归一化到范围 [0, 1]，同时设置 alpha 通道为0.2。 auto make_color = [](int r, int g, int b) -> Vec4f { return Vec4f(r / 255.0f, g / 255.0f, b / 255.0f, 0.2f); }; for (int i = 0; i < 256; i++) { intensity_color_table_pcl_.emplace_back(make_color(255, i, 0)); } for (int i = 0; i < 256; i++) { intensity_color_table_pcl_.emplace_back(make_color(i, 0, 255)); } for (int i = 0; i < 256; i++) { intensity_color_table_pcl_.emplace_back(make_color(0, 255, i)); } } void UiCloud::SetRenderColor(UiCloud::UseColor use_color) { use_color_ = use_color; } } // namespace lightning::ui ================================================ FILE: src/ui/ui_cloud.h ================================================ #pragma once #include "common/eigen_types.h" #include "common/point_def.h" namespace lightning::ui { /// 在UI中使用的点云 /// 固定不变的点云都可以用这个来渲染 class UiCloud { public: /// 使用哪种颜色渲染本点云 enum UseColor { PCL_COLOR, // PCL颜色，偏红 INTENSITY_COLOR, // 亮度 HEIGHT_COLOR, // 高度 GRAY_COLOR, // 显示为灰色 CUSTOM_COLOR, // 显示为自定颜色 }; UiCloud() {} UiCloud(CloudPtr cloud); /** * 从PCL点云来设置一个UI点云 * @param cloud PCL 点云: lP, coordinates are in Lidar frame * @param pose 点云位姿: Twi， */ void SetCloud(CloudPtr cloud, const SE3& pose); /// 渲染这个点云 void Render(); /// 指定内置颜色 void SetRenderColor(UseColor use_color); /// 指定自选颜色，RGBA void SetCustomColor(Vec4f custom_color); void SetPointSize(float point_size) { point_size_ = point_size; } private: Vec4f IntensityToRgbPCL(const float& intensity) const { int index = int(intensity * 3); index = index % intensity_color_table_pcl_.size(); return intensity_color_table_pcl_[index]; } UseColor use_color_ = UseColor::PCL_COLOR; Vec4f custom_color_ = Vec4f::Zero(); float point_size_ = 1.0f; std::vector xyz_data_; // 点的世界坐标 std::vector color_data_pcl_; // 根据intensity映射得到的颜色 std::vector color_data_intensity_; // 点的intensity不映射直接做颜色 std::vector color_data_height_; // 根据height映射得到的颜色 std::vector color_data_gray_; // 全部点都为灰色 /// PCL中intensity table void BuildIntensityTable(); // 颜色映射表 static std::vector intensity_color_table_pcl_; }; } // namespace lightning::ui ================================================ FILE: src/ui/ui_trajectory.cc ================================================ #include "ui/ui_trajectory.h" #include namespace lightning::ui { void UiTrajectory::AddPt(const SE3& pose) { // 如果轨迹点超出阈值 直接删除前一半点 pos_.emplace_back(pose.translation().cast()); if (pos_.size() > max_size_) { pos_.erase(pos_.begin(), pos_.begin() + pos_.size() / 2); } } void UiTrajectory::Render() { // 点线形式 glLineWidth(5.0); glBegin(GL_LINE_STRIP); glColor3f(color_[0], color_[1], color_[2]); for (const auto& p : pos_) { glVertex3f(p[0], p[1], p[2]); } glEnd(); } } // namespace lightning::ui ================================================ FILE: src/ui/ui_trajectory.h ================================================ #pragma once #include "common/eigen_types.h" #include namespace lightning::ui { /// UI中的轨迹绘制 class UiTrajectory { public: UiTrajectory(const Vec3f& color) : color_(color) { pos_.reserve(max_size_); } /// 增加一个轨迹点到opengl缓冲区 void AddPt(const SE3& pose); /// 渲染此轨迹 void Render(); void Clear() { pos_.clear(); pos_.reserve(max_size_); vbo_.Free(); } Vec3f At(const uint64_t idx) const { return pos_.at(idx); } private: int max_size_ = 1e6; // 记录的最大点数 std::vector pos_; // 轨迹记录数据 Eigen::Vector3f color_ = Eigen::Vector3f::Zero(); // 轨迹颜色显示 pangolin::GlBuffer vbo_; // 显存顶点信息 }; } // namespace lightning::ui ================================================ FILE: src/utils/async_message_process.h ================================================ // // Created by xiang on 2022/2/9. // #pragma once #include #include #include #include #include #include #include #include "common/eigen_types.h" #include "common/std_types.h" namespace lightning { /** * 异步消息处理类 * 内部有线程和队列机制，保证回调是串行的 * 主要用于分离各模块线程，避免写一堆锁或者条件变量 * * @tparam T * * NOTE skip设为1的时候实际不会跳帧。。设为2的时候实际走一帧跳一帧 */ template class AsyncMessageProcess { public: using ProcFunc = std::function; // 消息回调函数 AsyncMessageProcess() = default; AsyncMessageProcess(ProcFunc proc_func, std::string name = ""); ~AsyncMessageProcess() { Quit(); } /// 设置处理函数 void SetProcFunc(ProcFunc proc_func) { custom_func_ = proc_func; } /// 设置队列最大长度 void SetMaxSize(size_t size) { max_size_ = size; } /// 开始处理消息 void Start(); /// 添加一条消息 void AddMessage(const T& msg); /// 退出 void Quit(); /// 清空跳帧计数器，下一个数据会立即执行 void CleanSkipCnt(); void SetName(std::string name) { name_ = std::move(name); } void SetSkipParam(bool enable_skip, int skip_num) { enable_skip_ = enable_skip, skip_num_ = skip_num; } AsyncMessageProcess(const AsyncMessageProcess&) = delete; void operator=(const AsyncMessageProcess&) = delete; private: void ProcLoop(); std::thread proc_; std::mutex mutex_; std::condition_variable cv_msg_; std::deque msg_buffer_; bool update_flag_ = false; bool exit_flag_ = false; size_t max_size_ = 40; std::string name_; /// 跳帧 bool enable_skip_ = false; int skip_num_ = 0; int skip_cnt_ = 0; ProcFunc custom_func_; }; template void AsyncMessageProcess::CleanSkipCnt() { UL lock(mutex_); skip_cnt_ = 0; } template AsyncMessageProcess::AsyncMessageProcess(AsyncMessageProcess::ProcFunc proc_func, std::string name) { custom_func_ = std::move(proc_func); name_ = name; } template void AsyncMessageProcess::Start() { exit_flag_ = false; update_flag_ = false; proc_ = std::thread([this]() { ProcLoop(); }); } template void AsyncMessageProcess::ProcLoop() { while (!exit_flag_) { UL lock(mutex_); cv_msg_.wait(lock, [this]() { return update_flag_; }); // take the message and process it auto buffer = msg_buffer_; msg_buffer_.clear(); update_flag_ = false; lock.unlock(); // 处理之 for (const auto& msg : buffer) { custom_func_(msg); } } } template void AsyncMessageProcess::AddMessage(const T& msg) { UL lock(mutex_); if (enable_skip_) { if (skip_cnt_ != 0) { skip_cnt_++; skip_cnt_ = skip_cnt_ % skip_num_; return; } skip_cnt_++; skip_cnt_ = skip_cnt_ % skip_num_; } msg_buffer_.push_back(msg); while (msg_buffer_.size() > max_size_) { LOG(ERROR) << name_ << " exceeds largest size: " << max_size_; msg_buffer_.pop_front(); } update_flag_ = true; cv_msg_.notify_one(); } template void AsyncMessageProcess::Quit() { update_flag_ = true; exit_flag_ = true; cv_msg_.notify_one(); if (proc_.joinable()) { proc_.join(); } } } // namespace lightning ================================================ FILE: src/utils/pointcloud_utils.cc ================================================ // // Created by xiang on 25-4-22. // #include "utils/pointcloud_utils.h" #include #include namespace lightning { /// 体素滤波 CloudPtr VoxelGrid(CloudPtr cloud, float voxel_size) { pcl::VoxelGrid voxel; voxel.setLeafSize(voxel_size, voxel_size, voxel_size); voxel.setInputCloud(cloud); CloudPtr output(new PointCloudType); voxel.filter(*output); return output; } /// 移除地面 void RemoveGround(CloudPtr cloud, float z_min) { CloudPtr output(new PointCloudType); for (const auto &pt : cloud->points) { if (pt.z > z_min) { output->points.emplace_back(pt); } } output->height = 1; output->is_dense = false; output->width = output->points.size(); cloud->swap(*output); } } // namespace lightning ================================================ FILE: src/utils/pointcloud_utils.h ================================================ // // Created by xiang on 25-4-22. // #ifndef LIGHTNING_POINTCLOUD_UTILS_H #define LIGHTNING_POINTCLOUD_UTILS_H #include "common/point_def.h" namespace lightning { /// 体素滤波 CloudPtr VoxelGrid(CloudPtr cloud, float voxel_size = 0.05); /// 移除地面 void RemoveGround(CloudPtr cloud, float z_min = 0.5); } // namespace lightning #endif // LIGHTNING_POINTCLOUD_UTILS_H ================================================ FILE: src/utils/sync.h ================================================ // // Created by xiang on 25-3-12. // #ifndef LIGHTNING_SYNC_H #define LIGHTNING_SYNC_H #include "common/measure_group.h" #include "common/point_def.h" #include "common/std_types.h" #include namespace lightning { /** * 将scan和odom同步 * 类似于ros中的消息同步器，但适用于离线模型，不需要subscriber和publisher */ class MessageSync { public: using Callback = std::function; MessageSync(Callback cb) : callback_(cb) {} /// 处理 scan 数据 void ProcessScan(CloudPtr scan) { UL lock(scan_mutex_); double timestamp = math::ToSec(scan->header.stamp); if (timestamp < last_timestamp_scan_) { scan_buffer_.clear(); } last_timestamp_scan_ = timestamp; scan_buffer_.emplace_back(scan); Sync(); } /** * 处理 imu 消息 * @param msg */ void ProcessIMU(IMUPtr msg) { UL lock(imu_mutex_); double timestamp = msg->timestamp; if (timestamp < last_timestamp_imu_) { LOG(WARNING) << "imu loop back, time difference is " << last_timestamp_imu_ - timestamp; return; } imu_buffer_.emplace_back(msg); last_timestamp_imu_ = timestamp; while (imu_buffer_.size() >= 500) { imu_buffer_.pop_front(); } } /** * 处理 odom 消息 * @param msg */ void ProcessOdom(OdomPtr msg) { UL lock(odom_mutex_); double timestamp = msg->timestamp_; if (timestamp < last_timestamp_odom_) { LOG(WARNING) << "odom (enc) loop back, time difference is " << last_timestamp_odom_ - timestamp; } odom_buffer_.emplace_back(msg); last_timestamp_odom_ = timestamp; while (odom_buffer_.size() >= 500) { odom_buffer_.pop_front(); } } private: /// 尝试同步odom和scan inline bool Sync() { UL lock_odom(odom_mutex_); UL lock(imu_mutex_); if (scan_buffer_.empty() || imu_buffer_.empty()) { return false; } MeasureGroup measures = MeasureGroup(); // 这个要重新构建一个 measures.scan_raw_ = scan_buffer_.back(); // 使用最近的scan measures.timestamp_ = math::ToSec(measures.scan_raw_->header.stamp); // 这个给的是结束时间 double curr_time = math::ToSec(measures.scan_raw_->header.stamp); double curr_time_end = curr_time + lo::lidar_time_interval; /// 查找最近时刻的imu bool found = false; double best_dt[2] = {1e9, 1e9}; IMUPtr best = nullptr; OdomPtr best_odom = nullptr; // NOTE 插个值更好一些 // NOTE // 由于odom精确时间只有底层通讯程序最清楚，期初想把插值工作交给底层（odom发布源），只需要插值后，发出跟激光同一时间戳 // best_dt > -1 for (auto& imu : imu_buffer_) { double dt = fabs(imu->timestamp - curr_time_end); if (dt < best_dt[0]) { best_dt[0] = dt; best = imu; } } for (auto& odom : odom_buffer_) { double dt = fabs(odom->timestamp_ - curr_time_end); if (dt < best_dt[1]) { best_dt[1] = dt; best_odom = odom; } } // &&: refuse to solve difficult logic if one is pass if (best_dt[0] < max_time_diff_ && (odom_buffer_.empty() || best_dt[1] < max_time_diff_)) { found = true; } if (!found) { // clean the buffers while (!imu_buffer_.empty() && imu_buffer_.front()->timestamp < (curr_time_end - 0.1)) { imu_buffer_.pop_front(); } while (!odom_buffer_.empty() && odom_buffer_.front()->timestamp_ < (curr_time_end - 0.1)) { odom_buffer_.pop_front(); } scan_buffer_.clear(); return false; } // clean the buffers while (!imu_buffer_.empty()) { bool bbreak = imu_buffer_.front() == best; measures.imu_.emplace_back(imu_buffer_.front()); imu_buffer_.pop_front(); if (bbreak) break; } while (!odom_buffer_.empty()) { bool bbreak = odom_buffer_.front() == best_odom; measures.odom_.emplace_back(odom_buffer_.front()); odom_buffer_.pop_front(); if (bbreak) break; } /// 找到了 lock.unlock(); // callback阶段不需要再锁定IMU lock_odom.unlock(); if (callback_) { callback_(measures); } scan_buffer_.clear(); return true; } std::mutex scan_mutex_; std::mutex imu_mutex_; std::mutex odom_mutex_; Callback callback_; // 同步数据后的回调函数 std::deque scan_buffer_; // scan 数据缓冲 std::deque imu_buffer_; // odom imu cache std::deque odom_buffer_; // odom (enc) cache double last_timestamp_scan_ = -1.0; // 最近scan时间 double last_timestamp_imu_ = 0; // nearest odom imu timestamp double last_timestamp_odom_ = 0; // nearest odom (enc) timestamp const double max_time_diff_ = 0.5; // 同步之允许的时间误差 }; } // namespace lightning #endif // LIGHTNING_SYNC_H ================================================ FILE: src/utils/timer.cc ================================================ // // Created by gx on 23-11-23. // #include "utils/timer.h" #include #include #include namespace lightning { std::map Timer::records_; void Timer::PrintAll() { LOG(INFO) << (">>> ===== Printing run time ====="); for (auto& r : records_) { auto& rec = r.second.time_usage_in_ms_; std::sort(rec.begin(), rec.end()); LOG(INFO) << "> [" << r.first << "] average time usage: " << std::accumulate(rec.begin(), rec.end(), 0.0) / double(rec.size()) << " ms, med: " << rec[rec.size() * 0.5] << " 95%: " << rec[rec.size() * 0.95] << ", called times : " << r.second.time_usage_in_ms_.size(); } LOG(INFO) << ">>> ===== Printing run time end ====="; } void Timer::DumpIntoFile(const std::string& file_name) { std::ofstream ofs(file_name, std::ios::out); if (!ofs.is_open()) { LOG(ERROR) << "Failed to open file: " << file_name; return; } else { LOG(INFO) << "Dump Time Records into file: " << file_name; } size_t max_length = 0; for (const auto& iter : records_) { ofs << iter.first << ", "; if (iter.second.time_usage_in_ms_.size() > max_length) { max_length = iter.second.time_usage_in_ms_.size(); } } ofs << std::endl; for (size_t i = 0; i < max_length; ++i) { for (const auto& iter : records_) { if (i < iter.second.time_usage_in_ms_.size()) { ofs << iter.second.time_usage_in_ms_[i] << ","; } else { ofs << ","; } } ofs << std::endl; } ofs.close(); } double Timer::GetMeanTime(const std::string& func_name) { if (records_.find(func_name) == records_.end()) { return 0.0; } auto r = records_[func_name]; return std::accumulate(r.time_usage_in_ms_.begin(), r.time_usage_in_ms_.end(), 0.0) / double(r.time_usage_in_ms_.size()); } } // namespace lightning ================================================ FILE: src/utils/timer.h ================================================ // // Created by gx on 23-11-23. // #pragma once #include #include #include #include #include #include #include namespace lightning { /// 统计时间工具 class Timer { public: struct TimerRecord { TimerRecord() = default; TimerRecord(const std::string& name, double time_usage) { func_name_ = name; time_usage_in_ms_.emplace_back(time_usage); } std::string func_name_; std::deque time_usage_in_ms_; }; /** * 评价并记录函数用时 * @tparam F * @param func * @param func_name */ template static void Evaluate(F&& func, const std::string& func_name, bool print = false) { auto t1 = std::chrono::steady_clock::now(); std::forward(func)(); auto t2 = std::chrono::steady_clock::now(); auto time_used = std::chrono::duration_cast>(t2 - t1).count() * 1000; if (records_.find(func_name) != records_.end()) { records_[func_name].time_usage_in_ms_.emplace_back(time_used); while (records_[func_name].time_usage_in_ms_.size() > 2000) { records_[func_name].time_usage_in_ms_.pop_front(); } } else { records_.insert({func_name, TimerRecord(func_name, time_used)}); } if (print) { LOG(INFO) << "func <" << func_name << "> timer: " << time_used << " ms"; } } /// 打印记录的所有耗时 static void PrintAll(); /// 写入文件，方便作图分析 static void DumpIntoFile(const std::string& file_name); /// 获取某个函数的平均执行时间 static double GetMeanTime(const std::string& func_name); /// 清理记录 static void Clear() { records_.clear(); } private: static std::map records_; }; } // namespace lightning ================================================ FILE: src/wrapper/bag_io.cc ================================================ // // Created by xiang on 23-12-14. // #include "bag_io.h" #include #include #include #include namespace lightning { void RosbagIO::Go(int sleep_usec) { std::filesystem::path p(bag_file_); rosbag2_cpp::Reader reader(std::make_unique()); rosbag2_cpp::ConverterOptions cv_options{"cdr", "cdr"}; reader.open({bag_file_, "sqlite3"}, cv_options); while (reader.has_next()) { auto msg = reader.read_next(); auto iter = process_func_.find(msg->topic_name); if (iter != process_func_.end()) { iter->second(msg); } if (sleep_usec > 0) { usleep(sleep_usec); } if (lightning::debug::flg_exit) { return; } } LOG(INFO) << "bag " << bag_file_ << " finished."; } } // namespace lightning ================================================ FILE: src/wrapper/bag_io.h ================================================ // // Created by xiang on 23-12-14. // #ifndef LIGHTNING_BAG_IO_H #define LIGHTNING_BAG_IO_H #include #include #include #include #include #include #include #include #include #include #include "livox_ros_driver2/msg/custom_msg.hpp" #include "common/imu.h" #include "common/odom.h" #include "common/point_def.h" #include "core/lightning_math.hpp" #include "io/dataset_type.h" #include "wrapper/ros_utils.h" namespace lightning { /** * ROSBAG IO * 指定一个包名，添加一些回调函数，就可以顺序遍历这个包 * 现在可以指定ROS2 */ class RosbagIO { public: explicit RosbagIO(std::string bag_file, DatasetType dataset_type = DatasetType::NCLT) : bag_file_(std::move(bag_file)) { /// handle ctrl-c signal(SIGINT, lightning::debug::SigHandle); } using MsgType = std::shared_ptr; using MessageProcessFunction = std::function; /// 一些方便直接使用的topics, messages using Scan2DHandle = std::function; using PointCloud2Handle = std::function; using LivoxCloud2Handle = std::function; using FullPointCloudHandle = std::function; using ImuHandle = std::function; using OdomHandle = std::function; /** * 遍历文件内容，调用回调函数 * @param sleep_usec 每调用一个回调后的等待时间 */ void Go(int sleep_usec = 0); /// 通用处理函数 RosbagIO &AddHandle(const std::string &topic_name, MessageProcessFunction func) { process_func_.emplace(topic_name, func); return *this; } /// point cloud 2 处理 RosbagIO &AddPointCloud2Handle(const std::string &topic_name, PointCloud2Handle f) { return AddHandle(topic_name, [f, this](const MsgType &m) -> bool { auto msg = std::make_shared(); rclcpp::SerializedMessage data(*m->serialized_data); seri_cloud2_.deserialize_message(&data, msg.get()); return f(msg); }); } /// livox 处理 RosbagIO &AddLivoxCloudHandle(const std::string &topic_name, LivoxCloud2Handle f) { return AddHandle(topic_name, [f, this](const MsgType &m) -> bool { auto msg = std::make_shared(); rclcpp::SerializedMessage data(*m->serialized_data); seri_livox_.deserialize_message(&data, msg.get()); return f(msg); }); } RosbagIO &AddImuHandle(const std::string &topic_name, ImuHandle f) { return AddHandle(topic_name, [f, this](const MsgType &m) -> bool { auto msg = std::make_shared(); rclcpp::SerializedMessage data(*m->serialized_data); seri_imu_.deserialize_message(&data, msg.get()); IMUPtr imu = std::make_shared(); imu->timestamp = ToSec(msg->header.stamp); /// NOTE: 如果需要乘重力，请修改此处 imu->linear_acceleration = Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); // constant::kGRAVITY; imu->angular_velocity = Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); return f(imu); }); } /// odom 处理 // RosbagIO &AddOdomHandle(const std::string &topic_name, OdomHandle f) { // return AddHandle(topic_name, [f, this](const MsgType &m) -> bool { // auto msg = std::make_shared(); // rclcpp::SerializedMessage data(*m->serialized_data); // seri_odom_.deserialize_message(&data, msg.get()); // /// nav_msg 的 odometry 转 odom // return f(msg); // }); // } /// 清除现有的处理函数 void CleanProcessFunc() { process_func_.clear(); } private: std::map process_func_; /// 序列化 rclcpp::Serialization seri_odom_; rclcpp::Serialization seri_imu_; rclcpp::Serialization seri_cloud2_; rclcpp::Serialization seri_livox_; std::string bag_file_; DatasetType dataset_type_ = DatasetType::NCLT; }; } // namespace lightning #endif // SLAM_ROS_BAG_IO_H ================================================ FILE: src/wrapper/ros_utils.h ================================================ // // Created by xiang on 25-3-24. // #ifndef LIGHTNING_ROS_UTILS_H #define LIGHTNING_ROS_UTILS_H #include #include #include "common/point_def.h" namespace lightning { inline double ToSec(const builtin_interfaces::msg::Time &time) { return double(time.sec) + 1e-9 * time.nanosec; } inline uint64_t ToNanoSec(const builtin_interfaces::msg::Time &time) { return time.sec * 1e9 + time.nanosec; } } // namespace lightning #endif // LIGHTNING_ROS_UTILS_H ================================================ FILE: srv/LocCmd.srv ================================================ # 定位命令 # 服务名: /lightning/loc_cmd # 用于重定位、设置地图等指令 uint8 CMD_LOC_SET_MAP = 1 # 设置地图 uint8 CMD_LOC_INIT_POSE = 3 # 设置重定位姿，参数由x y yaw 给出 uint8 CMD_LOC_INIT_LIST = 4 # 设置重定位点列表，由建图工具设置并保存成队列 uint8 id # 命令id float64 x # 重定位点坐标x float64 y # 重定位点坐标x float64 z # 重定位点坐标x string map_id # 地图路径 std_msgs/Header header # 时间戳 --- uint32 result # 为0表示成功 ================================================ FILE: srv/SaveMap.srv ================================================ # 存储地图服务 # 服务名: /lightning/save_map # 启动slam节点后提供 string map_id # 地图名称（唯一标识） --- uint32 response # 为0时成功， 1保存地图已经在运行 2图像渲染重置失败 3地图数据为空，无法保存，4扩图定位失败 ================================================ FILE: thirdparty/Sophus/average.hpp ================================================ /// @file /// Calculation of biinvariant means. #ifndef SOPHUS_AVERAGE_HPP #define SOPHUS_AVERAGE_HPP #include "common.hpp" #include "rxso2.hpp" #include "rxso3.hpp" #include "se2.hpp" #include "se3.hpp" #include "sim2.hpp" #include "sim3.hpp" #include "so2.hpp" #include "so3.hpp" namespace Sophus { /// Calculates mean iteratively. /// /// Returns ``nullopt`` if it does not converge. /// template optional iterativeMean( SequenceContainer const& foo_Ts_bar, int max_num_iterations) { size_t N = foo_Ts_bar.size(); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); using Group = typename SequenceContainer::value_type; using Scalar = typename Group::Scalar; using Tangent = typename Group::Tangent; // This implements the algorithm in the beginning of Sec. 4.2 in // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. Group foo_T_average = foo_Ts_bar.front(); Scalar w = Scalar(1. / N); for (int i = 0; i < max_num_iterations; ++i) { Tangent average; setToZero(average); for (Group const& foo_T_bar : foo_Ts_bar) { average += w * (foo_T_average.inverse() * foo_T_bar).log(); } Group foo_T_newaverage = foo_T_average * Group::exp(average); if (squaredNorm( (foo_T_newaverage.inverse() * foo_T_average).log()) < Constants::epsilon()) { return foo_T_newaverage; } foo_T_average = foo_T_newaverage; } // LCOV_EXCL_START return nullopt; // LCOV_EXCL_STOP } #ifdef DOXYGEN_SHOULD_SKIP_THIS /// Mean implementation for any Lie group. template optional average( SequenceContainer const& foo_Ts_bar); #else // Mean implementation for SO(2). template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { // This implements rotational part of Proposition 12 from Sec. 6.2 of // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); SO2 foo_T_average = foo_Ts_bar.front(); Scalar w = Scalar(1. / N); Scalar average(0); for (SO2 const& foo_T_bar : foo_Ts_bar) { average += w * (foo_T_average.inverse() * foo_T_bar).log(); } return foo_T_average * SO2::exp(average); } // Mean implementation for RxSO(2). template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); RxSO2 foo_T_average = foo_Ts_bar.front(); Scalar w = Scalar(1. / N); Vector2 average(Scalar(0), Scalar(0)); for (RxSO2 const& foo_T_bar : foo_Ts_bar) { average += w * (foo_T_average.inverse() * foo_T_bar).log(); } return foo_T_average * RxSO2::exp(average); } namespace details { template void getQuaternion(T const&); template Eigen::Quaternion getUnitQuaternion(SO3 const& R) { return R.unit_quaternion(); } template Eigen::Quaternion getUnitQuaternion(RxSO3 const& sR) { return sR.so3().unit_quaternion(); } template Eigen::Quaternion averageUnitQuaternion( SequenceContainer const& foo_Ts_bar) { // This: http://stackoverflow.com/a/27410865/1221742 size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); Eigen::Matrix Q(4, N); int i = 0; Scalar w = Scalar(1. / N); for (auto const& foo_T_bar : foo_Ts_bar) { Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs(); ++i; } Eigen::Matrix QQt = Q * Q.transpose(); // TODO: Figure out why we can't use SelfAdjointEigenSolver here. Eigen::EigenSolver > es(QQt); std::complex max_eigenvalue = es.eigenvalues()[0]; Eigen::Matrix, 4, 1> max_eigenvector = es.eigenvectors().col(0); for (int i = 1; i < 4; i++) { if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) { max_eigenvalue = es.eigenvalues()[i]; max_eigenvector = es.eigenvectors().col(i); } } Eigen::Quaternion quat; quat.coeffs() << // max_eigenvector[0].real(), // max_eigenvector[1].real(), // max_eigenvector[2].real(), // max_eigenvector[3].real(); return quat; } } // namespace details // Mean implementation for SO(3). // // TODO: Detect degenerated cases and return nullopt. template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { return SO3(details::averageUnitQuaternion(foo_Ts_bar)); } // Mean implementation for R x SO(3). template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar) { size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar)); SOPHUS_ENSURE(N >= 1, "N must be >= 1."); Scalar scale_sum = Scalar(0); using std::exp; using std::log; for (RxSO3 const& foo_T_bar : foo_Ts_bar) { scale_sum += log(foo_T_bar.scale()); } return RxSO3(exp(scale_sum / Scalar(N)), SO3(details::averageUnitQuaternion(foo_Ts_bar))); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { // TODO: Implement Proposition 12 from Sec. 6.2 of // ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf. return iterativeMean(foo_Ts_bar, max_num_iterations); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } template enable_if_t< std::is_same >::value, optional > average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) { return iterativeMean(foo_Ts_bar, max_num_iterations); } #endif // DOXYGEN_SHOULD_SKIP_THIS } // namespace Sophus #endif // SOPHUS_AVERAGE_HPP ================================================ FILE: thirdparty/Sophus/common.hpp ================================================ /// @file /// Common functionality. #ifndef SOPHUS_COMMON_HPP #define SOPHUS_COMMON_HPP #include #include #include #include #include #include #if !defined(SOPHUS_DISABLE_ENSURES) #include "formatstring.hpp" #endif //!defined(SOPHUS_DISABLE_ENSURES) // following boost's assert.hpp #undef SOPHUS_ENSURE // ENSURES are similar to ASSERTS, but they are always checked for (including in // release builds). At the moment there are no ASSERTS in Sophus which should // only be used for checks which are performance critical. #ifdef __GNUC__ #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ #elif (_MSC_VER >= 1310) #define SOPHUS_FUNCTION __FUNCTION__ #else #define SOPHUS_FUNCTION "unknown" #endif // Make sure this compiles with older versions of Eigen which do not have // EIGEN_DEVICE_FUNC defined. #ifndef EIGEN_DEVICE_FUNC #define EIGEN_DEVICE_FUNC #endif // Make sure this compiles with older versions of Eigen which do not have // EIGEN_DEFAULT_COPY_CONSTRUCTOR defined #ifndef EIGEN_DEFAULT_COPY_CONSTRUCTOR #if EIGEN_HAS_CXX11 #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default; #else #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) #endif #ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS #error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS" #endif #define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived) #endif #ifndef SOPHUS_INHERIT_ASSIGNMENT_OPERATORS #ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS #error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS" #endif #define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) #endif #define SOPHUS_FUNC EIGEN_DEVICE_FUNC #if defined(SOPHUS_DISABLE_ENSURES) #define SOPHUS_ENSURE(expr, ...) ((void)0) #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) namespace Sophus { void ensureFailed(char const* function, char const* file, int line, char const* description); } #define SOPHUS_ENSURE(expr, ...) \ ((expr) ? ((void)0) \ : ::Sophus::ensureFailed( \ SOPHUS_FUNCTION, __FILE__, __LINE__, \ Sophus::details::FormatString(__VA_ARGS__).c_str())) #else // LCOV_EXCL_START namespace Sophus { template SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, char const* description, Args&&... args) { std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", function, file, line); #ifdef __CUDACC__ std::printf("%s", description); #else std::cout << details::FormatString(description, std::forward(args)...) << std::endl; std::abort(); #endif } } // namespace Sophus // LCOV_EXCL_STOP #define SOPHUS_ENSURE(expr, ...) \ ((expr) ? ((void)0) \ : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ ##__VA_ARGS__)) #endif namespace Sophus { template struct Constants { SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } SOPHUS_FUNC static Scalar epsilonSqrt() { using std::sqrt; return sqrt(epsilon()); } SOPHUS_FUNC static Scalar pi() { return Scalar(3.141592653589793238462643383279502884); } }; template <> struct Constants { SOPHUS_FUNC static float constexpr epsilon() { return static_cast(1e-5); } SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } SOPHUS_FUNC static float constexpr pi() { return 3.141592653589793238462643383279502884f; } }; /// Nullopt type of lightweight optional class. struct nullopt_t { explicit constexpr nullopt_t() {} }; constexpr nullopt_t nullopt{}; /// Lightweight optional implementation which requires ``T`` to have a /// default constructor. /// /// TODO: Replace with std::optional once Sophus moves to c++17. /// template class optional { public: optional() : is_valid_(false) {} optional(nullopt_t) : is_valid_(false) {} optional(T const& type) : type_(type), is_valid_(true) {} explicit operator bool() const { return is_valid_; } T const* operator->() const { SOPHUS_ENSURE(is_valid_, "must be valid"); return &type_; } T* operator->() { SOPHUS_ENSURE(is_valid_, "must be valid"); return &type_; } T const& operator*() const { SOPHUS_ENSURE(is_valid_, "must be valid"); return type_; } T& operator*() { SOPHUS_ENSURE(is_valid_, "must be valid"); return type_; } private: T type_; bool is_valid_; }; template using enable_if_t = typename std::enable_if::type; template struct IsUniformRandomBitGenerator { static const bool value = std::is_unsigned::value && std::is_unsigned::value && std::is_unsigned::value; }; } // namespace Sophus #endif // SOPHUS_COMMON_HPP ================================================ FILE: thirdparty/Sophus/formatstring.hpp ================================================ /// @file /// FormatString functionality #ifndef SOPHUS_FORMATSTRING_HPP #define SOPHUS_FORMATSTRING_HPP #include namespace Sophus { namespace details { // Following: http://stackoverflow.com/a/22759544 template class IsStreamable { private: template static auto test(int) -> decltype(std::declval() << std::declval(), std::true_type()); template static auto test(...) -> std::false_type; public: static bool const value = decltype(test(0))::value; }; template class ArgToStream { public: static void impl(std::stringstream& stream, T&& arg) { stream << std::forward(arg); } }; inline void FormatStream(std::stringstream& stream, char const* text) { stream << text; return; } // Following: http://en.cppreference.com/w/cpp/language/parameter_pack template void FormatStream(std::stringstream& stream, char const* text, T&& arg, Args&&... args) { static_assert(IsStreamable::value, "One of the args has no ostream overload!"); for (; *text != '\0'; ++text) { if (*text == '%') { ArgToStream::impl(stream, std::forward(arg)); FormatStream(stream, text + 1, std::forward(args)...); return; } stream << *text; } stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 << " args unused."; return; } template std::string FormatString(char const* text, Args&&... args) { std::stringstream stream; FormatStream(stream, text, std::forward(args)...); return stream.str(); } inline std::string FormatString() { return std::string(); } } // namespace details } // namespace Sophus #endif //SOPHUS_FORMATSTRING_HPP ================================================ FILE: thirdparty/Sophus/geometry.hpp ================================================ /// @file /// Transformations between poses and hyperplanes. #ifndef GEOMETRY_HPP #define GEOMETRY_HPP #include "se2.hpp" #include "se3.hpp" #include "so2.hpp" #include "so3.hpp" #include "types.hpp" namespace Sophus { /// Takes in a rotation ``R_foo_plane`` and returns the corresponding line /// normal along the y-axis (in reference frame ``foo``). /// template Vector2 normalFromSO2(SO2 const& R_foo_line) { return R_foo_line.matrix().col(1); } /// Takes in line normal in reference frame foo and constructs a corresponding /// rotation matrix ``R_foo_line``. /// /// Precondition: ``normal_foo`` must not be close to zero. /// template SO2 SO2FromNormal(Vector2 normal_foo) { SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", normal_foo.transpose()); normal_foo.normalize(); return SO2(normal_foo.y(), -normal_foo.x()); } /// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane /// normal along the z-axis /// (in reference frame ``foo``). /// template Vector3 normalFromSO3(SO3 const& R_foo_plane) { return R_foo_plane.matrix().col(2); } /// Takes in plane normal in reference frame foo and constructs a corresponding /// rotation matrix ``R_foo_plane``. /// /// Note: The ``plane`` frame is defined as such that the normal points along /// the positive z-axis. One can specify hints for the x-axis and y-axis /// of the ``plane`` frame. /// /// Preconditions: /// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to /// zero. /// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. /// template Matrix3 rotationFromNormal(Vector3 const& normal_foo, Vector3 xDirHint_foo = Vector3(T(1), T(0), T(0)), Vector3 yDirHint_foo = Vector3(T(0), T(1), T(0))) { SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), "xDirHint (%) and yDirHint (%) must be perpendicular.", xDirHint_foo.transpose(), yDirHint_foo.transpose()); using std::abs; using std::sqrt; T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); T const normal_foo_sqr_length = normal_foo.squaredNorm(); SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", xDirHint_foo.transpose()); SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", yDirHint_foo.transpose()); SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", normal_foo.transpose()); Matrix3 basis_foo; basis_foo.col(2) = normal_foo; if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { xDirHint_foo.normalize(); } if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { yDirHint_foo.normalize(); } if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { basis_foo.col(2).normalize(); } T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); if (abs_x_dot_z < abs_y_dot_z) { // basis_foo.z and xDirHint are far from parallel. basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); } else { // basis_foo.z and yDirHint are far from parallel. basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); } T det = basis_foo.determinant(); // sanity check SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), "Determinant of basis is not 1, but %. Basis is \n%\n", det, basis_foo); return basis_foo; } /// Takes in plane normal in reference frame foo and constructs a corresponding /// rotation matrix ``R_foo_plane``. /// /// See ``rotationFromNormal`` for details. /// template SO3 SO3FromNormal(Vector3 const& normal_foo) { return SO3(rotationFromNormal(normal_foo)); } /// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in /// reference frame ``foo``. /// /// Note: The plane is defined by X-axis of the ``line`` frame. /// template Line2 lineFromSE2(SE2 const& T_foo_line) { return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); } /// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. /// /// Note: The line is defined by X-axis of the frame ``line``. /// template SE2 SE2FromLine(Line2 const& line_foo) { T const d = line_foo.offset(); Vector2 const n = line_foo.normal(); SO2 const R_foo_plane = SO2FromNormal(n); return SE2(R_foo_plane, -d * n); } /// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in /// reference frame ``foo``. /// /// Note: The plane is defined by XY-plane of the frame ``plane``. /// template Plane3 planeFromSE3(SE3 const& T_foo_plane) { return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); } /// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. /// /// Note: The plane is defined by XY-plane of the frame ``plane``. /// template SE3 SE3FromPlane(Plane3 const& plane_foo) { T const d = plane_foo.offset(); Vector3 const n = plane_foo.normal(); SO3 const R_foo_plane = SO3FromNormal(n); return SE3(R_foo_plane, -d * n); } /// Takes in a hyperplane and returns unique representation by ensuring that the /// ``offset`` is not negative. /// template Eigen::Hyperplane makeHyperplaneUnique( Eigen::Hyperplane const& plane) { if (plane.offset() >= 0) { return plane; } return Eigen::Hyperplane(-plane.normal(), -plane.offset()); } } // namespace Sophus #endif // GEOMETRY_HPP ================================================ FILE: thirdparty/Sophus/interpolate.hpp ================================================ /// @file /// Interpolation for Lie groups. #ifndef SOPHUS_INTERPOLATE_HPP #define SOPHUS_INTERPOLATE_HPP #include #include "interpolate_details.hpp" namespace Sophus { /// This function interpolates between two Lie group elements ``foo_T_bar`` /// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. /// /// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` /// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns /// ``foo_T_bar``. /// /// (Since interpolation on Lie groups is inverse-invariant, we can equivalently /// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the /// return value being ``quiz_T_foo``.) /// /// Precondition: ``p`` must be in [0, 1]. /// template enable_if_t::supported, G> interpolate( G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { using Scalar = typename G::Scalar; Scalar inter_p(p); SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), "p (%) must in [0, 1]."); return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); } } // namespace Sophus #endif // SOPHUS_INTERPOLATE_HPP ================================================ FILE: thirdparty/Sophus/interpolate_details.hpp ================================================ #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP #define SOPHUS_INTERPOLATE_DETAILS_HPP #include "rxso2.hpp" #include "rxso3.hpp" #include "se2.hpp" #include "se3.hpp" #include "sim2.hpp" #include "sim3.hpp" #include "so2.hpp" #include "so3.hpp" namespace Sophus { namespace interp_details { template struct Traits; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { using std::abs; Scalar angle = foo_T_bar.log(); return abs(abs(angle) - Constants::pi()) < Constants::epsilon(); } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { using std::abs; Scalar angle = foo_T_bar.logAndTheta().theta; return abs(abs(angle) - Constants::pi()) < Constants::epsilon(); } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { return Traits>::hasShortestPathAmbiguity( foo_T_bar.rxso2().so2()); ; } }; template struct Traits> { static bool constexpr supported = true; static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { return Traits>::hasShortestPathAmbiguity( foo_T_bar.rxso3().so3()); ; } }; } // namespace interp_details } // namespace Sophus #endif // SOPHUS_INTERPOLATE_DETAILS_HPP ================================================ FILE: thirdparty/Sophus/num_diff.hpp ================================================ /// @file /// Numerical differentiation using finite differences #ifndef SOPHUS_NUM_DIFF_HPP #define SOPHUS_NUM_DIFF_HPP #include #include #include #include "types.hpp" namespace Sophus { namespace details { template class Curve { public: template static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { using ReturnType = decltype(curve(t)); static_assert(std::is_floating_point::value, "Scalar must be a floating point type."); static_assert(IsFloatingPoint::value, "ReturnType must be either a floating point scalar, " "vector or matrix."); return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); } }; template class VectorField { public: static Eigen::Matrix num_diff( std::function(Sophus::Vector)> vector_field, Sophus::Vector const& a, Scalar eps) { static_assert(std::is_floating_point::value, "Scalar must be a floating point type."); Eigen::Matrix J; Sophus::Vector h; h.setZero(); for (int i = 0; i < M; ++i) { h[i] = eps; J.col(i) = (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); h[i] = Scalar(0); } return J; } }; template class VectorField { public: static Eigen::Matrix num_diff( std::function(Scalar)> vector_field, Scalar const& a, Scalar eps) { return details::Curve::num_diff(std::move(vector_field), a, eps); } }; } // namespace details /// Calculates the derivative of a curve at a point ``t``. /// /// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it /// returns either a Scalar, a vector or a matrix. /// template auto curveNumDiff(Fn curve, Scalar t, Scalar h = Constants::epsilonSqrt()) -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { return details::Curve::num_diff(std::move(curve), t, h); } /// Calculates the derivative of a vector field at a point ``a``. /// /// Here, a vector field is a function from a vector space to another vector /// space. /// template Eigen::Matrix vectorFieldNumDiff( Fn vector_field, ScalarOrVector const& a, Scalar eps = Constants::epsilonSqrt()) { return details::VectorField::num_diff(std::move(vector_field), a, eps); } } // namespace Sophus #endif // SOPHUS_NUM_DIFF_HPP ================================================ FILE: thirdparty/Sophus/rotation_matrix.hpp ================================================ /// @file /// Rotation matrix helper functions. #ifndef SOPHUS_ROTATION_MATRIX_HPP #define SOPHUS_ROTATION_MATRIX_HPP #include #include #include "types.hpp" namespace Sophus { /// Takes in arbitrary square matrix and returns true if it is /// orthogonal. template SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { using Scalar = typename D::Scalar; static int const N = D::RowsAtCompileTime; static int const M = D::ColsAtCompileTime; static_assert(N == M, "must be a square matrix"); static_assert(N >= 2, "must have compile time dimension >= 2"); return (R * R.transpose() - Matrix::Identity()).norm() < Constants::epsilon(); } /// Takes in arbitrary square matrix and returns true if it is /// "scaled-orthogonal" with positive determinant. /// template SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { using Scalar = typename D::Scalar; static int const N = D::RowsAtCompileTime; static int const M = D::ColsAtCompileTime; using std::pow; using std::sqrt; Scalar det = sR.determinant(); if (det <= Scalar(0)) { return false; } Scalar scale_sqr = pow(det, Scalar(2. / N)); static_assert(N == M, "must be a square matrix"); static_assert(N >= 2, "must have compile time dimension >= 2"); return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) .template lpNorm() < sqrt(Constants::epsilon()); } /// Takes in arbitrary square matrix (2x2 or larger) and returns closest /// orthogonal matrix with positive determinant. template SOPHUS_FUNC enable_if_t< std::is_floating_point::value, Matrix> makeRotationMatrix(Eigen::MatrixBase const& R) { using Scalar = typename D::Scalar; static int const N = D::RowsAtCompileTime; static int const M = D::ColsAtCompileTime; static_assert(N == M, "must be a square matrix"); static_assert(N >= 2, "must have compile time dimension >= 2"); Eigen::JacobiSVD> svd( R, Eigen::ComputeFullU | Eigen::ComputeFullV); // Determine determinant of orthogonal matrix U*V'. Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); // Starting from the identity matrix D, set the last entry to d (+1 or // -1), so that det(U*D*V') = 1. Matrix Diag = Matrix::Identity(); Diag(N - 1, N - 1) = d; return svd.matrixU() * Diag * svd.matrixV().transpose(); } } // namespace Sophus #endif // SOPHUS_ROTATION_MATRIX_HPP ================================================ FILE: thirdparty/Sophus/rxso2.hpp ================================================ /// @file /// Direct product R X SO(2) - rotation and scaling in 2d. #ifndef SOPHUS_RXSO2_HPP #define SOPHUS_RXSO2_HPP #include "so2.hpp" namespace Sophus { template class RxSO2; using RxSO2d = RxSO2; using RxSO2f = RxSO2; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using ComplexType = Sophus::Vector2; }; template struct traits, Options_>> : traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using ComplexType = Map, Options>; }; template struct traits const, Options_>> : traits const> { static constexpr int Options = Options_; using Scalar = Scalar_; using ComplexType = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// RxSO2 base type - implements RxSO2 class but is storage agnostic /// /// This class implements the group ``R+ x SO(2)``, the direct product of the /// group of positive scalar 2x2 matrices (= isomorph to the positive /// real numbers) and the two-dimensional special orthogonal group SO(2). /// Geometrically, it is the group of rotation and scaling in two dimensions. /// As a matrix groups, R+ x SO(2) consists of matrices of the form ``s * R`` /// where ``R`` is an orthogonal matrix with ``det(R) = 1`` and ``s > 0`` /// being a positive real number. In particular, it has the following form: /// /// | s * cos(theta) s * -sin(theta) | /// | s * sin(theta) s * cos(theta) | /// /// where ``theta`` being the rotation angle. Internally, it is represented by /// the first column of the rotation matrix, or in other words by a non-zero /// complex number. /// /// R+ x SO(2) is not compact, but a commutative group. First it is not compact /// since the scale factor is not bound. Second it is commutative since /// ``sR(alpha, s1) * sR(beta, s2) = sR(beta, s2) * sR(alpha, s1)``, simply /// because ``alpha + beta = beta + alpha`` and ``s1 * s2 = s2 * s1`` with /// ``alpha`` and ``beta`` being rotation angles and ``s1``, ``s2`` being scale /// factors. /// /// This class has the explicit class invariant that the scale ``s`` is not /// too close to zero. Strictly speaking, it must hold that: /// /// ``complex().norm() >= Constants::epsilon()``. /// /// In order to obey this condition, group multiplication is implemented with /// saturation such that a product always has a scale which is equal or greater /// this threshold. template class RxSO2Base { public: static constexpr int Options = Eigen::internal::traits::Options; using Scalar = typename Eigen::internal::traits::Scalar; using ComplexType = typename Eigen::internal::traits::ComplexType; using ComplexTemporaryType = Sophus::Vector2; /// Degrees of freedom of manifold, number of dimensions in tangent space /// (one for rotation and one for scaling). static int constexpr DoF = 2; /// Number of internal parameters used (complex number is a tuple). static int constexpr num_parameters = 2; /// Group transformations are 2x2 matrices. static int constexpr N = 2; using Transformation = Matrix; using Point = Vector2; using HomogeneousPoint = Vector3; using Line = ParametrizedLine2; using Tangent = Vector; using Adjoint = Matrix; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with RxSO2 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using RxSO2Product = RxSO2>; template using PointProduct = Vector2>; template using HomogeneousPointProduct = Vector3>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// /// For RxSO(2), it simply returns the identity matrix. /// SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); } /// Returns rotation angle. /// SOPHUS_FUNC Scalar angle() const { return SO2(complex()).log(); } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC RxSO2 cast() const { return RxSO2(complex().template cast()); } /// This provides unsafe read/write access to internal data. RxSO(2) is /// represented by a complex number (two parameters). When using direct /// write access, the user needs to take care of that the complex number is /// not set close to zero. /// /// Note: The first parameter represents the real part, while the /// second parameter represent the imaginary part. /// SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { return complex().data(); } /// Returns group inverse. /// SOPHUS_FUNC RxSO2 inverse() const { Scalar squared_scale = complex().squaredNorm(); return RxSO2(complex().x() / squared_scale, -complex().y() / squared_scale); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (scaled rotation matrices) to elements of the tangent /// space (rotation-vector plus logarithm of scale factor). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of RxSO2. /// SOPHUS_FUNC Tangent log() const { using std::log; Tangent theta_sigma; theta_sigma[1] = log(scale()); theta_sigma[0] = SO2(complex()).log(); return theta_sigma; } /// Returns 2x2 matrix representation of the instance. /// /// For RxSO2, the matrix representation is an scaled orthogonal matrix ``sR`` /// with ``det(R)=s^2``, thus a scaled rotation matrix ``R`` with scale /// ``s``. /// SOPHUS_FUNC Transformation matrix() const { Transformation sR; // clang-format off sR << complex()[0], -complex()[1], complex()[1], complex()[0]; // clang-format on return sR; } /// Assignment operator. /// SOPHUS_FUNC RxSO2Base& operator=(RxSO2Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC RxSO2Base& operator=( RxSO2Base const& other) { complex_nonconst() = other.complex(); return *this; } /// Group multiplication, which is rotation concatenation and scale /// multiplication. /// /// Note: This function performs saturation for products close to zero in /// order to ensure the class invariant. /// template SOPHUS_FUNC RxSO2Product operator*( RxSO2Base const& other) const { using ResultT = ReturnScalar; Scalar lhs_real = complex().x(); Scalar lhs_imag = complex().y(); typename OtherDerived::Scalar const& rhs_real = other.complex().x(); typename OtherDerived::Scalar const& rhs_imag = other.complex().y(); /// complex multiplication typename RxSO2Product::ComplexType result_complex( lhs_real * rhs_real - lhs_imag * rhs_imag, lhs_real * rhs_imag + lhs_imag * rhs_real); const ResultT squared_scale = result_complex.squaredNorm(); if (squared_scale < Constants::epsilon() * Constants::epsilon()) { /// Saturation to ensure class invariant. result_complex.normalize(); result_complex *= Constants::epsilon(); } return RxSO2Product(result_complex); } /// Group action on 2-points. /// /// This function rotates a 2 dimensional point ``p`` by the SO2 element /// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor ``s``: /// /// ``p_bar = s * (bar_R_foo * p_foo)``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return matrix() * p; } /// Group action on homogeneous 2-points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rsp = *this * p.template head<2>(); return HomogeneousPointProduct(rsp(0), rsp(1), p(2)); } /// Group action on lines. /// /// This function rotates a parameterized line ``l(t) = o + t * d`` by the SO2 /// element and scales it by the scale factor /// /// Origin ``o`` is rotated and scaled /// Direction ``d`` is rotated (preserving it's norm) /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), (*this) * l.direction() / scale()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO2's Scalar type. /// /// Note: This function performs saturation for products close to zero in /// order to ensure the class invariant. /// template >::value>::type> SOPHUS_FUNC RxSO2Base& operator*=( RxSO2Base const& other) { *static_cast(this) = *this * other; return *this; } /// Returns internal parameters of RxSO(2). /// /// It returns (c[0], c[1]), with c being the complex number. /// SOPHUS_FUNC Sophus::Vector params() const { return complex(); } /// Sets non-zero complex /// /// Precondition: ``z`` must not be close to zero. SOPHUS_FUNC void setComplex(Vector2 const& z) { SOPHUS_ENSURE(z.squaredNorm() > Constants::epsilon() * Constants::epsilon(), "Scale factor must be greater-equal epsilon."); static_cast(this)->complex_nonconst() = z; } /// Accessor of complex. /// SOPHUS_FUNC ComplexType const& complex() const { return static_cast(this)->complex(); } /// Returns rotation matrix. /// SOPHUS_FUNC Transformation rotationMatrix() const { ComplexTemporaryType norm_quad = complex(); norm_quad.normalize(); return SO2(norm_quad).matrix(); } /// Returns scale. /// SOPHUS_FUNC Scalar scale() const { return complex().norm(); } /// Setter of rotation angle, leaves scale as is. /// SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2(theta)); } /// Setter of complex using rotation matrix ``R``, leaves scale as is. /// /// Precondition: ``R`` must be orthogonal with determinant of one. /// SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { setSO2(SO2(R)); } /// Sets scale and leaves rotation as is. /// SOPHUS_FUNC void setScale(Scalar const& scale) { using std::sqrt; complex_nonconst().normalize(); complex_nonconst() *= scale; } /// Setter of complex number using scaled rotation matrix ``sR``. /// /// Precondition: The 2x2 matrix must be "scaled orthogonal" /// and have a positive determinant. /// SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR), "sR must be scaled orthogonal:\n %", sR); complex_nonconst() = sR.col(0); } /// Setter of SO(2) rotations, leaves scale as is. /// SOPHUS_FUNC void setSO2(SO2 const& so2) { using std::sqrt; Scalar saved_scale = scale(); complex_nonconst() = so2.unit_complex(); complex_nonconst() *= saved_scale; } SOPHUS_FUNC SO2 so2() const { return SO2(complex()); } protected: /// Mutator of complex is private to ensure class invariant. /// SOPHUS_FUNC ComplexType& complex_nonconst() { return static_cast(this)->complex_nonconst(); } }; /// RxSO2 using storage; derived from RxSO2Base. template class RxSO2 : public RxSO2Base> { public: using Base = RxSO2Base>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using ComplexMember = Eigen::Matrix; /// ``Base`` is friend so complex_nonconst can be accessed from ``Base``. friend class RxSO2Base>; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes complex number to identity rotation and /// scale to 1. /// SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {} /// Copy constructor /// SOPHUS_FUNC RxSO2(RxSO2 const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC RxSO2(RxSO2Base const& other) : complex_(other.complex()) {} /// Constructor from scaled rotation matrix /// /// Precondition: rotation matrix need to be scaled orthogonal with /// determinant of ``s^2``. /// SOPHUS_FUNC explicit RxSO2(Transformation const& sR) { this->setScaledRotationMatrix(sR); } /// Constructor from scale factor and rotation matrix ``R``. /// /// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant /// of 1 and ``scale`` must to be close to zero. /// SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R) : RxSO2((scale * SO2(R).unit_complex()).eval()) {} /// Constructor from scale factor and SO2 /// /// Precondition: ``scale`` must be close to zero. /// SOPHUS_FUNC RxSO2(Scalar const& scale, SO2 const& so2) : RxSO2((scale * so2.unit_complex()).eval()) {} /// Constructor from complex number. /// /// Precondition: complex number must not be close to zero. /// SOPHUS_FUNC explicit RxSO2(Vector2 const& z) : complex_(z) { SOPHUS_ENSURE(complex_.squaredNorm() >= Constants::epsilon() * Constants::epsilon(), "Scale factor must be greater-equal epsilon: % vs %", complex_.squaredNorm(), Constants::epsilon() * Constants::epsilon()); } /// Constructor from complex number. /// /// Precondition: complex number must not be close to zero. /// SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag) : RxSO2(Vector2(real, imag)) {} /// Accessor of complex. /// SOPHUS_FUNC ComplexMember const& complex() const { return complex_; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Group exponential /// /// This functions takes in an element of tangent space (= rotation angle /// plus logarithm of scale) and returns the corresponding element of the /// group RxSO2. /// /// To be more specific, this function computes ``expmat(hat(theta))`` /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the /// hat()-operator of RSO2. /// SOPHUS_FUNC static RxSO2 exp(Tangent const& a) { using std::exp; Scalar const theta = a[0]; Scalar const sigma = a[1]; Scalar s = exp(sigma); Vector2 z = SO2::exp(theta).unit_complex(); z *= s; return RxSO2(z); } /// Returns the ith infinitesimal generators of ``R+ x SO(2)``. /// /// The infinitesimal generators of RxSO2 are: /// /// ``` /// | 0 -1 | /// G_0 = | 1 0 | /// /// | 1 0 | /// G_1 = | 0 1 | /// ``` /// /// Precondition: ``i`` must be 0, or 1. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= 1, "i should be 0 or 1."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// hat-operator /// /// It takes in the 2-vector representation ``a`` (= rotation angle plus /// logarithm of scale) and returns the corresponding matrix representation /// of Lie algebra element. /// /// Formally, the hat()-operator of RxSO2 is defined as /// /// ``hat(.): R^2 -> R^{2x2}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2) /// /// with ``G_i`` being the ith infinitesimal generator of RxSO2. /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation A; // clang-format off A << a(1), -a(0), a(0), a(1); // clang-format on return A; } /// Lie bracket /// /// It computes the Lie bracket of RxSO(2). To be more specific, it computes /// /// ``[omega_1, omega_2]_rxso2 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of RxSO2. /// SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { Vector2 res; res.setZero(); return res; } /// Draw uniform sample from RxSO(2) manifold. /// /// The scale factor is drawn uniformly in log2-space from [-1, 1], /// hence the scale is in [0.5, 2)]. /// template static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); using std::exp2; return RxSO2(exp2(uniform(generator)), SO2::sampleUniform(generator)); } /// vee-operator /// /// It takes the 2x2-matrix representation ``Omega`` and maps it to the /// corresponding vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | d -x | /// | x d | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { using std::abs; return Tangent(Omega(1, 0), Omega(0, 0)); } protected: SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; } ComplexMember complex_; }; } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``RxSO2``; derived from RxSO2Base. /// /// Allows us to wrap RxSO2 objects around POD array (e.g. external z style /// complex). template class Map, Options> : public Sophus::RxSO2Base, Options>> { using Base = Sophus::RxSO2Base, Options>>; public: using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; /// ``Base`` is friend so complex_nonconst can be accessed from ``Base``. friend class Sophus::RxSO2Base, Options>>; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map); // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {} /// Accessor of complex. /// SOPHUS_FUNC Map, Options> const& complex() const { return complex_; } protected: SOPHUS_FUNC Map, Options>& complex_nonconst() { return complex_; } Map, Options> complex_; }; /// Specialization of Eigen::Map for ``RxSO2 const``; derived from RxSO2Base. /// /// Allows us to wrap RxSO2 objects around POD array (e.g. external z style /// complex). template class Map const, Options> : public Sophus::RxSO2Base const, Options>> { public: using Base = Sophus::RxSO2Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : complex_(coeffs) {} /// Accessor of complex. /// SOPHUS_FUNC Map const, Options> const& complex() const { return complex_; } protected: Map const, Options> const complex_; }; } // namespace Eigen #endif /// SOPHUS_RXSO2_HPP ================================================ FILE: thirdparty/Sophus/rxso3.hpp ================================================ /// @file /// Direct product R X SO(3) - rotation and scaling in 3d. #ifndef SOPHUS_RXSO3_HPP #define SOPHUS_RXSO3_HPP #include "so3.hpp" namespace Sophus { template class RxSO3; using RxSO3d = RxSO3; using RxSO3f = RxSO3; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using QuaternionType = Eigen::Quaternion; }; template struct traits, Options_>> : traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using QuaternionType = Map, Options>; }; template struct traits const, Options_>> : traits const> { static constexpr int Options = Options_; using Scalar = Scalar_; using QuaternionType = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// RxSO3 base type - implements RxSO3 class but is storage agnostic /// /// This class implements the group ``R+ x SO(3)``, the direct product of the /// group of positive scalar 3x3 matrices (= isomorph to the positive /// real numbers) and the three-dimensional special orthogonal group SO(3). /// Geometrically, it is the group of rotation and scaling in three dimensions. /// As a matrix groups, RxSO3 consists of matrices of the form ``s * R`` /// where ``R`` is an orthogonal matrix with ``det(R)=1`` and ``s > 0`` /// being a positive real number. /// /// Internally, RxSO3 is represented by the group of non-zero quaternions. /// In particular, the scale equals the squared(!) norm of the quaternion ``q``, /// ``s = |q|^2``. This is a most compact representation since the degrees of /// freedom (DoF) of RxSO3 (=4) equals the number of internal parameters (=4). /// /// This class has the explicit class invariant that the scale ``s`` is not /// too close to zero. Strictly speaking, it must hold that: /// /// ``quaternion().squaredNorm() >= Constants::epsilon()``. /// /// In order to obey this condition, group multiplication is implemented with /// saturation such that a product always has a scale which is equal or greater /// this threshold. template class RxSO3Base { public: static constexpr int Options = Eigen::internal::traits::Options; using Scalar = typename Eigen::internal::traits::Scalar; using QuaternionType = typename Eigen::internal::traits::QuaternionType; using QuaternionTemporaryType = Eigen::Quaternion; /// Degrees of freedom of manifold, number of dimensions in tangent space /// (three for rotation and one for scaling). static int constexpr DoF = 4; /// Number of internal parameters used (quaternion is a 4-tuple). static int constexpr num_parameters = 4; /// Group transformations are 3x3 matrices. static int constexpr N = 3; using Transformation = Matrix; using Point = Vector3; using HomogeneousPoint = Vector4; using Line = ParametrizedLine3; using Tangent = Vector; using Adjoint = Matrix; struct TangentAndTheta { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Tangent tangent; Scalar theta; }; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with RxSO3 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using RxSO3Product = RxSO3>; template using PointProduct = Vector3>; template using HomogeneousPointProduct = Vector4>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// /// For RxSO(3), it simply returns the rotation matrix corresponding to ``A``. /// SOPHUS_FUNC Adjoint Adj() const { Adjoint res; res.setIdentity(); res.template topLeftCorner<3, 3>() = rotationMatrix(); return res; } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC RxSO3 cast() const { return RxSO3(quaternion().template cast()); } /// This provides unsafe read/write access to internal data. RxSO(3) is /// represented by an Eigen::Quaternion (four parameters). When using direct /// write access, the user needs to take care of that the quaternion is not /// set close to zero. /// /// Note: The first three Scalars represent the imaginary parts, while the /// forth Scalar represent the real part. /// SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { return quaternion().coeffs().data(); } /// Returns group inverse. /// SOPHUS_FUNC RxSO3 inverse() const { return RxSO3(quaternion().inverse()); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (scaled rotation matrices) to elements of the tangent /// space (rotation-vector plus logarithm of scale factor). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of RxSO3. /// SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; } /// As above, but also returns ``theta = |omega|``. /// SOPHUS_FUNC TangentAndTheta logAndTheta() const { using std::log; Scalar scale = quaternion().squaredNorm(); TangentAndTheta result; result.tangent[3] = log(scale); auto omega_and_theta = SO3(quaternion()).logAndTheta(); result.tangent.template head<3>() = omega_and_theta.tangent; result.theta = omega_and_theta.theta; return result; } /// Returns 3x3 matrix representation of the instance. /// /// For RxSO3, the matrix representation is an scaled orthogonal matrix ``sR`` /// with ``det(R)=s^3``, thus a scaled rotation matrix ``R`` with scale /// ``s``. /// SOPHUS_FUNC Transformation matrix() const { Transformation sR; Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x(); Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y(); Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z(); Scalar const w_sq = quaternion().w() * quaternion().w(); Scalar const two_vx = Scalar(2) * quaternion().vec().x(); Scalar const two_vy = Scalar(2) * quaternion().vec().y(); Scalar const two_vz = Scalar(2) * quaternion().vec().z(); Scalar const two_vx_vy = two_vx * quaternion().vec().y(); Scalar const two_vx_vz = two_vx * quaternion().vec().z(); Scalar const two_vx_w = two_vx * quaternion().w(); Scalar const two_vy_vz = two_vy * quaternion().vec().z(); Scalar const two_vy_w = two_vy * quaternion().w(); Scalar const two_vz_w = two_vz * quaternion().w(); sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq; sR(1, 0) = two_vx_vy + two_vz_w; sR(2, 0) = two_vx_vz - two_vy_w; sR(0, 1) = two_vx_vy - two_vz_w; sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq; sR(2, 1) = two_vx_w + two_vy_vz; sR(0, 2) = two_vx_vz + two_vy_w; sR(1, 2) = -two_vx_w + two_vy_vz; sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq; return sR; } /// Assignment operator. /// SOPHUS_FUNC RxSO3Base& operator=(RxSO3Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC RxSO3Base& operator=( RxSO3Base const& other) { quaternion_nonconst() = other.quaternion(); return *this; } /// Group multiplication, which is rotation concatenation and scale /// multiplication. /// /// Note: This function performs saturation for products close to zero in /// order to ensure the class invariant. /// template SOPHUS_FUNC RxSO3Product operator*( RxSO3Base const& other) const { using ResultT = ReturnScalar; typename RxSO3Product::QuaternionType result_quaternion( quaternion() * other.quaternion()); ResultT scale = result_quaternion.squaredNorm(); if (scale < Constants::epsilon()) { SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero."); /// Saturation to ensure class invariant. result_quaternion.normalize(); result_quaternion.coeffs() *= sqrt(Constants::epsilon()); } return RxSO3Product(result_quaternion); } /// Group action on 3-points. /// /// This function rotates a 3 dimensional point ``p`` by the SO3 element /// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor /// ``s``: /// /// ``p_bar = s * (bar_R_foo * p_foo)``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { // Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459 Scalar scale = quaternion().squaredNorm(); PointProduct two_vec_cross_p = quaternion().vec().cross(p); two_vec_cross_p += two_vec_cross_p; return scale * p + (quaternion().w() * two_vec_cross_p + quaternion().vec().cross(two_vec_cross_p)); } /// Group action on homogeneous 3-points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const auto rsp = *this * p.template head<3>(); return HomogeneousPointProduct(rsp(0), rsp(1), rsp(2), p(3)); } /// Group action on lines. /// /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3 /// element ans scales it by the scale factor: /// /// Origin ``o`` is rotated and scaled /// Direction ``d`` is rotated (preserving it's norm) /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), (*this) * l.direction() / quaternion().squaredNorm()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO3's Scalar type. /// /// Note: This function performs saturation for products close to zero in /// order to ensure the class invariant. /// template >::value>::type> SOPHUS_FUNC RxSO3Base& operator*=( RxSO3Base const& other) { *static_cast(this) = *this * other; return *this; } /// Returns internal parameters of RxSO(3). /// /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the /// quaternion. /// SOPHUS_FUNC Sophus::Vector params() const { return quaternion().coeffs(); } /// Sets non-zero quaternion /// /// Precondition: ``quat`` must not be close to zero. SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quat) { SOPHUS_ENSURE(quat.squaredNorm() > Constants::epsilon() * Constants::epsilon(), "Scale factor must be greater-equal epsilon."); static_cast(this)->quaternion_nonconst() = quat; } /// Accessor of quaternion. /// SOPHUS_FUNC QuaternionType const& quaternion() const { return static_cast(this)->quaternion(); } /// Returns rotation matrix. /// SOPHUS_FUNC Transformation rotationMatrix() const { QuaternionTemporaryType norm_quad = quaternion(); norm_quad.normalize(); return norm_quad.toRotationMatrix(); } /// Returns scale. /// SOPHUS_FUNC Scalar scale() const { return quaternion().squaredNorm(); } /// Setter of quaternion using rotation matrix ``R``, leaves scale as is. /// SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { using std::sqrt; Scalar saved_scale = scale(); quaternion_nonconst() = R; quaternion_nonconst().coeffs() *= sqrt(saved_scale); } /// Sets scale and leaves rotation as is. /// /// Note: This function as a significant computational cost, since it has to /// call the square root twice. /// SOPHUS_FUNC void setScale(Scalar const& scale) { using std::sqrt; quaternion_nonconst().normalize(); quaternion_nonconst().coeffs() *= sqrt(scale); } /// Setter of quaternion using scaled rotation matrix ``sR``. /// /// Precondition: The 3x3 matrix must be "scaled orthogonal" /// and have a positive determinant. /// SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { Transformation squared_sR = sR * sR.transpose(); Scalar squared_scale = Scalar(1. / 3.) * (squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2)); SOPHUS_ENSURE(squared_scale >= Constants::epsilon() * Constants::epsilon(), "Scale factor must be greater-equal epsilon."); Scalar scale = sqrt(squared_scale); quaternion_nonconst() = sR / scale; quaternion_nonconst().coeffs() *= sqrt(scale); } /// Setter of SO(3) rotations, leaves scale as is. /// SOPHUS_FUNC void setSO3(SO3 const& so3) { using std::sqrt; Scalar saved_scale = scale(); quaternion_nonconst() = so3.unit_quaternion(); quaternion_nonconst().coeffs() *= sqrt(saved_scale); } SOPHUS_FUNC SO3 so3() const { return SO3(quaternion()); } protected: /// Mutator of quaternion is private to ensure class invariant. /// SOPHUS_FUNC QuaternionType& quaternion_nonconst() { return static_cast(this)->quaternion_nonconst(); } }; /// RxSO3 using storage; derived from RxSO3Base. template class RxSO3 : public RxSO3Base> { public: using Base = RxSO3Base>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using QuaternionMember = Eigen::Quaternion; /// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``. friend class RxSO3Base>; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes quaternion to identity rotation and scale /// to 1. /// SOPHUS_FUNC RxSO3() : quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {} /// Copy constructor /// SOPHUS_FUNC RxSO3(RxSO3 const& other) = default; /// Copy-like constructor from OtherDerived /// template SOPHUS_FUNC RxSO3(RxSO3Base const& other) : quaternion_(other.quaternion()) {} /// Constructor from scaled rotation matrix /// /// Precondition: rotation matrix need to be scaled orthogonal with /// determinant of ``s^3``. /// SOPHUS_FUNC explicit RxSO3(Transformation const& sR) { this->setScaledRotationMatrix(sR); } /// Constructor from scale factor and rotation matrix ``R``. /// /// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant /// of 1 and ``scale`` must not be close to zero. /// SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R) : quaternion_(R) { SOPHUS_ENSURE(scale >= Constants::epsilon(), "Scale factor must be greater-equal epsilon."); using std::sqrt; quaternion_.coeffs() *= sqrt(scale); } /// Constructor from scale factor and SO3 /// /// Precondition: ``scale`` must not to be close to zero. /// SOPHUS_FUNC RxSO3(Scalar const& scale, SO3 const& so3) : quaternion_(so3.unit_quaternion()) { SOPHUS_ENSURE(scale >= Constants::epsilon(), "Scale factor must be greater-equal epsilon."); using std::sqrt; quaternion_.coeffs() *= sqrt(scale); } /// Constructor from quaternion /// /// Precondition: quaternion must not be close to zero. /// template SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase const& quat) : quaternion_(quat) { static_assert(std::is_same::value, "must be same Scalar type."); SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants::epsilon(), "Scale factor must be greater-equal epsilon."); } /// Accessor of quaternion. /// SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Group exponential /// /// This functions takes in an element of tangent space (= rotation 3-vector /// plus logarithm of scale) and returns the corresponding element of the /// group RxSO3. /// /// To be more specific, thixs function computes ``expmat(hat(omega))`` /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the /// hat()-operator of RSO3. /// SOPHUS_FUNC static RxSO3 exp(Tangent const& a) { Scalar theta; return expAndTheta(a, &theta); } /// As above, but also returns ``theta = |omega|`` as out-parameter. /// /// Precondition: ``theta`` must not be ``nullptr``. /// SOPHUS_FUNC static RxSO3 expAndTheta(Tangent const& a, Scalar* theta) { SOPHUS_ENSURE(theta != nullptr, "must not be nullptr."); using std::exp; using std::sqrt; Vector3 const omega = a.template head<3>(); Scalar sigma = a[3]; Scalar sqrt_scale = sqrt(exp(sigma)); Eigen::Quaternion quat = SO3::expAndTheta(omega, theta).unit_quaternion(); quat.coeffs() *= sqrt_scale; return RxSO3(quat); } /// Returns the ith infinitesimal generators of ``R+ x SO(3)``. /// /// The infinitesimal generators of RxSO3 are: /// /// ``` /// | 0 0 0 | /// G_0 = | 0 0 -1 | /// | 0 1 0 | /// /// | 0 0 1 | /// G_1 = | 0 0 0 | /// | -1 0 0 | /// /// | 0 -1 0 | /// G_2 = | 1 0 0 | /// | 0 0 0 | /// /// | 1 0 0 | /// G_2 = | 0 1 0 | /// | 0 0 1 | /// ``` /// /// Precondition: ``i`` must be 0, 1, 2 or 3. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= 3, "i should be in range [0,3]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// hat-operator /// /// It takes in the 4-vector representation ``a`` (= rotation vector plus /// logarithm of scale) and returns the corresponding matrix representation /// of Lie algebra element. /// /// Formally, the hat()-operator of RxSO3 is defined as /// /// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2,3) /// /// with ``G_i`` being the ith infinitesimal generator of RxSO3. /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation A; // clang-format off A << a(3), -a(2), a(1), a(2), a(3), -a(0), -a(1), a(0), a(3); // clang-format on return A; } /// Lie bracket /// /// It computes the Lie bracket of RxSO(3). To be more specific, it computes /// /// ``[omega_1, omega_2]_rxso3 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of RxSO3. /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector3 const omega1 = a.template head<3>(); Vector3 const omega2 = b.template head<3>(); Vector4 res; res.template head<3>() = omega1.cross(omega2); res[3] = Scalar(0); return res; } /// Draw uniform sample from RxSO(3) manifold. /// /// The scale factor is drawn uniformly in log2-space from [-1, 1], /// hence the scale is in [0.5, 2]. /// template static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); using std::exp2; return RxSO3(exp2(uniform(generator)), SO3::sampleUniform(generator)); } /// vee-operator /// /// It takes the 3x3-matrix representation ``Omega`` and maps it to the /// corresponding vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | d -c b | /// | c d -a | /// | -b a d | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { using std::abs; return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0)); } protected: SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quaternion_; } QuaternionMember quaternion_; }; } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``RxSO3``; derived from RxSO3Base /// /// Allows us to wrap RxSO3 objects around POD array (e.g. external c style /// quaternion). template class Map, Options> : public Sophus::RxSO3Base, Options>> { public: using Base = Sophus::RxSO3Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; /// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``. friend class Sophus::RxSO3Base, Options>>; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map); // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {} /// Accessor of quaternion. /// SOPHUS_FUNC Map, Options> const& quaternion() const { return quaternion_; } protected: SOPHUS_FUNC Map, Options>& quaternion_nonconst() { return quaternion_; } Map, Options> quaternion_; }; /// Specialization of Eigen::Map for ``RxSO3 const``; derived from RxSO3Base. /// /// Allows us to wrap RxSO3 objects around POD array (e.g. external c style /// quaternion). template class Map const, Options> : public Sophus::RxSO3Base const, Options>> { public: using Base = Sophus::RxSO3Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : quaternion_(coeffs) {} /// Accessor of quaternion. /// SOPHUS_FUNC Map const, Options> const& quaternion() const { return quaternion_; } protected: Map const, Options> const quaternion_; }; } // namespace Eigen #endif /// SOPHUS_RXSO3_HPP ================================================ FILE: thirdparty/Sophus/se2.hpp ================================================ /// @file /// Special Euclidean group SE(2) - rotation and translation in 2d. #ifndef SOPHUS_SE2_HPP #define SOPHUS_SE2_HPP #include "so2.hpp" namespace Sophus { template class SE2; using SE2d = SE2; using SE2f = SE2; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { using Scalar = Scalar_; using TranslationType = Sophus::Vector2; using SO2Type = Sophus::SO2; }; template struct traits, Options>> : traits> { using Scalar = Scalar_; using TranslationType = Map, Options>; using SO2Type = Map, Options>; }; template struct traits const, Options>> : traits const> { using Scalar = Scalar_; using TranslationType = Map const, Options>; using SO2Type = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// SE2 base type - implements SE2 class but is storage agnostic. /// /// SE(2) is the group of rotations and translation in 2d. It is the /// semi-direct product of SO(2) and the 2d Euclidean vector space. The class /// is represented using a composition of SO2Group for rotation and a 2-vector /// for translation. /// /// SE(2) is neither compact, nor a commutative group. /// /// See SO2Group for more details of the rotation representation in 2d. /// template class SE2Base { public: using Scalar = typename Eigen::internal::traits::Scalar; using TranslationType = typename Eigen::internal::traits::TranslationType; using SO2Type = typename Eigen::internal::traits::SO2Type; /// Degrees of freedom of manifold, number of dimensions in tangent space /// (two for translation, three for rotation). static int constexpr DoF = 3; /// Number of internal parameters used (tuple for complex, two for /// translation). static int constexpr num_parameters = 4; /// Group transformations are 3x3 matrices. static int constexpr N = 3; using Transformation = Matrix; using Point = Vector2; using HomogeneousPoint = Vector3; using Line = ParametrizedLine2; using Tangent = Vector; using Adjoint = Matrix; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with SE2 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using SE2Product = SE2>; template using PointProduct = Vector2>; template using HomogeneousPointProduct = Vector3>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// SOPHUS_FUNC Adjoint Adj() const { Matrix const& R = so2().matrix(); Transformation res; res.setIdentity(); res.template topLeftCorner<2, 2>() = R; res(0, 2) = translation()[1]; res(1, 2) = -translation()[0]; return res; } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC SE2 cast() const { return SE2(so2().template cast(), translation().template cast()); } /// Returns derivative of this * exp(x) wrt x at x=0. /// SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() const { Matrix J; Sophus::Vector2 const c = unit_complex(); Scalar o(0); J(0, 0) = o; J(0, 1) = o; J(0, 2) = -c[1]; J(1, 0) = o; J(1, 1) = o; J(1, 2) = c[0]; J(2, 0) = c[0]; J(2, 1) = -c[1]; J(2, 2) = o; J(3, 0) = c[1]; J(3, 1) = c[0]; J(3, 2) = o; return J; } /// Returns group inverse. /// SOPHUS_FUNC SE2 inverse() const { SO2 const invR = so2().inverse(); return SE2(invR, invR * (translation() * Scalar(-1))); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rigid body transformations) to elements of the /// tangent space (twist). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of SE(2). /// SOPHUS_FUNC Tangent log() const { using std::abs; Tangent upsilon_theta; Scalar theta = so2().log(); upsilon_theta[2] = theta; Scalar halftheta = Scalar(0.5) * theta; Scalar halftheta_by_tan_of_halftheta; Vector2 z = so2().unit_complex(); Scalar real_minus_one = z.x() - Scalar(1.); if (abs(real_minus_one) < Constants::epsilon()) { halftheta_by_tan_of_halftheta = Scalar(1.) - Scalar(1. / 12) * theta * theta; } else { halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one); } Matrix V_inv; V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta, halftheta_by_tan_of_halftheta; upsilon_theta.template head<2>() = V_inv * translation(); return upsilon_theta; } /// Normalize SO2 element /// /// It re-normalizes the SO2 element. /// SOPHUS_FUNC void normalize() { so2().normalize(); } /// Returns 3x3 matrix representation of the instance. /// /// It has the following form: /// /// | R t | /// | o 1 | /// /// where ``R`` is a 2x2 rotation matrix, ``t`` a translation 2-vector and /// ``o`` a 2-column vector of zeros. /// SOPHUS_FUNC Transformation matrix() const { Transformation homogenious_matrix; homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3(); homogenious_matrix.row(2) = Matrix(Scalar(0), Scalar(0), Scalar(1)); return homogenious_matrix; } /// Returns the significant first two rows of the matrix above. /// SOPHUS_FUNC Matrix matrix2x3() const { Matrix matrix; matrix.template topLeftCorner<2, 2>() = rotationMatrix(); matrix.col(2) = translation(); return matrix; } /// Assignment operator. /// SOPHUS_FUNC SE2Base& operator=(SE2Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC SE2Base& operator=(SE2Base const& other) { so2() = other.so2(); translation() = other.translation(); return *this; } /// Group multiplication, which is rotation concatenation. /// template SOPHUS_FUNC SE2Product operator*( SE2Base const& other) const { return SE2Product( so2() * other.so2(), translation() + so2() * other.translation()); } /// Group action on 2-points. /// /// This function rotates and translates a two dimensional point ``p`` by the /// SE(2) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body /// transformation): /// /// ``p_bar = bar_R_foo * p_foo + t_bar``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return so2() * p + translation(); } /// Group action on homogeneous 2-points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = so2() * p.template head<2>() + p(2) * translation(); return HomogeneousPointProduct(tp(0), tp(1), p(2)); } /// Group action on lines. /// /// This function rotates and translates a parametrized line /// ``l(t) = o + t * d`` by the SE(2) element: /// /// Origin ``o`` is rotated and translated using SE(2) action /// Direction ``d`` is rotated using SO(2) action /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), so2() * l.direction()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO2's Scalar type. /// template >::value>::type> SOPHUS_FUNC SE2Base& operator*=(SE2Base const& other) { *static_cast(this) = *this * other; return *this; } /// Returns internal parameters of SE(2). /// /// It returns (c[0], c[1], t[0], t[1]), /// with c being the unit complex number, t the translation 3-vector. /// SOPHUS_FUNC Sophus::Vector params() const { Sophus::Vector p; p << so2().params(), translation(); return p; } /// Returns rotation matrix. /// SOPHUS_FUNC Matrix rotationMatrix() const { return so2().matrix(); } /// Takes in complex number, and normalizes it. /// /// Precondition: The complex number must not be close to zero. /// SOPHUS_FUNC void setComplex(Sophus::Vector2 const& complex) { return so2().setComplex(complex); } /// Sets ``so3`` using ``rotation_matrix``. /// /// Precondition: ``R`` must be orthogonal and ``det(R)=1``. /// SOPHUS_FUNC void setRotationMatrix(Matrix const& R) { SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", R.determinant()); typename SO2Type::ComplexTemporaryType const complex( Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1))); so2().setComplex(complex); } /// Mutator of SO3 group. /// SOPHUS_FUNC SO2Type& so2() { return static_cast(this)->so2(); } /// Accessor of SO3 group. /// SOPHUS_FUNC SO2Type const& so2() const { return static_cast(this)->so2(); } /// Mutator of translation vector. /// SOPHUS_FUNC TranslationType& translation() { return static_cast(this)->translation(); } /// Accessor of translation vector /// SOPHUS_FUNC TranslationType const& translation() const { return static_cast(this)->translation(); } /// Accessor of unit complex number. /// SOPHUS_FUNC typename Eigen::internal::traits::SO2Type::ComplexT const& unit_complex() const { return so2().unit_complex(); } }; /// SE2 using default storage; derived from SE2Base. template class SE2 : public SE2Base> { public: using Base = SE2Base>; static int constexpr DoF = Base::DoF; static int constexpr num_parameters = Base::num_parameters; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using SO2Member = SO2; using TranslationMember = Vector2; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes rigid body motion to the identity. /// SOPHUS_FUNC SE2(); /// Copy constructor /// SOPHUS_FUNC SE2(SE2 const& other) = default; /// Copy-like constructor from OtherDerived /// template SOPHUS_FUNC SE2(SE2Base const& other) : so2_(other.so2()), translation_(other.translation()) { static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from SO3 and translation vector /// template SOPHUS_FUNC SE2(SO2Base const& so2, Eigen::MatrixBase const& translation) : so2_(so2), translation_(translation) { static_assert(std::is_same::value, "must be same Scalar type"); static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from rotation matrix and translation vector /// /// Precondition: Rotation matrix needs to be orthogonal with determinant /// of 1. /// SOPHUS_FUNC SE2(typename SO2::Transformation const& rotation_matrix, Point const& translation) : so2_(rotation_matrix), translation_(translation) {} /// Constructor from rotation angle and translation vector. /// SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation) : so2_(theta), translation_(translation) {} /// Constructor from complex number and translation vector /// /// Precondition: ``complex`` must not be close to zero. SOPHUS_FUNC SE2(Vector2 const& complex, Point const& translation) : so2_(complex), translation_(translation) {} /// Constructor from 3x3 matrix /// /// Precondition: Rotation matrix needs to be orthogonal with determinant /// of 1. The last row must be ``(0, 0, 1)``. /// SOPHUS_FUNC explicit SE2(Transformation const& T) : so2_(T.template topLeftCorner<2, 2>().eval()), translation_(T.template block<2, 1>(0, 2)) {} /// This provides unsafe read/write access to internal data. SO(2) is /// represented by a complex number (two parameters). When using direct write /// access, the user needs to take care of that the complex number stays /// normalized. /// SOPHUS_FUNC Scalar* data() { // so2_ and translation_ are layed out sequentially with no padding return so2_.data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { /// so2_ and translation_ are layed out sequentially with no padding return so2_.data(); } /// Accessor of SO3 /// SOPHUS_FUNC SO2Member& so2() { return so2_; } /// Mutator of SO3 /// SOPHUS_FUNC SO2Member const& so2() const { return so2_; } /// Mutator of translation vector /// SOPHUS_FUNC TranslationMember& translation() { return translation_; } /// Accessor of translation vector /// SOPHUS_FUNC TranslationMember const& translation() const { return translation_; } /// Returns derivative of exp(x) wrt. x. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( Tangent const& upsilon_theta) { using std::abs; using std::cos; using std::pow; using std::sin; Sophus::Matrix J; Sophus::Vector upsilon = upsilon_theta.template head<2>(); Scalar theta = upsilon_theta[2]; if (abs(theta) < Constants::epsilon()) { Scalar const o(0); Scalar const i(1); // clang-format off J << o, o, o, o, o, i, i, o, -Scalar(0.5) * upsilon[1], o, i, Scalar(0.5) * upsilon[0]; // clang-format on return J; } Scalar const c0 = sin(theta); Scalar const c1 = cos(theta); Scalar const c2 = 1.0 / theta; Scalar const c3 = c0 * c2; Scalar const c4 = -c1 + Scalar(1); Scalar const c5 = c2 * c4; Scalar const c6 = c1 * c2; Scalar const c7 = pow(theta, -2); Scalar const c8 = c0 * c7; Scalar const c9 = c4 * c7; Scalar const o = Scalar(0); J(0, 0) = o; J(0, 1) = o; J(0, 2) = -c0; J(1, 0) = o; J(1, 1) = o; J(1, 2) = c1; J(2, 0) = c3; J(2, 1) = -c5; J(2, 2) = -c3 * upsilon[1] + c6 * upsilon[0] - c8 * upsilon[0] + c9 * upsilon[1]; J(3, 0) = c5; J(3, 1) = c3; J(3, 2) = c3 * upsilon[0] + c6 * upsilon[1] - c8 * upsilon[1] - c9 * upsilon[0]; return J; } /// Returns derivative of exp(x) wrt. x_i at x=0. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_at_0() { Sophus::Matrix J; Scalar const o(0); Scalar const i(1); // clang-format off J << o, o, o, o, o, i, i, o, o, o, i, o; // clang-format on return J; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Group exponential /// /// This functions takes in an element of tangent space (= twist ``a``) and /// returns the corresponding element of the group SE(2). /// /// The first two components of ``a`` represent the translational part /// ``upsilon`` in the tangent space of SE(2), while the last three components /// of ``a`` represents the rotation vector ``omega``. /// To be more specific, this function computes ``expmat(hat(a))`` with /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator /// of SE(2), see below. /// SOPHUS_FUNC static SE2 exp(Tangent const& a) { Scalar theta = a[2]; SO2 so2 = SO2::exp(theta); Scalar sin_theta_by_theta; Scalar one_minus_cos_theta_by_theta; using std::abs; if (abs(theta) < Constants::epsilon()) { Scalar theta_sq = theta * theta; sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq; one_minus_cos_theta_by_theta = Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq; } else { sin_theta_by_theta = so2.unit_complex().y() / theta; one_minus_cos_theta_by_theta = (Scalar(1.) - so2.unit_complex().x()) / theta; } Vector2 trans( sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1], one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]); return SE2(so2, trans); } /// Returns closest SE3 given arbitrary 4x4 matrix. /// template static SOPHUS_FUNC enable_if_t::value, SE2> fitToSE2(Matrix3 const& T) { return SE2(SO2::fitToSO2(T.template block<2, 2>(0, 0)), T.template block<2, 1>(0, 2)); } /// Returns the ith infinitesimal generators of SE(2). /// /// The infinitesimal generators of SE(2) are: /// /// ``` /// | 0 0 1 | /// G_0 = | 0 0 0 | /// | 0 0 0 | /// /// | 0 0 0 | /// G_1 = | 0 0 1 | /// | 0 0 0 | /// /// | 0 -1 0 | /// G_2 = | 1 0 0 | /// | 0 0 0 | /// ``` /// /// Precondition: ``i`` must be in 0, 1 or 2. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// hat-operator /// /// It takes in the 3-vector representation (= twist) and returns the /// corresponding matrix representation of Lie algebra element. /// /// Formally, the hat()-operator of SE(3) is defined as /// /// ``hat(.): R^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2) /// /// with ``G_i`` being the ith infinitesimal generator of SE(2). /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; Omega.setZero(); Omega.template topLeftCorner<2, 2>() = SO2::hat(a[2]); Omega.col(2).template head<2>() = a.template head<2>(); return Omega; } /// Lie bracket /// /// It computes the Lie bracket of SE(2). To be more specific, it computes /// /// ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of SE(2). /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector2 upsilon1 = a.template head<2>(); Vector2 upsilon2 = b.template head<2>(); Scalar theta1 = a[2]; Scalar theta2 = b[2]; return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1], theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0)); } /// Construct pure rotation. /// static SOPHUS_FUNC SE2 rot(Scalar const& x) { return SE2(SO2(x), Sophus::Vector2::Zero()); } /// Draw uniform sample from SE(2) manifold. /// /// Translations are drawn component-wise from the range [-1, 1]. /// template static SE2 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); return SE2(SO2::sampleUniform(generator), Vector2(uniform(generator), uniform(generator))); } /// Construct a translation only SE(2) instance. /// template static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) { return SE2(SO2(), Vector2(x, y)); } static SOPHUS_FUNC SE2 trans(Vector2 const& xy) { return SE2(SO2(), xy); } /// Construct x-axis translation. /// static SOPHUS_FUNC SE2 transX(Scalar const& x) { return SE2::trans(x, Scalar(0)); } /// Construct y-axis translation. /// static SOPHUS_FUNC SE2 transY(Scalar const& y) { return SE2::trans(Scalar(0), y); } /// vee-operator /// /// It takes the 3x3-matrix representation ``Omega`` and maps it to the /// corresponding 3-vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | 0 -d a | /// | d 0 b | /// | 0 0 0 | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { SOPHUS_ENSURE( Omega.row(2).template lpNorm<1>() < Constants::epsilon(), "Omega: \n%", Omega); Tangent upsilon_omega; upsilon_omega.template head<2>() = Omega.col(2).template head<2>(); upsilon_omega[2] = SO2::vee(Omega.template topLeftCorner<2, 2>()); return upsilon_omega; } protected: SO2Member so2_; TranslationMember translation_; }; template SE2::SE2() : translation_(TranslationMember::Zero()) { static_assert(std::is_standard_layout::value, "Assume standard layout for the use of offsetof check below."); static_assert( offsetof(SE2, so2_) + sizeof(Scalar) * SO2::num_parameters == offsetof(SE2, translation_), "This class assumes packed storage and hence will only work " "correctly depending on the compiler (options) - in " "particular when using [this->data(), this-data() + " "num_parameters] to access the raw data in a contiguous fashion."); } } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``SE2``; derived from SE2Base. /// /// Allows us to wrap SE2 objects around POD array. template class Map, Options> : public Sophus::SE2Base, Options>> { public: using Base = Sophus::SE2Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map) // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : so2_(coeffs), translation_(coeffs + Sophus::SO2::num_parameters) {} /// Mutator of SO3 /// SOPHUS_FUNC Map, Options>& so2() { return so2_; } /// Accessor of SO3 /// SOPHUS_FUNC Map, Options> const& so2() const { return so2_; } /// Mutator of translation vector /// SOPHUS_FUNC Map, Options>& translation() { return translation_; } /// Accessor of translation vector /// SOPHUS_FUNC Map, Options> const& translation() const { return translation_; } protected: Map, Options> so2_; Map, Options> translation_; }; /// Specialization of Eigen::Map for ``SE2 const``; derived from SE2Base. /// /// Allows us to wrap SE2 objects around POD array. template class Map const, Options> : public Sophus::SE2Base const, Options>> { public: using Base = Sophus::SE2Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : so2_(coeffs), translation_(coeffs + Sophus::SO2::num_parameters) {} /// Accessor of SO3 /// SOPHUS_FUNC Map const, Options> const& so2() const { return so2_; } /// Accessor of translation vector /// SOPHUS_FUNC Map const, Options> const& translation() const { return translation_; } protected: Map const, Options> const so2_; Map const, Options> const translation_; }; } // namespace Eigen #endif ================================================ FILE: thirdparty/Sophus/se3.hpp ================================================ /// @file /// Special Euclidean group SE(3) - rotation and translation in 3d. #ifndef SOPHUS_SE3_HPP #define SOPHUS_SE3_HPP #include "so3.hpp" namespace Sophus { template class SE3; using SE3d = SE3; using SE3f = SE3; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { using Scalar = Scalar_; using TranslationType = Sophus::Vector3; using SO3Type = Sophus::SO3; }; template struct traits, Options>> : traits> { using Scalar = Scalar_; using TranslationType = Map, Options>; using SO3Type = Map, Options>; }; template struct traits const, Options>> : traits const> { using Scalar = Scalar_; using TranslationType = Map const, Options>; using SO3Type = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// SE3 base type - implements SE3 class but is storage agnostic. /// /// SE(3) is the group of rotations and translation in 3d. It is the /// semi-direct product of SO(3) and the 3d Euclidean vector space. The class /// is represented using a composition of SO3 for rotation and a one 3-vector /// for translation. /// /// SE(3) is neither compact, nor a commutative group. /// /// See SO3 for more details of the rotation representation in 3d. /// template class SE3Base { public: using Scalar = typename Eigen::internal::traits::Scalar; using TranslationType = typename Eigen::internal::traits::TranslationType; using SO3Type = typename Eigen::internal::traits::SO3Type; using QuaternionType = typename SO3Type::QuaternionType; /// Degrees of freedom of manifold, number of dimensions in tangent space /// (three for translation, three for rotation). static int constexpr DoF = 6; /// Number of internal parameters used (4-tuple for quaternion, three for /// translation). static int constexpr num_parameters = 7; /// Group transformations are 4x4 matrices. static int constexpr N = 4; using Transformation = Matrix; using Point = Vector3; using HomogeneousPoint = Vector4; using Line = ParametrizedLine3; using Tangent = Vector; using Adjoint = Matrix; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with SE3 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using SE3Product = SE3>; template using PointProduct = Vector3>; template using HomogeneousPointProduct = Vector4>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// SOPHUS_FUNC Adjoint Adj() const { Sophus::Matrix3 const R = so3().matrix(); Adjoint res; res.block(0, 0, 3, 3) = R; res.block(3, 3, 3, 3) = R; res.block(0, 3, 3, 3) = SO3::hat(translation()) * R; res.block(3, 0, 3, 3) = Matrix3::Zero(3, 3); return res; } /// Extract rotation angle about canonical X-axis /// Scalar angleX() const { return so3().angleX(); } /// Extract rotation angle about canonical Y-axis /// Scalar angleY() const { return so3().angleY(); } /// Extract rotation angle about canonical Z-axis /// Scalar angleZ() const { return so3().angleZ(); } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC SE3 cast() const { return SE3(so3().template cast(), translation().template cast()); } /// Returns derivative of this * exp(x) wrt x at x=0. /// SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() const { Matrix J; Eigen::Quaternion const q = unit_quaternion(); Scalar const c0 = Scalar(0.5) * q.w(); Scalar const c1 = Scalar(0.5) * q.z(); Scalar const c2 = -c1; Scalar const c3 = Scalar(0.5) * q.y(); Scalar const c4 = Scalar(0.5) * q.x(); Scalar const c5 = -c4; Scalar const c6 = -c3; Scalar const c7 = q.w() * q.w(); Scalar const c8 = q.x() * q.x(); Scalar const c9 = q.y() * q.y(); Scalar const c10 = -c9; Scalar const c11 = q.z() * q.z(); Scalar const c12 = -c11; Scalar const c13 = Scalar(2) * q.w(); Scalar const c14 = c13 * q.z(); Scalar const c15 = Scalar(2) * q.x(); Scalar const c16 = c15 * q.y(); Scalar const c17 = c13 * q.y(); Scalar const c18 = c15 * q.z(); Scalar const c19 = c7 - c8; Scalar const c20 = c13 * q.x(); Scalar const c21 = Scalar(2) * q.y() * q.z(); J(0, 0) = 0; J(0, 1) = 0; J(0, 2) = 0; J(0, 3) = c0; J(0, 4) = c2; J(0, 5) = c3; J(1, 0) = 0; J(1, 1) = 0; J(1, 2) = 0; J(1, 3) = c1; J(1, 4) = c0; J(1, 5) = c5; J(2, 0) = 0; J(2, 1) = 0; J(2, 2) = 0; J(2, 3) = c6; J(2, 4) = c4; J(2, 5) = c0; J(3, 0) = 0; J(3, 1) = 0; J(3, 2) = 0; J(3, 3) = c5; J(3, 4) = c6; J(3, 5) = c2; J(4, 0) = c10 + c12 + c7 + c8; J(4, 1) = -c14 + c16; J(4, 2) = c17 + c18; J(4, 3) = 0; J(4, 4) = 0; J(4, 5) = 0; J(5, 0) = c14 + c16; J(5, 1) = c12 + c19 + c9; J(5, 2) = -c20 + c21; J(5, 3) = 0; J(5, 4) = 0; J(5, 5) = 0; J(6, 0) = -c17 + c18; J(6, 1) = c20 + c21; J(6, 2) = c10 + c11 + c19; J(6, 3) = 0; J(6, 4) = 0; J(6, 5) = 0; return J; } /// Returns group inverse. /// SOPHUS_FUNC SE3 inverse() const { SO3 invR = so3().inverse(); return SE3(invR, invR * (translation() * Scalar(-1))); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rigid body transformations) to elements of the /// tangent space (twist). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of SE(3). /// SOPHUS_FUNC Tangent log() const { // For the derivation of the logarithm of SE(3), see // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices // and logarithms of orthogonal matrices", IJRA 2002. // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf // (Sec. 6., pp. 8) using std::abs; using std::cos; using std::sin; Tangent upsilon_omega; auto omega_and_theta = so3().logAndTheta(); Scalar theta = omega_and_theta.theta; upsilon_omega.template tail<3>() = omega_and_theta.tangent; Matrix3 const Omega = SO3::hat(upsilon_omega.template tail<3>()); if (abs(theta) < Constants::epsilon()) { Matrix3 const V_inv = Matrix3::Identity() - Scalar(0.5) * Omega + Scalar(1. / 12.) * (Omega * Omega); upsilon_omega.template head<3>() = V_inv * translation(); } else { Scalar const half_theta = Scalar(0.5) * theta; Matrix3 const V_inv = (Matrix3::Identity() - Scalar(0.5) * Omega + (Scalar(1) - theta * cos(half_theta) / (Scalar(2) * sin(half_theta))) / (theta * theta) * (Omega * Omega)); upsilon_omega.template head<3>() = V_inv * translation(); } return upsilon_omega; } /// It re-normalizes the SO3 element. /// /// Note: Because of the class invariant of SO3, there is typically no need to /// call this function directly. /// SOPHUS_FUNC void normalize() { so3().normalize(); } /// Returns 4x4 matrix representation of the instance. /// /// It has the following form: /// /// | R t | /// | o 1 | /// /// where ``R`` is a 3x3 rotation matrix, ``t`` a translation 3-vector and /// ``o`` a 3-column vector of zeros. /// SOPHUS_FUNC Transformation matrix() const { Transformation homogenious_matrix; homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4(); homogenious_matrix.row(3) = Matrix(Scalar(0), Scalar(0), Scalar(0), Scalar(1)); return homogenious_matrix; } /// Returns the significant first three rows of the matrix above. /// SOPHUS_FUNC Matrix matrix3x4() const { Matrix matrix; matrix.template topLeftCorner<3, 3>() = rotationMatrix(); matrix.col(3) = translation(); return matrix; } /// Assignment operator. /// SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) { so3() = other.so3(); translation() = other.translation(); return *this; } /// Group multiplication, which is rotation concatenation. /// template SOPHUS_FUNC SE3Product operator*( SE3Base const& other) const { return SE3Product( so3() * other.so3(), translation() + so3() * other.translation()); } /// Group action on 3-points. /// /// This function rotates and translates a three dimensional point ``p`` by /// the SE(3) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body /// transformation): /// /// ``p_bar = bar_R_foo * p_foo + t_bar``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return so3() * p + translation(); } /// Group action on homogeneous 3-points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = so3() * p.template head<3>() + p(3) * translation(); return HomogeneousPointProduct(tp(0), tp(1), tp(2), p(3)); } /// Group action on lines. /// /// This function rotates and translates a parametrized line /// ``l(t) = o + t * d`` by the SE(3) element: /// /// Origin is transformed using SE(3) action /// Direction is transformed using rotation part /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), so3() * l.direction()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SE3's Scalar type. /// template >::value>::type> SOPHUS_FUNC SE3Base& operator*=(SE3Base const& other) { *static_cast(this) = *this * other; return *this; } /// Returns rotation matrix. /// SOPHUS_FUNC Matrix3 rotationMatrix() const { return so3().matrix(); } /// Mutator of SO3 group. /// SOPHUS_FUNC SO3Type& so3() { return static_cast(this)->so3(); } /// Accessor of SO3 group. /// SOPHUS_FUNC SO3Type const& so3() const { return static_cast(this)->so3(); } /// Takes in quaternion, and normalizes it. /// /// Precondition: The quaternion must not be close to zero. /// SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quat) { so3().setQuaternion(quat); } /// Sets ``so3`` using ``rotation_matrix``. /// /// Precondition: ``R`` must be orthogonal and ``det(R)=1``. /// SOPHUS_FUNC void setRotationMatrix(Matrix3 const& R) { SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", R.determinant()); so3().setQuaternion(Eigen::Quaternion(R)); } /// Returns internal parameters of SE(3). /// /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]), /// with q being the unit quaternion, t the translation 3-vector. /// SOPHUS_FUNC Sophus::Vector params() const { Sophus::Vector p; p << so3().params(), translation(); return p; } /// Mutator of translation vector. /// SOPHUS_FUNC TranslationType& translation() { return static_cast(this)->translation(); } /// Accessor of translation vector /// SOPHUS_FUNC TranslationType const& translation() const { return static_cast(this)->translation(); } /// Accessor of unit quaternion. /// SOPHUS_FUNC QuaternionType const& unit_quaternion() const { return this->so3().unit_quaternion(); } }; /// SE3 using default storage; derived from SE3Base. template class SE3 : public SE3Base> { using Base = SE3Base>; public: static int constexpr DoF = Base::DoF; static int constexpr num_parameters = Base::num_parameters; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using SO3Member = SO3; using TranslationMember = Vector3; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes rigid body motion to the identity. /// SOPHUS_FUNC SE3(); /// Copy constructor /// SOPHUS_FUNC SE3(SE3 const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC SE3(SE3Base const& other) : so3_(other.so3()), translation_(other.translation()) { static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from SO3 and translation vector /// template SOPHUS_FUNC SE3(SO3Base const& so3, Eigen::MatrixBase const& translation) : so3_(so3), translation_(translation) { static_assert(std::is_same::value, "must be same Scalar type"); static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from rotation matrix and translation vector /// /// Precondition: Rotation matrix needs to be orthogonal with determinant /// of 1. /// SOPHUS_FUNC SE3(Matrix3 const& rotation_matrix, Point const& translation) : so3_(rotation_matrix), translation_(translation) {} /// Constructor from quaternion and translation vector. /// /// Precondition: ``quaternion`` must not be close to zero. /// SOPHUS_FUNC SE3(Eigen::Quaternion const& quaternion, Point const& translation) : so3_(quaternion), translation_(translation) {} /// Constructor from 4x4 matrix /// /// Precondition: Rotation matrix needs to be orthogonal with determinant /// of 1. The last row must be ``(0, 0, 0, 1)``. /// SOPHUS_FUNC explicit SE3(Matrix4 const& T) : so3_(T.template topLeftCorner<3, 3>()), translation_(T.template block<3, 1>(0, 3)) { SOPHUS_ENSURE((T.row(3) - Matrix(Scalar(0), Scalar(0), Scalar(0), Scalar(1))) .squaredNorm() < Constants::epsilon(), "Last row is not (0,0,0,1), but (%).", T.row(3)); } /// This provides unsafe read/write access to internal data. SO(3) is /// represented by an Eigen::Quaternion (four parameters). When using direct /// write access, the user needs to take care of that the quaternion stays /// normalized. /// SOPHUS_FUNC Scalar* data() { // so3_ and translation_ are laid out sequentially with no padding return so3_.data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { // so3_ and translation_ are laid out sequentially with no padding return so3_.data(); } /// Accessor of SO3 /// SOPHUS_FUNC SO3Member& so3() { return so3_; } /// Mutator of SO3 /// SOPHUS_FUNC SO3Member const& so3() const { return so3_; } /// Mutator of translation vector /// SOPHUS_FUNC TranslationMember& translation() { return translation_; } /// Accessor of translation vector /// SOPHUS_FUNC TranslationMember const& translation() const { return translation_; } /// Returns derivative of exp(x) wrt. x. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( Tangent const& upsilon_omega) { using std::cos; using std::pow; using std::sin; using std::sqrt; Sophus::Matrix J; Sophus::Vector upsilon = upsilon_omega.template head<3>(); Sophus::Vector omega = upsilon_omega.template tail<3>(); Scalar const c0 = omega[0] * omega[0]; Scalar const c1 = omega[1] * omega[1]; Scalar const c2 = omega[2] * omega[2]; Scalar const c3 = c0 + c1 + c2; Scalar const o(0); Scalar const h(0.5); Scalar const i(1); if (c3 < Constants::epsilon()) { Scalar const ux = Scalar(0.5) * upsilon[0]; Scalar const uy = Scalar(0.5) * upsilon[1]; Scalar const uz = Scalar(0.5) * upsilon[2]; /// clang-format off J << o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, i, o, o, o, uz, -uy, o, i, o, -uz, o, ux, o, o, i, uy, -ux, o; /// clang-format on return J; } Scalar const c4 = sqrt(c3); Scalar const c5 = Scalar(1.0) / c4; Scalar const c6 = Scalar(0.5) * c4; Scalar const c7 = sin(c6); Scalar const c8 = c5 * c7; Scalar const c9 = pow(c3, -3.0L / 2.0L); Scalar const c10 = c7 * c9; Scalar const c11 = Scalar(1.0) / c3; Scalar const c12 = cos(c6); Scalar const c13 = Scalar(0.5) * c11 * c12; Scalar const c14 = c7 * c9 * omega[0]; Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0]; Scalar const c16 = -c14 * omega[1] + c15 * omega[1]; Scalar const c17 = -c14 * omega[2] + c15 * omega[2]; Scalar const c18 = omega[1] * omega[2]; Scalar const c19 = -c10 * c18 + c13 * c18; Scalar const c20 = c5 * omega[0]; Scalar const c21 = Scalar(0.5) * c7; Scalar const c22 = c5 * omega[1]; Scalar const c23 = c5 * omega[2]; Scalar const c24 = -c1; Scalar const c25 = -c2; Scalar const c26 = c24 + c25; Scalar const c27 = sin(c4); Scalar const c28 = -c27 + c4; Scalar const c29 = c28 * c9; Scalar const c30 = cos(c4); Scalar const c31 = -c30 + Scalar(1); Scalar const c32 = c11 * c31; Scalar const c33 = c32 * omega[2]; Scalar const c34 = c29 * omega[0]; Scalar const c35 = c34 * omega[1]; Scalar const c36 = c32 * omega[1]; Scalar const c37 = c34 * omega[2]; Scalar const c38 = pow(c3, -5.0L / 2.0L); Scalar const c39 = Scalar(3) * c28 * c38 * omega[0]; Scalar const c40 = c26 * c9; Scalar const c41 = -c20 * c30 + c20; Scalar const c42 = c27 * c9 * omega[0]; Scalar const c43 = c42 * omega[1]; Scalar const c44 = pow(c3, -2); Scalar const c45 = Scalar(2) * c31 * c44 * omega[0]; Scalar const c46 = c45 * omega[1]; Scalar const c47 = c29 * omega[2]; Scalar const c48 = c43 - c46 + c47; Scalar const c49 = Scalar(3) * c0 * c28 * c38; Scalar const c50 = c9 * omega[0] * omega[2]; Scalar const c51 = c41 * c50 - c49 * omega[2]; Scalar const c52 = c9 * omega[0] * omega[1]; Scalar const c53 = c41 * c52 - c49 * omega[1]; Scalar const c54 = c42 * omega[2]; Scalar const c55 = c45 * omega[2]; Scalar const c56 = c29 * omega[1]; Scalar const c57 = -c54 + c55 + c56; Scalar const c58 = Scalar(-2) * c56; Scalar const c59 = Scalar(3) * c28 * c38 * omega[1]; Scalar const c60 = -c22 * c30 + c22; Scalar const c61 = -c18 * c39; Scalar const c62 = c32 + c61; Scalar const c63 = c27 * c9; Scalar const c64 = c1 * c63; Scalar const c65 = Scalar(2) * c31 * c44; Scalar const c66 = c1 * c65; Scalar const c67 = c50 * c60; Scalar const c68 = -c1 * c39 + c52 * c60; Scalar const c69 = c18 * c63; Scalar const c70 = c18 * c65; Scalar const c71 = c34 - c69 + c70; Scalar const c72 = Scalar(-2) * c47; Scalar const c73 = Scalar(3) * c28 * c38 * omega[2]; Scalar const c74 = -c23 * c30 + c23; Scalar const c75 = -c32 + c61; Scalar const c76 = c2 * c63; Scalar const c77 = c2 * c65; Scalar const c78 = c52 * c74; Scalar const c79 = c34 + c69 - c70; Scalar const c80 = -c2 * c39 + c50 * c74; Scalar const c81 = -c0; Scalar const c82 = c25 + c81; Scalar const c83 = c32 * omega[0]; Scalar const c84 = c18 * c29; Scalar const c85 = Scalar(-2) * c34; Scalar const c86 = c82 * c9; Scalar const c87 = c0 * c63; Scalar const c88 = c0 * c65; Scalar const c89 = c9 * omega[1] * omega[2]; Scalar const c90 = c41 * c89; Scalar const c91 = c54 - c55 + c56; Scalar const c92 = -c1 * c73 + c60 * c89; Scalar const c93 = -c43 + c46 + c47; Scalar const c94 = -c2 * c59 + c74 * c89; Scalar const c95 = c24 + c81; Scalar const c96 = c9 * c95; J(0, 0) = o; J(0, 1) = o; J(0, 2) = o; J(0, 3) = -c0 * c10 + c0 * c13 + c8; J(0, 4) = c16; J(0, 5) = c17; J(1, 0) = o; J(1, 1) = o; J(1, 2) = o; J(1, 3) = c16; J(1, 4) = -c1 * c10 + c1 * c13 + c8; J(1, 5) = c19; J(2, 0) = o; J(2, 1) = o; J(2, 2) = o; J(2, 3) = c17; J(2, 4) = c19; J(2, 5) = -c10 * c2 + c13 * c2 + c8; J(3, 0) = o; J(3, 1) = o; J(3, 2) = o; J(3, 3) = -c20 * c21; J(3, 4) = -c21 * c22; J(3, 5) = -c21 * c23; J(4, 0) = c26 * c29 + Scalar(1); J(4, 1) = -c33 + c35; J(4, 2) = c36 + c37; J(4, 3) = upsilon[0] * (-c26 * c39 + c40 * c41) + upsilon[1] * (c53 + c57) + upsilon[2] * (c48 + c51); J(4, 4) = upsilon[0] * (-c26 * c59 + c40 * c60 + c58) + upsilon[1] * (c68 + c71) + upsilon[2] * (c62 + c64 - c66 + c67); J(4, 5) = upsilon[0] * (-c26 * c73 + c40 * c74 + c72) + upsilon[1] * (c75 - c76 + c77 + c78) + upsilon[2] * (c79 + c80); J(5, 0) = c33 + c35; J(5, 1) = c29 * c82 + Scalar(1); J(5, 2) = -c83 + c84; J(5, 3) = upsilon[0] * (c53 + c91) + upsilon[1] * (-c39 * c82 + c41 * c86 + c85) + upsilon[2] * (c75 - c87 + c88 + c90); J(5, 4) = upsilon[0] * (c68 + c79) + upsilon[1] * (-c59 * c82 + c60 * c86) + upsilon[2] * (c92 + c93); J(5, 5) = upsilon[0] * (c62 + c76 - c77 + c78) + upsilon[1] * (c72 - c73 * c82 + c74 * c86) + upsilon[2] * (c57 + c94); J(6, 0) = -c36 + c37; J(6, 1) = c83 + c84; J(6, 2) = c29 * c95 + Scalar(1); J(6, 3) = upsilon[0] * (c51 + c93) + upsilon[1] * (c62 + c87 - c88 + c90) + upsilon[2] * (-c39 * c95 + c41 * c96 + c85); J(6, 4) = upsilon[0] * (-c64 + c66 + c67 + c75) + upsilon[1] * (c48 + c92) + upsilon[2] * (c58 - c59 * c95 + c60 * c96); J(6, 5) = upsilon[0] * (c71 + c80) + upsilon[1] * (c91 + c94) + upsilon[2] * (-c73 * c95 + c74 * c96); return J; } /// Returns derivative of exp(x) wrt. x_i at x=0. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_at_0() { Sophus::Matrix J; Scalar const o(0); Scalar const h(0.5); Scalar const i(1); // clang-format off J << o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, i, o, o, o, o, o, o, i, o, o, o, o, o, o, i, o, o, o; // clang-format on return J; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Group exponential /// /// This functions takes in an element of tangent space (= twist ``a``) and /// returns the corresponding element of the group SE(3). /// /// The first three components of ``a`` represent the translational part /// ``upsilon`` in the tangent space of SE(3), while the last three components /// of ``a`` represents the rotation vector ``omega``. /// To be more specific, this function computes ``expmat(hat(a))`` with /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator /// of SE(3), see below. /// SOPHUS_FUNC static SE3 exp(Tangent const& a) { using std::cos; using std::sin; Vector3 const omega = a.template tail<3>(); Scalar theta; SO3 const so3 = SO3::expAndTheta(omega, &theta); Matrix3 const Omega = SO3::hat(omega); Matrix3 const Omega_sq = Omega * Omega; Matrix3 V; if (theta < Constants::epsilon()) { V = so3.matrix(); /// Note: That is an accurate expansion! } else { Scalar theta_sq = theta * theta; V = (Matrix3::Identity() + (Scalar(1) - cos(theta)) / (theta_sq)*Omega + (theta - sin(theta)) / (theta_sq * theta) * Omega_sq); } return SE3(so3, V * a.template head<3>()); } /// Returns closest SE3 given arbirary 4x4 matrix. /// template SOPHUS_FUNC static enable_if_t::value, SE3> fitToSE3(Matrix4 const& T) { return SE3(SO3::fitToSO3(T.template block<3, 3>(0, 0)), T.template block<3, 1>(0, 3)); } /// Returns the ith infinitesimal generators of SE(3). /// /// The infinitesimal generators of SE(3) are: /// /// ``` /// | 0 0 0 1 | /// G_0 = | 0 0 0 0 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_1 = | 0 0 0 1 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_2 = | 0 0 0 0 | /// | 0 0 0 1 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_3 = | 0 0 -1 0 | /// | 0 1 0 0 | /// | 0 0 0 0 | /// /// | 0 0 1 0 | /// G_4 = | 0 0 0 0 | /// | -1 0 0 0 | /// | 0 0 0 0 | /// /// | 0 -1 0 0 | /// G_5 = | 1 0 0 0 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// ``` /// /// Precondition: ``i`` must be in [0, 5]. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= 5, "i should be in range [0,5]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// hat-operator /// /// It takes in the 6-vector representation (= twist) and returns the /// corresponding matrix representation of Lie algebra element. /// /// Formally, the hat()-operator of SE(3) is defined as /// /// ``hat(.): R^6 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (for i=0,...,5) /// /// with ``G_i`` being the ith infinitesimal generator of SE(3). /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; Omega.setZero(); Omega.template topLeftCorner<3, 3>() = SO3::hat(a.template tail<3>()); Omega.col(3).template head<3>() = a.template head<3>(); return Omega; } /// Lie bracket /// /// It computes the Lie bracket of SE(3). To be more specific, it computes /// /// ``[omega_1, omega_2]_se3 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of SE(3). /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector3 const upsilon1 = a.template head<3>(); Vector3 const upsilon2 = b.template head<3>(); Vector3 const omega1 = a.template tail<3>(); Vector3 const omega2 = b.template tail<3>(); Tangent res; res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2); res.template tail<3>() = omega1.cross(omega2); return res; } /// Construct x-axis rotation. /// static SOPHUS_FUNC SE3 rotX(Scalar const& x) { return SE3(SO3::rotX(x), Sophus::Vector3::Zero()); } /// Construct y-axis rotation. /// static SOPHUS_FUNC SE3 rotY(Scalar const& y) { return SE3(SO3::rotY(y), Sophus::Vector3::Zero()); } /// Construct z-axis rotation. /// static SOPHUS_FUNC SE3 rotZ(Scalar const& z) { return SE3(SO3::rotZ(z), Sophus::Vector3::Zero()); } /// Draw uniform sample from SE(3) manifold. /// /// Translations are drawn component-wise from the range [-1, 1]. /// template static SE3 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); return SE3(SO3::sampleUniform(generator), Vector3(uniform(generator), uniform(generator), uniform(generator))); } /// Construct a translation only SE3 instance. /// template static SOPHUS_FUNC SE3 trans(T0 const& x, T1 const& y, T2 const& z) { return SE3(SO3(), Vector3(x, y, z)); } static SOPHUS_FUNC SE3 trans(Vector3 const& xyz) { return SE3(SO3(), xyz); } /// Construct x-axis translation. /// static SOPHUS_FUNC SE3 transX(Scalar const& x) { return SE3::trans(x, Scalar(0), Scalar(0)); } /// Construct y-axis translation. /// static SOPHUS_FUNC SE3 transY(Scalar const& y) { return SE3::trans(Scalar(0), y, Scalar(0)); } /// Construct z-axis translation. /// static SOPHUS_FUNC SE3 transZ(Scalar const& z) { return SE3::trans(Scalar(0), Scalar(0), z); } /// vee-operator /// /// It takes 4x4-matrix representation ``Omega`` and maps it to the /// corresponding 6-vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | 0 -f e a | /// | f 0 -d b | /// | -e d 0 c /// | 0 0 0 0 | . /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { Tangent upsilon_omega; upsilon_omega.template head<3>() = Omega.col(3).template head<3>(); upsilon_omega.template tail<3>() = SO3::vee(Omega.template topLeftCorner<3, 3>()); return upsilon_omega; } protected: SO3Member so3_; TranslationMember translation_; }; template SE3::SE3() : translation_(TranslationMember::Zero()) { static_assert(std::is_standard_layout::value, "Assume standard layout for the use of offsetof check below."); static_assert( offsetof(SE3, so3_) + sizeof(Scalar) * SO3::num_parameters == offsetof(SE3, translation_), "This class assumes packed storage and hence will only work " "correctly depending on the compiler (options) - in " "particular when using [this->data(), this-data() + " "num_parameters] to access the raw data in a contiguous fashion."); } } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``SE3``; derived from SE3Base. /// /// Allows us to wrap SE3 objects around POD array. template class Map, Options> : public Sophus::SE3Base, Options>> { public: using Base = Sophus::SE3Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map) // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : so3_(coeffs), translation_(coeffs + Sophus::SO3::num_parameters) {} /// Mutator of SO3 /// SOPHUS_FUNC Map, Options>& so3() { return so3_; } /// Accessor of SO3 /// SOPHUS_FUNC Map, Options> const& so3() const { return so3_; } /// Mutator of translation vector /// SOPHUS_FUNC Map>& translation() { return translation_; } /// Accessor of translation vector /// SOPHUS_FUNC Map> const& translation() const { return translation_; } protected: Map, Options> so3_; Map, Options> translation_; }; /// Specialization of Eigen::Map for ``SE3 const``; derived from SE3Base. /// /// Allows us to wrap SE3 objects around POD array. template class Map const, Options> : public Sophus::SE3Base const, Options>> { public: using Base = Sophus::SE3Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : so3_(coeffs), translation_(coeffs + Sophus::SO3::num_parameters) {} /// Accessor of SO3 /// SOPHUS_FUNC Map const, Options> const& so3() const { return so3_; } /// Accessor of translation vector /// SOPHUS_FUNC Map const, Options> const& translation() const { return translation_; } protected: Map const, Options> const so3_; Map const, Options> const translation_; }; } // namespace Eigen #endif ================================================ FILE: thirdparty/Sophus/sim2.hpp ================================================ /// @file /// Similarity group Sim(2) - scaling, rotation and translation in 2d. #ifndef SOPHUS_SIM2_HPP #define SOPHUS_SIM2_HPP #include "rxso2.hpp" #include "sim_details.hpp" namespace Sophus { template class Sim2; using Sim2d = Sim2; using Sim2f = Sim2; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { using Scalar = Scalar_; using TranslationType = Sophus::Vector2; using RxSO2Type = Sophus::RxSO2; }; template struct traits, Options>> : traits> { using Scalar = Scalar_; using TranslationType = Map, Options>; using RxSO2Type = Map, Options>; }; template struct traits const, Options>> : traits const> { using Scalar = Scalar_; using TranslationType = Map const, Options>; using RxSO2Type = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// Sim2 base type - implements Sim2 class but is storage agnostic. /// /// Sim(2) is the group of rotations and translation and scaling in 2d. It is /// the semi-direct product of R+xSO(2) and the 2d Euclidean vector space. The /// class is represented using a composition of RxSO2 for scaling plus /// rotation and a 2-vector for translation. /// /// Sim(2) is neither compact, nor a commutative group. /// /// See RxSO2 for more details of the scaling + rotation representation in 2d. /// template class Sim2Base { public: using Scalar = typename Eigen::internal::traits::Scalar; using TranslationType = typename Eigen::internal::traits::TranslationType; using RxSO2Type = typename Eigen::internal::traits::RxSO2Type; /// Degrees of freedom of manifold, number of dimensions in tangent space /// (two for translation, one for rotation and one for scaling). static int constexpr DoF = 4; /// Number of internal parameters used (2-tuple for complex number, two for /// translation). static int constexpr num_parameters = 4; /// Group transformations are 3x3 matrices. static int constexpr N = 3; using Transformation = Matrix; using Point = Vector2; using HomogeneousPoint = Vector3; using Line = ParametrizedLine2; using Tangent = Vector; using Adjoint = Matrix; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with SIM2 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using Sim2Product = Sim2>; template using PointProduct = Vector2>; template using HomogeneousPointProduct = Vector3>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// SOPHUS_FUNC Adjoint Adj() const { Adjoint res; res.setZero(); res.template block<2, 2>(0, 0) = rxso2().matrix(); res(0, 2) = translation()[1]; res(1, 2) = -translation()[0]; res.template block<2, 1>(0, 3) = -translation(); res(2, 2) = Scalar(1); res(3, 3) = Scalar(1); return res; } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC Sim2 cast() const { return Sim2(rxso2().template cast(), translation().template cast()); } /// Returns group inverse. /// SOPHUS_FUNC Sim2 inverse() const { RxSO2 invR = rxso2().inverse(); return Sim2(invR, invR * (translation() * Scalar(-1))); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rigid body transformations) to elements of the /// tangent space (twist). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of Sim(2). /// SOPHUS_FUNC Tangent log() const { /// The derivation of the closed-form Sim(2) logarithm for is done /// analogously to the closed-form solution of the SE(2) logarithm, see /// J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices /// and logarithms of orthogonal matrices", IJRA 2002. /// https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf /// (Sec. 6., pp. 8) Tangent res; Vector2 const theta_sigma = rxso2().log(); Scalar const theta = theta_sigma[0]; Scalar const sigma = theta_sigma[1]; Matrix2 const Omega = SO2::hat(theta); Matrix2 const W_inv = details::calcWInv(Omega, theta, sigma, scale()); res.segment(0, 2) = W_inv * translation(); res[2] = theta; res[3] = sigma; return res; } /// Returns 3x3 matrix representation of the instance. /// /// It has the following form: /// /// | s*R t | /// | o 1 | /// /// where ``R`` is a 2x2 rotation matrix, ``s`` a scale factor, ``t`` a /// translation 2-vector and ``o`` a 2-column vector of zeros. /// SOPHUS_FUNC Transformation matrix() const { Transformation homogenious_matrix; homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3(); homogenious_matrix.row(2) = Matrix(Scalar(0), Scalar(0), Scalar(1)); return homogenious_matrix; } /// Returns the significant first two rows of the matrix above. /// SOPHUS_FUNC Matrix matrix2x3() const { Matrix matrix; matrix.template topLeftCorner<2, 2>() = rxso2().matrix(); matrix.col(2) = translation(); return matrix; } /// Assignment operator. /// SOPHUS_FUNC Sim2Base& operator=(Sim2Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC Sim2Base& operator=( Sim2Base const& other) { rxso2() = other.rxso2(); translation() = other.translation(); return *this; } /// Group multiplication, which is rotation plus scaling concatenation. /// /// Note: That scaling is calculated with saturation. See RxSO2 for /// details. /// template SOPHUS_FUNC Sim2Product operator*( Sim2Base const& other) const { return Sim2Product( rxso2() * other.rxso2(), translation() + rxso2() * other.translation()); } /// Group action on 2-points. /// /// This function rotates, scales and translates a two dimensional point /// ``p`` by the Sim(2) element ``(bar_sR_foo, t_bar)`` (= similarity /// transformation): /// /// ``p_bar = bar_sR_foo * p_foo + t_bar``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return rxso2() * p + translation(); } /// Group action on homogeneous 2-points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = rxso2() * p.template head<2>() + p(2) * translation(); return HomogeneousPointProduct(tp(0), tp(1), p(2)); } /// Group action on lines. /// /// This function rotates, scales and translates a parametrized line /// ``l(t) = o + t * d`` by the Sim(2) element: /// /// Origin ``o`` is rotated, scaled and translated /// Direction ``d`` is rotated /// SOPHUS_FUNC Line operator*(Line const& l) const { Line rotatedLine = rxso2() * l; return Line(rotatedLine.origin() + translation(), rotatedLine.direction()); } /// Returns internal parameters of Sim(2). /// /// It returns (c[0], c[1], t[0], t[1]), /// with c being the complex number, t the translation 3-vector. /// SOPHUS_FUNC Sophus::Vector params() const { Sophus::Vector p; p << rxso2().params(), translation(); return p; } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO2's Scalar type. /// template >::value>::type> SOPHUS_FUNC Sim2Base& operator*=( Sim2Base const& other) { *static_cast(this) = *this * other; return *this; } /// Setter of non-zero complex number. /// /// Precondition: ``z`` must not be close to zero. /// SOPHUS_FUNC void setComplex(Vector2 const& z) { rxso2().setComplex(z); } /// Accessor of complex number. /// SOPHUS_FUNC typename Eigen::internal::traits::RxSO2Type::ComplexType const& complex() const { return rxso2().complex(); } /// Returns Rotation matrix /// SOPHUS_FUNC Matrix2 rotationMatrix() const { return rxso2().rotationMatrix(); } /// Mutator of SO2 group. /// SOPHUS_FUNC RxSO2Type& rxso2() { return static_cast(this)->rxso2(); } /// Accessor of SO2 group. /// SOPHUS_FUNC RxSO2Type const& rxso2() const { return static_cast(this)->rxso2(); } /// Returns scale. /// SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); } /// Setter of complex number using rotation matrix ``R``, leaves scale as is. /// SOPHUS_FUNC void setRotationMatrix(Matrix2& R) { rxso2().setRotationMatrix(R); } /// Sets scale and leaves rotation as is. /// /// Note: This function as a significant computational cost, since it has to /// call the square root twice. /// SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scale); } /// Setter of complexnumber using scaled rotation matrix ``sR``. /// /// Precondition: The 2x2 matrix must be "scaled orthogonal" /// and have a positive determinant. /// SOPHUS_FUNC void setScaledRotationMatrix(Matrix2 const& sR) { rxso2().setScaledRotationMatrix(sR); } /// Mutator of translation vector /// SOPHUS_FUNC TranslationType& translation() { return static_cast(this)->translation(); } /// Accessor of translation vector /// SOPHUS_FUNC TranslationType const& translation() const { return static_cast(this)->translation(); } }; /// Sim2 using default storage; derived from Sim2Base. template class Sim2 : public Sim2Base> { public: using Base = Sim2Base>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using RxSo2Member = RxSO2; using TranslationMember = Vector2; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes similarity transform to the identity. /// SOPHUS_FUNC Sim2(); /// Copy constructor /// SOPHUS_FUNC Sim2(Sim2 const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC Sim2(Sim2Base const& other) : rxso2_(other.rxso2()), translation_(other.translation()) { static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from RxSO2 and translation vector /// template SOPHUS_FUNC Sim2(RxSO2Base const& rxso2, Eigen::MatrixBase const& translation) : rxso2_(rxso2), translation_(translation) { static_assert(std::is_same::value, "must be same Scalar type"); static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from complex number and translation vector. /// /// Precondition: complex number must not be close to zero. /// template SOPHUS_FUNC Sim2(Vector2 const& complex_number, Eigen::MatrixBase const& translation) : rxso2_(complex_number), translation_(translation) { static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from 3x3 matrix /// /// Precondition: Top-left 2x2 matrix needs to be "scaled-orthogonal" with /// positive determinant. The last row must be ``(0, 0, 1)``. /// SOPHUS_FUNC explicit Sim2(Matrix const& T) : rxso2_((T.template topLeftCorner<2, 2>()).eval()), translation_(T.template block<2, 1>(0, 2)) {} /// This provides unsafe read/write access to internal data. Sim(2) is /// represented by a complex number (two parameters) and a 2-vector. When /// using direct write access, the user needs to take care of that the /// complex number is not set close to zero. /// SOPHUS_FUNC Scalar* data() { // rxso2_ and translation_ are laid out sequentially with no padding return rxso2_.data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { // rxso2_ and translation_ are laid out sequentially with no padding return rxso2_.data(); } /// Accessor of RxSO2 /// SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; } /// Mutator of RxSO2 /// SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; } /// Mutator of translation vector /// SOPHUS_FUNC TranslationMember& translation() { return translation_; } /// Accessor of translation vector /// SOPHUS_FUNC TranslationMember const& translation() const { return translation_; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Derivative of Lie bracket with respect to first element. /// /// This function returns ``D_a [a, b]`` with ``D_a`` being the /// differential operator with respect to ``a``, ``[a, b]`` being the lie /// bracket of the Lie algebra sim(2). /// See ``lieBracket()`` below. /// /// Group exponential /// /// This functions takes in an element of tangent space and returns the /// corresponding element of the group Sim(2). /// /// The first two components of ``a`` represent the translational part /// ``upsilon`` in the tangent space of Sim(2), the following two components /// of ``a`` represents the rotation ``theta`` and the final component /// represents the logarithm of the scaling factor ``sigma``. /// To be more specific, this function computes ``expmat(hat(a))`` with /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator /// of Sim(2), see below. /// SOPHUS_FUNC static Sim2 exp(Tangent const& a) { // For the derivation of the exponential map of Sim(N) see // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual // SLAM", PhD thesis, 2012. // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186) Vector2 const upsilon = a.segment(0, 2); Scalar const theta = a[2]; Scalar const sigma = a[3]; RxSO2 rxso2 = RxSO2::exp(a.template tail<2>()); Matrix2 const Omega = SO2::hat(theta); Matrix2 const W = details::calcW(Omega, theta, sigma); return Sim2(rxso2, W * upsilon); } /// Returns the ith infinitesimal generators of Sim(2). /// /// The infinitesimal generators of Sim(2) are: /// /// ``` /// | 0 0 1 | /// G_0 = | 0 0 0 | /// | 0 0 0 | /// /// | 0 0 0 | /// G_1 = | 0 0 1 | /// | 0 0 0 | /// /// | 0 -1 0 | /// G_2 = | 1 0 0 | /// | 0 0 0 | /// /// | 1 0 0 | /// G_3 = | 0 1 0 | /// | 0 0 0 | /// ``` /// /// Precondition: ``i`` must be in [0, 3]. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 || i <= 3, "i should be in range [0,3]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// hat-operator /// /// It takes in the 4-vector representation and returns the corresponding /// matrix representation of Lie algebra element. /// /// Formally, the hat()-operator of Sim(2) is defined as /// /// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,...,6) /// /// with ``G_i`` being the ith infinitesimal generator of Sim(2). /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; Omega.template topLeftCorner<2, 2>() = RxSO2::hat(a.template tail<2>()); Omega.col(2).template head<2>() = a.template head<2>(); Omega.row(2).setZero(); return Omega; } /// Lie bracket /// /// It computes the Lie bracket of Sim(2). To be more specific, it computes /// /// ``[omega_1, omega_2]_sim2 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(2). /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector2 const upsilon1 = a.template head<2>(); Vector2 const upsilon2 = b.template head<2>(); Scalar const theta1 = a[2]; Scalar const theta2 = b[2]; Scalar const sigma1 = a[3]; Scalar const sigma2 = b[3]; Tangent res; res[0] = -theta1 * upsilon2[1] + theta2 * upsilon1[1] + sigma1 * upsilon2[0] - sigma2 * upsilon1[0]; res[1] = theta1 * upsilon2[0] - theta2 * upsilon1[0] + sigma1 * upsilon2[1] - sigma2 * upsilon1[1]; res[2] = Scalar(0); res[3] = Scalar(0); return res; } /// Draw uniform sample from Sim(2) manifold. /// /// Translations are drawn component-wise from the range [-1, 1]. /// The scale factor is drawn uniformly in log2-space from [-1, 1], /// hence the scale is in [0.5, 2]. /// template static Sim2 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); return Sim2(RxSO2::sampleUniform(generator), Vector2(uniform(generator), uniform(generator))); } /// vee-operator /// /// It takes the 3x3-matrix representation ``Omega`` and maps it to the /// corresponding 4-vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | d -c a | /// | c d b | /// | 0 0 0 | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { Tangent upsilon_omega_sigma; upsilon_omega_sigma.template head<2>() = Omega.col(2).template head<2>(); upsilon_omega_sigma.template tail<2>() = RxSO2::vee(Omega.template topLeftCorner<2, 2>()); return upsilon_omega_sigma; } protected: RxSo2Member rxso2_; TranslationMember translation_; }; template Sim2::Sim2() : translation_(TranslationMember::Zero()) { static_assert(std::is_standard_layout::value, "Assume standard layout for the use of offsetof check below."); static_assert( offsetof(Sim2, rxso2_) + sizeof(Scalar) * RxSO2::num_parameters == offsetof(Sim2, translation_), "This class assumes packed storage and hence will only work " "correctly depending on the compiler (options) - in " "particular when using [this->data(), this-data() + " "num_parameters] to access the raw data in a contiguous fashion."); } } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``Sim2``; derived from Sim2Base. /// /// Allows us to wrap Sim2 objects around POD array. template class Map, Options> : public Sophus::Sim2Base, Options>> { public: using Base = Sophus::Sim2Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map); // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : rxso2_(coeffs), translation_(coeffs + Sophus::RxSO2::num_parameters) {} /// Mutator of RxSO2 /// SOPHUS_FUNC Map, Options>& rxso2() { return rxso2_; } /// Accessor of RxSO2 /// SOPHUS_FUNC Map, Options> const& rxso2() const { return rxso2_; } /// Mutator of translation vector /// SOPHUS_FUNC Map, Options>& translation() { return translation_; } /// Accessor of translation vector SOPHUS_FUNC Map, Options> const& translation() const { return translation_; } protected: Map, Options> rxso2_; Map, Options> translation_; }; /// Specialization of Eigen::Map for ``Sim2 const``; derived from Sim2Base. /// /// Allows us to wrap RxSO2 objects around POD array. template class Map const, Options> : public Sophus::Sim2Base const, Options>> { public: using Base = Sophus::Sim2Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : rxso2_(coeffs), translation_(coeffs + Sophus::RxSO2::num_parameters) {} /// Accessor of RxSO2 /// SOPHUS_FUNC Map const, Options> const& rxso2() const { return rxso2_; } /// Accessor of translation vector /// SOPHUS_FUNC Map const, Options> const& translation() const { return translation_; } protected: Map const, Options> const rxso2_; Map const, Options> const translation_; }; } // namespace Eigen #endif ================================================ FILE: thirdparty/Sophus/sim3.hpp ================================================ /// @file /// Similarity group Sim(3) - scaling, rotation and translation in 3d. #ifndef SOPHUS_SIM3_HPP #define SOPHUS_SIM3_HPP #include "rxso3.hpp" #include "sim_details.hpp" namespace Sophus { template class Sim3; using Sim3d = Sim3; using Sim3f = Sim3; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { using Scalar = Scalar_; using TranslationType = Sophus::Vector3; using RxSO3Type = Sophus::RxSO3; }; template struct traits, Options>> : traits> { using Scalar = Scalar_; using TranslationType = Map, Options>; using RxSO3Type = Map, Options>; }; template struct traits const, Options>> : traits const> { using Scalar = Scalar_; using TranslationType = Map const, Options>; using RxSO3Type = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// Sim3 base type - implements Sim3 class but is storage agnostic. /// /// Sim(3) is the group of rotations and translation and scaling in 3d. It is /// the semi-direct product of R+xSO(3) and the 3d Euclidean vector space. The /// class is represented using a composition of RxSO3 for scaling plus /// rotation and a 3-vector for translation. /// /// Sim(3) is neither compact, nor a commutative group. /// /// See RxSO3 for more details of the scaling + rotation representation in 3d. /// template class Sim3Base { public: using Scalar = typename Eigen::internal::traits::Scalar; using TranslationType = typename Eigen::internal::traits::TranslationType; using RxSO3Type = typename Eigen::internal::traits::RxSO3Type; using QuaternionType = typename RxSO3Type::QuaternionType; /// Degrees of freedom of manifold, number of dimensions in tangent space /// (three for translation, three for rotation and one for scaling). static int constexpr DoF = 7; /// Number of internal parameters used (4-tuple for quaternion, three for /// translation). static int constexpr num_parameters = 7; /// Group transformations are 4x4 matrices. static int constexpr N = 4; using Transformation = Matrix; using Point = Vector3; using HomogeneousPoint = Vector4; using Line = ParametrizedLine3; using Tangent = Vector; using Adjoint = Matrix; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with Sim3 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using Sim3Product = Sim3>; template using PointProduct = Vector3>; template using HomogeneousPointProduct = Vector4>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// SOPHUS_FUNC Adjoint Adj() const { Matrix3 const R = rxso3().rotationMatrix(); Adjoint res; res.setZero(); res.template block<3, 3>(0, 0) = rxso3().matrix(); res.template block<3, 3>(0, 3) = SO3::hat(translation()) * R; res.template block<3, 1>(0, 6) = -translation(); res.template block<3, 3>(3, 3) = R; res(6, 6) = Scalar(1); return res; } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC Sim3 cast() const { return Sim3(rxso3().template cast(), translation().template cast()); } /// Returns group inverse. /// SOPHUS_FUNC Sim3 inverse() const { RxSO3 invR = rxso3().inverse(); return Sim3(invR, invR * (translation() * Scalar(-1))); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rigid body transformations) to elements of the /// tangent space (twist). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of Sim(3). /// SOPHUS_FUNC Tangent log() const { // The derivation of the closed-form Sim(3) logarithm for is done // analogously to the closed-form solution of the SE(3) logarithm, see // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices // and logarithms of orthogonal matrices", IJRA 2002. // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf // (Sec. 6., pp. 8) Tangent res; auto omega_sigma_and_theta = rxso3().logAndTheta(); Vector3 const omega = omega_sigma_and_theta.tangent.template head<3>(); Scalar sigma = omega_sigma_and_theta.tangent[3]; Matrix3 const Omega = SO3::hat(omega); Matrix3 const W_inv = details::calcWInv( Omega, omega_sigma_and_theta.theta, sigma, scale()); res.segment(0, 3) = W_inv * translation(); res.segment(3, 3) = omega; res[6] = sigma; return res; } /// Returns 4x4 matrix representation of the instance. /// /// It has the following form: /// /// | s*R t | /// | o 1 | /// /// where ``R`` is a 3x3 rotation matrix, ``s`` a scale factor, ``t`` a /// translation 3-vector and ``o`` a 3-column vector of zeros. /// SOPHUS_FUNC Transformation matrix() const { Transformation homogenious_matrix; homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4(); homogenious_matrix.row(3) = Matrix(Scalar(0), Scalar(0), Scalar(0), Scalar(1)); return homogenious_matrix; } /// Returns the significant first three rows of the matrix above. /// SOPHUS_FUNC Matrix matrix3x4() const { Matrix matrix; matrix.template topLeftCorner<3, 3>() = rxso3().matrix(); matrix.col(3) = translation(); return matrix; } /// Assignment operator. /// SOPHUS_FUNC Sim3Base& operator=(Sim3Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC Sim3Base& operator=( Sim3Base const& other) { rxso3() = other.rxso3(); translation() = other.translation(); return *this; } /// Group multiplication, which is rotation plus scaling concatenation. /// /// Note: That scaling is calculated with saturation. See RxSO3 for /// details. /// template SOPHUS_FUNC Sim3Product operator*( Sim3Base const& other) const { return Sim3Product( rxso3() * other.rxso3(), translation() + rxso3() * other.translation()); } /// Group action on 3-points. /// /// This function rotates, scales and translates a three dimensional point /// ``p`` by the Sim(3) element ``(bar_sR_foo, t_bar)`` (= similarity /// transformation): /// /// ``p_bar = bar_sR_foo * p_foo + t_bar``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { return rxso3() * p + translation(); } /// Group action on homogeneous 3-points. See above for more details. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { const PointProduct tp = rxso3() * p.template head<3>() + p(3) * translation(); return HomogeneousPointProduct(tp(0), tp(1), tp(2), p(3)); } /// Group action on lines. /// /// This function rotates, scales and translates a parametrized line /// ``l(t) = o + t * d`` by the Sim(3) element: /// /// Origin ``o`` is rotated, scaled and translated /// Direction ``d`` is rotated /// SOPHUS_FUNC Line operator*(Line const& l) const { Line rotatedLine = rxso3() * l; return Line(rotatedLine.origin() + translation(), rotatedLine.direction()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO3's Scalar type. /// template >::value>::type> SOPHUS_FUNC Sim3Base& operator*=( Sim3Base const& other) { *static_cast(this) = *this * other; return *this; } /// Returns internal parameters of Sim(3). /// /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]), /// with q being the quaternion, t the translation 3-vector. /// SOPHUS_FUNC Sophus::Vector params() const { Sophus::Vector p; p << rxso3().params(), translation(); return p; } /// Setter of non-zero quaternion. /// /// Precondition: ``quat`` must not be close to zero. /// SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quat) { rxso3().setQuaternion(quat); } /// Accessor of quaternion. /// SOPHUS_FUNC QuaternionType const& quaternion() const { return rxso3().quaternion(); } /// Returns Rotation matrix /// SOPHUS_FUNC Matrix3 rotationMatrix() const { return rxso3().rotationMatrix(); } /// Mutator of SO3 group. /// SOPHUS_FUNC RxSO3Type& rxso3() { return static_cast(this)->rxso3(); } /// Accessor of SO3 group. /// SOPHUS_FUNC RxSO3Type const& rxso3() const { return static_cast(this)->rxso3(); } /// Returns scale. /// SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); } /// Setter of quaternion using rotation matrix ``R``, leaves scale as is. /// SOPHUS_FUNC void setRotationMatrix(Matrix3& R) { rxso3().setRotationMatrix(R); } /// Sets scale and leaves rotation as is. /// /// Note: This function as a significant computational cost, since it has to /// call the square root twice. /// SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); } /// Setter of quaternion using scaled rotation matrix ``sR``. /// /// Precondition: The 3x3 matrix must be "scaled orthogonal" /// and have a positive determinant. /// SOPHUS_FUNC void setScaledRotationMatrix(Matrix3 const& sR) { rxso3().setScaledRotationMatrix(sR); } /// Mutator of translation vector /// SOPHUS_FUNC TranslationType& translation() { return static_cast(this)->translation(); } /// Accessor of translation vector /// SOPHUS_FUNC TranslationType const& translation() const { return static_cast(this)->translation(); } }; /// Sim3 using default storage; derived from Sim3Base. template class Sim3 : public Sim3Base> { public: using Base = Sim3Base>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using RxSo3Member = RxSO3; using TranslationMember = Vector3; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes similarity transform to the identity. /// SOPHUS_FUNC Sim3(); /// Copy constructor /// SOPHUS_FUNC Sim3(Sim3 const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC Sim3(Sim3Base const& other) : rxso3_(other.rxso3()), translation_(other.translation()) { static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from RxSO3 and translation vector /// template SOPHUS_FUNC Sim3(RxSO3Base const& rxso3, Eigen::MatrixBase const& translation) : rxso3_(rxso3), translation_(translation) { static_assert(std::is_same::value, "must be same Scalar type"); static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from quaternion and translation vector. /// /// Precondition: quaternion must not be close to zero. /// template SOPHUS_FUNC Sim3(Eigen::QuaternionBase const& quaternion, Eigen::MatrixBase const& translation) : rxso3_(quaternion), translation_(translation) { static_assert(std::is_same::value, "must be same Scalar type"); static_assert(std::is_same::value, "must be same Scalar type"); } /// Constructor from 4x4 matrix /// /// Precondition: Top-left 3x3 matrix needs to be "scaled-orthogonal" with /// positive determinant. The last row must be ``(0, 0, 0, 1)``. /// SOPHUS_FUNC explicit Sim3(Matrix const& T) : rxso3_(T.template topLeftCorner<3, 3>()), translation_(T.template block<3, 1>(0, 3)) {} /// This provides unsafe read/write access to internal data. Sim(3) is /// represented by an Eigen::Quaternion (four parameters) and a 3-vector. When /// using direct write access, the user needs to take care of that the /// quaternion is not set close to zero. /// SOPHUS_FUNC Scalar* data() { // rxso3_ and translation_ are laid out sequentially with no padding return rxso3_.data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { // rxso3_ and translation_ are laid out sequentially with no padding return rxso3_.data(); } /// Accessor of RxSO3 /// SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; } /// Mutator of RxSO3 /// SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; } /// Mutator of translation vector /// SOPHUS_FUNC TranslationMember& translation() { return translation_; } /// Accessor of translation vector /// SOPHUS_FUNC TranslationMember const& translation() const { return translation_; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Group exponential /// /// This functions takes in an element of tangent space and returns the /// corresponding element of the group Sim(3). /// /// The first three components of ``a`` represent the translational part /// ``upsilon`` in the tangent space of Sim(3), the following three components /// of ``a`` represents the rotation vector ``omega`` and the final component /// represents the logarithm of the scaling factor ``sigma``. /// To be more specific, this function computes ``expmat(hat(a))`` with /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator /// of Sim(3), see below. /// SOPHUS_FUNC static Sim3 exp(Tangent const& a) { // For the derivation of the exponential map of Sim(3) see // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual // SLAM", PhD thesis, 2012. // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186) Vector3 const upsilon = a.segment(0, 3); Vector3 const omega = a.segment(3, 3); Scalar const sigma = a[6]; Scalar theta; RxSO3 rxso3 = RxSO3::expAndTheta(a.template tail<4>(), &theta); Matrix3 const Omega = SO3::hat(omega); Matrix3 const W = details::calcW(Omega, theta, sigma); return Sim3(rxso3, W * upsilon); } /// Returns the ith infinitesimal generators of Sim(3). /// /// The infinitesimal generators of Sim(3) are: /// /// ``` /// | 0 0 0 1 | /// G_0 = | 0 0 0 0 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_1 = | 0 0 0 1 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_2 = | 0 0 0 0 | /// | 0 0 0 1 | /// | 0 0 0 0 | /// /// | 0 0 0 0 | /// G_3 = | 0 0 -1 0 | /// | 0 1 0 0 | /// | 0 0 0 0 | /// /// | 0 0 1 0 | /// G_4 = | 0 0 0 0 | /// | -1 0 0 0 | /// | 0 0 0 0 | /// /// | 0 -1 0 0 | /// G_5 = | 1 0 0 0 | /// | 0 0 0 0 | /// | 0 0 0 0 | /// /// | 1 0 0 0 | /// G_6 = | 0 1 0 0 | /// | 0 0 1 0 | /// | 0 0 0 0 | /// ``` /// /// Precondition: ``i`` must be in [0, 6]. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// hat-operator /// /// It takes in the 7-vector representation and returns the corresponding /// matrix representation of Lie algebra element. /// /// Formally, the hat()-operator of Sim(3) is defined as /// /// ``hat(.): R^7 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (for i=0,...,6) /// /// with ``G_i`` being the ith infinitesimal generator of Sim(3). /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; Omega.template topLeftCorner<3, 3>() = RxSO3::hat(a.template tail<4>()); Omega.col(3).template head<3>() = a.template head<3>(); Omega.row(3).setZero(); return Omega; } /// Lie bracket /// /// It computes the Lie bracket of Sim(3). To be more specific, it computes /// /// ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(3). /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector3 const upsilon1 = a.template head<3>(); Vector3 const upsilon2 = b.template head<3>(); Vector3 const omega1 = a.template segment<3>(3); Vector3 const omega2 = b.template segment<3>(3); Scalar sigma1 = a[6]; Scalar sigma2 = b[6]; Tangent res; res.template head<3>() = SO3::hat(omega1) * upsilon2 + SO3::hat(upsilon1) * omega2 + sigma1 * upsilon2 - sigma2 * upsilon1; res.template segment<3>(3) = omega1.cross(omega2); res[6] = Scalar(0); return res; } /// Draw uniform sample from Sim(3) manifold. /// /// Translations are drawn component-wise from the range [-1, 1]. /// The scale factor is drawn uniformly in log2-space from [-1, 1], /// hence the scale is in [0.5, 2]. /// template static Sim3 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution uniform(Scalar(-1), Scalar(1)); return Sim3(RxSO3::sampleUniform(generator), Vector3(uniform(generator), uniform(generator), uniform(generator))); } /// vee-operator /// /// It takes the 4x4-matrix representation ``Omega`` and maps it to the /// corresponding 7-vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | g -f e a | /// | f g -d b | /// | -e d g c | /// | 0 0 0 0 | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { Tangent upsilon_omega_sigma; upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>(); upsilon_omega_sigma.template tail<4>() = RxSO3::vee(Omega.template topLeftCorner<3, 3>()); return upsilon_omega_sigma; } protected: RxSo3Member rxso3_; TranslationMember translation_; }; template Sim3::Sim3() : translation_(TranslationMember::Zero()) { static_assert(std::is_standard_layout::value, "Assume standard layout for the use of offsetof check below."); static_assert( offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3::num_parameters == offsetof(Sim3, translation_), "This class assumes packed storage and hence will only work " "correctly depending on the compiler (options) - in " "particular when using [this->data(), this-data() + " "num_parameters] to access the raw data in a contiguous fashion."); } } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``Sim3``; derived from Sim3Base. /// /// Allows us to wrap Sim3 objects around POD array. template class Map, Options> : public Sophus::Sim3Base, Options>> { public: using Base = Sophus::Sim3Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map); // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : rxso3_(coeffs), translation_(coeffs + Sophus::RxSO3::num_parameters) {} /// Mutator of RxSO3 /// SOPHUS_FUNC Map, Options>& rxso3() { return rxso3_; } /// Accessor of RxSO3 /// SOPHUS_FUNC Map, Options> const& rxso3() const { return rxso3_; } /// Mutator of translation vector /// SOPHUS_FUNC Map, Options>& translation() { return translation_; } /// Accessor of translation vector SOPHUS_FUNC Map, Options> const& translation() const { return translation_; } protected: Map, Options> rxso3_; Map, Options> translation_; }; /// Specialization of Eigen::Map for ``Sim3 const``; derived from Sim3Base. /// /// Allows us to wrap RxSO3 objects around POD array. template class Map const, Options> : public Sophus::Sim3Base const, Options>> { using Base = Sophus::Sim3Base const, Options>>; public: using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : rxso3_(coeffs), translation_(coeffs + Sophus::RxSO3::num_parameters) {} /// Accessor of RxSO3 /// SOPHUS_FUNC Map const, Options> const& rxso3() const { return rxso3_; } /// Accessor of translation vector /// SOPHUS_FUNC Map const, Options> const& translation() const { return translation_; } protected: Map const, Options> const rxso3_; Map const, Options> const translation_; }; } // namespace Eigen #endif ================================================ FILE: thirdparty/Sophus/sim_details.hpp ================================================ #ifndef SOPHUS_SIM_DETAILS_HPP #define SOPHUS_SIM_DETAILS_HPP #include "types.hpp" namespace Sophus { namespace details { template Matrix calcW(Matrix const& Omega, Scalar const theta, Scalar const sigma) { using std::abs; using std::cos; using std::exp; using std::sin; static Matrix const I = Matrix::Identity(); static Scalar const one(1); static Scalar const half(0.5); Matrix const Omega2 = Omega * Omega; Scalar const scale = exp(sigma); Scalar A, B, C; if (abs(sigma) < Constants::epsilon()) { C = one; if (abs(theta) < Constants::epsilon()) { A = half; B = Scalar(1. / 6.); } else { Scalar theta_sq = theta * theta; A = (one - cos(theta)) / theta_sq; B = (theta - sin(theta)) / (theta_sq * theta); } } else { C = (scale - one) / sigma; if (abs(theta) < Constants::epsilon()) { Scalar sigma_sq = sigma * sigma; A = ((sigma - one) * scale + one) / sigma_sq; B = (scale * half * sigma_sq + scale - one - sigma * scale) / (sigma_sq * sigma); } else { Scalar theta_sq = theta * theta; Scalar a = scale * sin(theta); Scalar b = scale * cos(theta); Scalar c = theta_sq + sigma * sigma; A = (a * sigma + (one - b) * theta) / (theta * c); B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); } } return A * Omega + B * Omega2 + C * I; } template Matrix calcWInv(Matrix const& Omega, Scalar const theta, Scalar const sigma, Scalar const scale) { using std::abs; using std::cos; using std::sin; static Matrix const I = Matrix::Identity(); static Scalar const half(0.5); static Scalar const one(1); static Scalar const two(2); Matrix const Omega2 = Omega * Omega; Scalar const scale_sq = scale * scale; Scalar const theta_sq = theta * theta; Scalar const sin_theta = sin(theta); Scalar const cos_theta = cos(theta); Scalar a, b, c; if (abs(sigma * sigma) < Constants::epsilon()) { c = one - half * sigma; a = -half; if (abs(theta_sq) < Constants::epsilon()) { b = Scalar(1. / 12.); } else { b = (theta * sin_theta + two * cos_theta - two) / (two * theta_sq * (cos_theta - one)); } } else { Scalar const scale_cu = scale_sq * scale; c = sigma / (scale - one); if (abs(theta_sq) < Constants::epsilon()) { a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); } else { Scalar const s_sin_theta = scale * sin_theta; Scalar const s_cos_theta = scale * cos_theta; a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / (theta * (scale_sq - two * s_cos_theta + one)); b = -scale * (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - scale * sigma + sigma * cos_theta - sigma) / (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + two * s_cos_theta + scale - one)); } } return a * Omega + b * Omega2 + c * I; } } // namespace details } // namespace Sophus #endif ================================================ FILE: thirdparty/Sophus/so2.hpp ================================================ /// @file /// Special orthogonal group SO(2) - rotation in 2d. #ifndef SOPHUS_SO2_HPP #define SOPHUS_SO2_HPP #include #include // Include only the selective set of Eigen headers that we need. // This helps when using Sophus with unusual compilers, like nvcc. #include #include "rotation_matrix.hpp" #include "types.hpp" namespace Sophus { template class SO2; using SO2d = SO2; using SO2f = SO2; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using ComplexType = Sophus::Vector2; }; template struct traits, Options_>> : traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using ComplexType = Map, Options>; }; template struct traits const, Options_>> : traits const> { static constexpr int Options = Options_; using Scalar = Scalar_; using ComplexType = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// SO2 base type - implements SO2 class but is storage agnostic. /// /// SO(2) is the group of rotations in 2d. As a matrix group, it is the set of /// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being /// the transpose of ``R``) and have a positive determinant. In particular, the /// determinant is 1. Let ``theta`` be the rotation angle, the rotation matrix /// can be written in close form: /// /// | cos(theta) -sin(theta) | /// | sin(theta) cos(theta) | /// /// As a matter of fact, the first column of those matrices is isomorph to the /// set of unit complex numbers U(1). Thus, internally, SO2 is represented as /// complex number with length 1. /// /// SO(2) is a compact and commutative group. First it is compact since the set /// of rotation matrices is a closed and bounded set. Second it is commutative /// since ``R(alpha) * R(beta) = R(beta) * R(alpha)``, simply because ``alpha + /// beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles /// (about the same axis). /// /// Class invariant: The 2-norm of ``unit_complex`` must be close to 1. /// Technically speaking, it must hold that: /// /// ``|unit_complex().squaredNorm() - 1| <= Constants::epsilon()``. template class SO2Base { public: static constexpr int Options = Eigen::internal::traits::Options; using Scalar = typename Eigen::internal::traits::Scalar; using ComplexT = typename Eigen::internal::traits::ComplexType; using ComplexTemporaryType = Sophus::Vector2; /// Degrees of freedom of manifold, number of dimensions in tangent space (one /// since we only have in-plane rotations). static int constexpr DoF = 1; /// Number of internal parameters used (complex numbers are a tuples). static int constexpr num_parameters = 2; /// Group transformations are 2x2 matrices. static int constexpr N = 2; using Transformation = Matrix; using Point = Vector2; using HomogeneousPoint = Vector3; using Line = ParametrizedLine2; using Tangent = Scalar; using Adjoint = Scalar; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with SO2 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; template using SO2Product = SO2>; template using PointProduct = Vector2>; template using HomogeneousPointProduct = Vector3>; /// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// /// It simply ``1``, since ``SO(2)`` is a commutative group. /// SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC SO2 cast() const { return SO2(unit_complex().template cast()); } /// This provides unsafe read/write access to internal data. SO(2) is /// represented by a unit complex number (two parameters). When using direct /// write access, the user needs to take care of that the complex number stays /// normalized. /// SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); } /// Returns group inverse. /// SOPHUS_FUNC SO2 inverse() const { return SO2(unit_complex().x(), -unit_complex().y()); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rotation matrices) to elements of the tangent space /// (rotation angles). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of SO(2). /// SOPHUS_FUNC Scalar log() const { using std::atan2; return atan2(unit_complex().y(), unit_complex().x()); } /// It re-normalizes ``unit_complex`` to unit length. /// /// Note: Because of the class invariant, there is typically no need to call /// this function directly. /// SOPHUS_FUNC void normalize() { using std::sqrt; Scalar length = sqrt(unit_complex().x() * unit_complex().x() + unit_complex().y() * unit_complex().y()); SOPHUS_ENSURE(length >= Constants::epsilon(), "Complex number should not be close to zero!"); unit_complex_nonconst().x() /= length; unit_complex_nonconst().y() /= length; } /// Returns 2x2 matrix representation of the instance. /// /// For SO(2), the matrix representation is an orthogonal matrix ``R`` with /// ``det(R)=1``, thus the so-called "rotation matrix". /// SOPHUS_FUNC Transformation matrix() const { Scalar const& real = unit_complex().x(); Scalar const& imag = unit_complex().y(); Transformation R; // clang-format off R << real, -imag, imag, real; // clang-format on return R; } /// Assignment operator /// SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) { unit_complex_nonconst() = other.unit_complex(); return *this; } /// Group multiplication, which is rotation concatenation. /// template SOPHUS_FUNC SO2Product operator*( SO2Base const& other) const { using ResultT = ReturnScalar; Scalar const lhs_real = unit_complex().x(); Scalar const lhs_imag = unit_complex().y(); typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x(); typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y(); // complex multiplication ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag; ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real; ResultT const squared_norm = result_real * result_real + result_imag * result_imag; // We can assume that the squared-norm is close to 1 since we deal with a // unit complex number. Due to numerical precision issues, there might // be a small drift after pose concatenation. Hence, we need to renormalizes // the complex number here. // Since squared-norm is close to 1, we do not need to calculate the costly // square-root, but can use an approximation around 1 (see // http://stackoverflow.com/a/12934750 for details). if (squared_norm != ResultT(1.0)) { ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm); return SO2Product(result_real * scale, result_imag * scale); } return SO2Product(result_real, result_imag); } /// Group action on 2-points. /// /// This function rotates a 2 dimensional point ``p`` by the SO2 element /// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*( Eigen::MatrixBase const& p) const { Scalar const& real = unit_complex().x(); Scalar const& imag = unit_complex().y(); return PointProduct(real * p[0] - imag * p[1], imag * p[0] + real * p[1]); } /// Group action on homogeneous 2-points. /// /// This function rotates a homogeneous 2 dimensional point ``p`` by the SO2 /// element ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. /// template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*( Eigen::MatrixBase const& p) const { Scalar const& real = unit_complex().x(); Scalar const& imag = unit_complex().y(); return HomogeneousPointProduct( real * p[0] - imag * p[1], imag * p[0] + real * p[1], p[2]); } /// Group action on lines. /// /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO2 /// element: /// /// Both direction ``d`` and origin ``o`` are rotated as a 2 dimensional point /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), (*this) * l.direction()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO2's Scalar type. /// template >::value>::type> SOPHUS_FUNC SO2Base operator*=(SO2Base const& other) { *static_cast(this) = *this * other; return *this; } /// Returns derivative of this * SO2::exp(x) wrt. x at x=0. /// SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() const { return Matrix(-unit_complex()[1], unit_complex()[0]); } /// Returns internal parameters of SO(2). /// /// It returns (c[0], c[1]), with c being the unit complex number. /// SOPHUS_FUNC Sophus::Vector params() const { return unit_complex(); } /// Takes in complex number / tuple and normalizes it. /// /// Precondition: The complex number must not be close to zero. /// SOPHUS_FUNC void setComplex(Point const& complex) { unit_complex_nonconst() = complex; normalize(); } /// Accessor of unit quaternion. /// SOPHUS_FUNC ComplexT const& unit_complex() const { return static_cast(this)->unit_complex(); } private: /// Mutator of unit_complex is private to ensure class invariant. That is /// the complex number must stay close to unit length. /// SOPHUS_FUNC ComplexT& unit_complex_nonconst() { return static_cast(this)->unit_complex_nonconst(); } }; /// SO2 using default storage; derived from SO2Base. template class SO2 : public SO2Base> { public: using Base = SO2Base>; static int constexpr DoF = Base::DoF; static int constexpr num_parameters = Base::num_parameters; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using ComplexMember = Vector2; /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``. friend class SO2Base>; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes unit complex number to identity rotation. /// SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {} /// Copy constructor /// SOPHUS_FUNC SO2(SO2 const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC SO2(SO2Base const& other) : unit_complex_(other.unit_complex()) {} /// Constructor from rotation matrix /// /// Precondition: rotation matrix need to be orthogonal with determinant of 1. /// SOPHUS_FUNC explicit SO2(Transformation const& R) : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1))) { SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", R.determinant()); } /// Constructor from pair of real and imaginary number. /// /// Precondition: The pair must not be close to zero. /// SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag) : unit_complex_(real, imag) { Base::normalize(); } /// Constructor from 2-vector. /// /// Precondition: The vector must not be close to zero. /// template SOPHUS_FUNC explicit SO2(Eigen::MatrixBase const& complex) : unit_complex_(complex) { static_assert(std::is_same::value, "must be same Scalar type"); Base::normalize(); } /// Constructor from an rotation angle. /// SOPHUS_FUNC explicit SO2(Scalar theta) { unit_complex_nonconst() = SO2::exp(theta).unit_complex(); } /// Accessor of unit complex number /// SOPHUS_FUNC ComplexMember const& unit_complex() const { return unit_complex_; } /// Group exponential /// /// This functions takes in an element of tangent space (= rotation angle /// ``theta``) and returns the corresponding element of the group SO(2). /// /// To be more specific, this function computes ``expmat(hat(omega))`` /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the /// hat()-operator of SO(2). /// SOPHUS_FUNC static SO2 exp(Tangent const& theta) { using std::cos; using std::sin; return SO2(cos(theta), sin(theta)); } /// Returns derivative of exp(x) wrt. x. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x( Tangent const& theta) { using std::cos; using std::sin; return Sophus::Matrix(-sin(theta), cos(theta)); } /// Returns derivative of exp(x) wrt. x_i at x=0. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_at_0() { return Sophus::Matrix(Scalar(0), Scalar(1)); } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) { return generator(); } /// Returns the infinitesimal generators of SO(2). /// /// The infinitesimal generators of SO(2) is: /// /// | 0 1 | /// | -1 0 | /// SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); } /// hat-operator /// /// It takes in the scalar representation ``theta`` (= rotation angle) and /// returns the corresponding matrix representation of Lie algebra element. /// /// Formally, the hat()-operator of SO(2) is defined as /// /// ``hat(.): R^2 -> R^{2x2}, hat(theta) = theta * G`` /// /// with ``G`` being the infinitesimal generator of SO(2). /// /// The corresponding inverse is the vee()-operator, see below. /// SOPHUS_FUNC static Transformation hat(Tangent const& theta) { Transformation Omega; // clang-format off Omega << Scalar(0), -theta, theta, Scalar(0); // clang-format on return Omega; } /// Returns closed SO2 given arbitrary 2x2 matrix. /// template static SOPHUS_FUNC enable_if_t::value, SO2> fitToSO2(Transformation const& R) { return SO2(makeRotationMatrix(R)); } /// Lie bracket /// /// It returns the Lie bracket of SO(2). Since SO(2) is a commutative group, /// the Lie bracket is simple ``0``. /// SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { return Scalar(0); } /// Draw uniform sample from SO(2) manifold. /// template static SO2 sampleUniform(UniformRandomBitGenerator& generator) { static_assert(IsUniformRandomBitGenerator::value, "generator must meet the UniformRandomBitGenerator concept"); std::uniform_real_distribution uniform(-Constants::pi(), Constants::pi()); return SO2(uniform(generator)); } /// vee-operator /// /// It takes the 2x2-matrix representation ``Omega`` and maps it to the /// corresponding scalar representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | 0 -a | /// | a 0 | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { using std::abs; return Omega(1, 0); } protected: /// Mutator of complex number is protected to ensure class invariant. /// SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; } ComplexMember unit_complex_; }; } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``SO2``; derived from SO2Base. /// /// Allows us to wrap SO2 objects around POD array (e.g. external c style /// complex number / tuple). template class Map, Options> : public Sophus::SO2Base, Options>> { public: using Base = Sophus::SO2Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``. friend class Sophus::SO2Base, Options>>; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map) // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : unit_complex_(coeffs) {} /// Accessor of unit complex number. /// SOPHUS_FUNC Map, Options> const& unit_complex() const { return unit_complex_; } protected: /// Mutator of unit_complex is protected to ensure class invariant. /// SOPHUS_FUNC Map, Options>& unit_complex_nonconst() { return unit_complex_; } Map, Options> unit_complex_; }; /// Specialization of Eigen::Map for ``SO2 const``; derived from SO2Base. /// /// Allows us to wrap SO2 objects around POD array (e.g. external c style /// complex number / tuple). template class Map const, Options> : public Sophus::SO2Base const, Options>> { public: using Base = Sophus::SO2Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {} /// Accessor of unit complex number. /// SOPHUS_FUNC Map const, Options> const& unit_complex() const { return unit_complex_; } protected: /// Mutator of unit_complex is protected to ensure class invariant. /// Map const, Options> const unit_complex_; }; } // namespace Eigen #endif // SOPHUS_SO2_HPP ================================================ FILE: thirdparty/Sophus/so3.hpp ================================================ /// @file /// Special orthogonal group SO(3) - rotation in 3d. #ifndef SOPHUS_SO3_HPP #define SOPHUS_SO3_HPP #include "rotation_matrix.hpp" #include "so2.hpp" #include "types.hpp" // Include only the selective set of Eigen headers that we need. // This helps when using Sophus with unusual compilers, like nvcc. #include #include #include namespace Sophus { template class SO3; using SO3d = SO3; using SO3f = SO3; } // namespace Sophus namespace Eigen { namespace internal { template struct traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using QuaternionType = Eigen::Quaternion; }; template struct traits, Options_>> : traits> { static constexpr int Options = Options_; using Scalar = Scalar_; using QuaternionType = Map, Options>; }; template struct traits const, Options_>> : traits const> { static constexpr int Options = Options_; using Scalar = Scalar_; using QuaternionType = Map const, Options>; }; } // namespace internal } // namespace Eigen namespace Sophus { /// SO3 base type - implements SO3 class but is storage agnostic. /// /// SO(3) is the group of rotations in 3d. As a matrix group, it is the set of /// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being /// the transpose of ``R``) and have a positive determinant. In particular, the /// determinant is 1. Internally, the group is represented as a unit quaternion. /// Unit quaternion can be seen as members of the special unitary group SU(2). /// SU(2) is a double cover of SO(3). Hence, for every rotation matrix ``R``, /// there exist two unit quaternions: ``(r, v)`` and ``(-r, -v)``, with ``r`` /// the real part and ``v`` being the imaginary 3-vector part of the quaternion. /// /// SO(3) is a compact, but non-commutative group. First it is compact since the /// set of rotation matrices is a closed and bounded set. Second it is /// non-commutative since the equation ``R_1 * R_2 = R_2 * R_1`` does not hold /// in general. For example rotating an object by some degrees about its /// ``x``-axis and then by some degrees about its y axis, does not lead to the /// same orientation when rotation first about ``y`` and then about ``x``. /// /// Class invariant: The 2-norm of ``unit_quaternion`` must be close to 1. /// Technically speaking, it must hold that: /// /// ``|unit_quaternion().squaredNorm() - 1| <= Constants::epsilon()``. template class SO3Base { public: static constexpr int Options = Eigen::internal::traits::Options; using Scalar = typename Eigen::internal::traits::Scalar; using QuaternionType = typename Eigen::internal::traits::QuaternionType; using QuaternionTemporaryType = Eigen::Quaternion; /// Degrees of freedom of group, number of dimensions in tangent space. static int constexpr DoF = 3; /// Number of internal parameters used (quaternion is a 4-tuple). static int constexpr num_parameters = 4; /// Group transformations are 3x3 matrices. static int constexpr N = 3; using Transformation = Matrix; using Point = Vector3; using HomogeneousPoint = Vector4; using Line = ParametrizedLine3; using Tangent = Vector; using Adjoint = Matrix; struct TangentAndTheta { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Tangent tangent; Scalar theta; }; /// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with SO3 operations. template using ReturnScalar = typename Eigen::ScalarBinaryOpTraits::ReturnType; template using SO3Product = SO3>; template using PointProduct = Vector3>; template using HomogeneousPointProduct = Vector4>; /// Adjoint transformation // /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. // /// For SO(3), it simply returns the rotation matrix corresponding to ``A``. /// SOPHUS_FUNC Adjoint Adj() const { return matrix(); } /// hat-operator /// /// It takes in the 3-vector representation ``omega`` (= rotation vector) and /// returns the corresponding matrix representation of Lie algebra element. /// /// Formally, the hat()-operator of SO(3) is defined as /// /// ``hat(.): R^3 -> R^{3x3}, hat(omega) = sum_i omega_i * G_i`` /// (for i=0,1,2) /// /// with ``G_i`` being the ith infinitesimal generator of SO(3). /// /// The corresponding inverse is the vee()-operator, see below. /// inline SOPHUS_FUNC static Transformation hat(Tangent const& omega) { Transformation Omega; // clang-format off Omega << Scalar(0), -omega(2), omega(1), omega(2), Scalar(0), -omega(0), -omega(1), omega(0), Scalar(0); // clang-format on return Omega; } /// Extract rotation angle about canonical X-axis /// template SOPHUS_FUNC enable_if_t::value, S> angleX() const { Sophus::Matrix3 R = matrix(); Sophus::Matrix2 Rx = R.template block<2, 2>(1, 1); return SO2(makeRotationMatrix(Rx)).log(); } /// Extract rotation angle about canonical Y-axis /// template SOPHUS_FUNC enable_if_t::value, S> angleY() const { Sophus::Matrix3 R = matrix(); Sophus::Matrix2 Ry; // clang-format off Ry << R(0, 0), R(2, 0), R(0, 2), R(2, 2); // clang-format on return SO2(makeRotationMatrix(Ry)).log(); } /// Extract rotation angle about canonical Z-axis /// template SOPHUS_FUNC enable_if_t::value, S> angleZ() const { Sophus::Matrix3 R = matrix(); Sophus::Matrix2 Rz = R.template block<2, 2>(0, 0); return SO2(makeRotationMatrix(Rz)).log(); } /// Returns copy of instance casted to NewScalarType. /// template SOPHUS_FUNC SO3 cast() const { return SO3(unit_quaternion().template cast()); } /// This provides unsafe read/write access to internal data. SO(3) is /// represented by an Eigen::Quaternion (four parameters). When using direct /// write access, the user needs to take care of that the quaternion stays /// normalized. /// /// Note: The first three Scalars represent the imaginary parts, while the /// forth Scalar represent the real part. /// SOPHUS_FUNC Scalar* data() { return unit_quaternion_nonconst().coeffs().data(); } /// Const version of data() above. /// SOPHUS_FUNC Scalar const* data() const { return unit_quaternion().coeffs().data(); } /// Returns derivative of this * SO3::exp(x) wrt. x at x=0. /// SOPHUS_FUNC Matrix Dx_this_mul_exp_x_at_0() const { Matrix J; Eigen::Quaternion const q = unit_quaternion(); Scalar const c0 = Scalar(0.5) * q.w(); Scalar const c1 = Scalar(0.5) * q.z(); Scalar const c2 = -c1; Scalar const c3 = Scalar(0.5) * q.y(); Scalar const c4 = Scalar(0.5) * q.x(); Scalar const c5 = -c4; Scalar const c6 = -c3; J(0, 0) = c0; J(0, 1) = c2; J(0, 2) = c3; J(1, 0) = c1; J(1, 1) = c0; J(1, 2) = c5; J(2, 0) = c6; J(2, 1) = c4; J(2, 2) = c0; J(3, 0) = c5; J(3, 1) = c6; J(3, 2) = c2; return J; } /// Returns internal parameters of SO(3). /// /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the /// unit quaternion. /// SOPHUS_FUNC Sophus::Vector params() const { return unit_quaternion().coeffs(); } /// Returns group inverse. /// SOPHUS_FUNC SO3 inverse() const { return SO3(unit_quaternion().conjugate()); } /// Logarithmic map /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rotation matrices) to elements of the tangent space /// (rotation-vector). /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of SO(3). /// SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; } /// As above, but also returns ``theta = |omega|``. /// SOPHUS_FUNC TangentAndTheta logAndTheta() const { TangentAndTheta J; using std::abs; using std::atan; using std::sqrt; Scalar squared_n = unit_quaternion().vec().squaredNorm(); Scalar w = unit_quaternion().w(); Scalar two_atan_nbyw_by_n; /// Atan-based log thanks to /// /// C. Hertzberg et al.: /// "Integrating Generic Sensor Fusion Algorithms with Sound State /// Representation through Encapsulation of Manifolds" /// Information Fusion, 2011 if (squared_n < Constants::epsilon() * Constants::epsilon()) { // If quaternion is normalized and n=0, then w should be 1; // w=0 should never happen here! SOPHUS_ENSURE(abs(w) >= Constants::epsilon(), "Quaternion (%) should be normalized!", unit_quaternion().coeffs().transpose()); Scalar squared_w = w * w; two_atan_nbyw_by_n = Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w); J.theta = Scalar(2) * squared_n / w; } else { Scalar n = sqrt(squared_n); if (abs(w) < Constants::epsilon()) { if (w > Scalar(0)) { two_atan_nbyw_by_n = Constants::pi() / n; } else { two_atan_nbyw_by_n = -Constants::pi() / n; } } else { two_atan_nbyw_by_n = Scalar(2) * atan(n / w) / n; } J.theta = two_atan_nbyw_by_n * n; } J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec(); return J; } /// It re-normalizes ``unit_quaternion`` to unit length. /// /// Note: Because of the class invariant, there is typically no need to call /// this function directly. /// SOPHUS_FUNC void normalize() { Scalar length = unit_quaternion_nonconst().norm(); SOPHUS_ENSURE(length >= Constants::epsilon(), "Quaternion (%) should not be close to zero!", unit_quaternion_nonconst().coeffs().transpose()); unit_quaternion_nonconst().coeffs() /= length; } /// Returns 3x3 matrix representation of the instance. /// /// For SO(3), the matrix representation is an orthogonal matrix ``R`` with /// ``det(R)=1``, thus the so-called "rotation matrix". /// SOPHUS_FUNC Transformation matrix() const { return unit_quaternion().toRotationMatrix(); } /// Assignment operator. /// SOPHUS_FUNC SO3Base& operator=(SO3Base const& other) = default; /// Assignment-like operator from OtherDerived. /// template SOPHUS_FUNC SO3Base& operator=(SO3Base const& other) { unit_quaternion_nonconst() = other.unit_quaternion(); return *this; } /// Group multiplication, which is rotation concatenation. /// template SOPHUS_FUNC SO3Product operator*(SO3Base const& other) const { using QuaternionProductType = typename SO3Product::QuaternionType; const QuaternionType& a = unit_quaternion(); const typename OtherDerived::QuaternionType& b = other.unit_quaternion(); /// NOTE: We cannot use Eigen's Quaternion multiplication because it always /// returns a Quaternion of the same Scalar as this object, so it is not /// able to multiple Jets and doubles correctly. return SO3Product( QuaternionProductType(a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x())); } /// Group action on 3-points. /// /// This function rotates a 3 dimensional point ``p`` by the SO3 element /// ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``. /// /// Since SO3 is internally represented by a unit quaternion ``q``, it is /// implemented as ``p_bar = q * p_foo * q^{*}`` /// with ``q^{*}`` being the quaternion conjugate of ``q``. /// /// Geometrically, ``p`` is rotated by angle ``|omega|`` around the /// axis ``omega/|omega|`` with ``omega := vee(log(bar_R_foo))``. /// /// For ``vee``-operator, see below. /// template ::value>::type> SOPHUS_FUNC PointProduct operator*(Eigen::MatrixBase const& p) const { /// NOTE: We cannot use Eigen's Quaternion transformVector because it always /// returns a Vector3 of the same Scalar as this quaternion, so it is not /// able to be applied to Jets and doubles correctly. const QuaternionType& q = unit_quaternion(); PointProduct uv = q.vec().cross(p); uv += uv; return p + q.w() * uv + q.vec().cross(uv); } /// Group action on homogeneous 3-points. See above for more details. template ::value>::type> SOPHUS_FUNC HomogeneousPointProduct operator*(Eigen::MatrixBase const& p) const { const auto rp = *this * p.template head<3>(); return HomogeneousPointProduct(rp(0), rp(1), rp(2), p(3)); } /// Group action on lines. /// /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3 /// element: /// /// Both direction ``d`` and origin ``o`` are rotated as a 3 dimensional point /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), (*this) * l.direction()); } /// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO3's Scalar type. /// template >::value>::type> SOPHUS_FUNC SO3Base& operator*=(SO3Base const& other) { *static_cast(this) = *this * other; return *this; } /// Takes in quaternion, and normalizes it. /// /// Precondition: The quaternion must not be close to zero. /// SOPHUS_FUNC void setQuaternion(Eigen::Quaternion const& quaternion) { unit_quaternion_nonconst() = quaternion; normalize(); } /// Accessor of unit quaternion. /// SOPHUS_FUNC QuaternionType const& unit_quaternion() const { return static_cast(this)->unit_quaternion(); } // newly-added // right jacobian SOPHUS_FUNC static Transformation jl(const Point& Omega) { Scalar theta = Omega.norm(); if (theta < 1e-6) { return Transformation::Identity(); } Point a = Omega; a.normalize(); double sin_theta = std::sin(theta); double cos_theta = std::cos(theta); return (sin_theta / theta) * Transformation ::Identity() + (1 - sin_theta / theta) * a * a.transpose() + (1 - cos_theta) / theta * hat(a); } SOPHUS_FUNC static Transformation jl_inv(const Point& Omega) { Scalar theta = Omega.norm(); if (theta < 1e-6) { return Transformation::Identity(); } Point a = Omega; a.normalize(); double cot_half_theta = cos(0.5 * theta) / sin(0.5 * theta); return 0.5 * theta * cot_half_theta * Transformation ::Identity() + (1 - 0.5 * theta * cot_half_theta) * a * a.transpose() - 0.5 * theta * hat(a); } SOPHUS_FUNC static Transformation jr(const Point& Omega) { return jl(-Omega); } SOPHUS_FUNC Transformation jr() { return jr(log()); } SOPHUS_FUNC Transformation jr_inv() { return jl_inv(-log()); } SOPHUS_FUNC static Transformation jr_inv(const Point& Omega) { return jl_inv(-Omega); } SOPHUS_FUNC static Transformation jr_inv(const SO3Base& R) { return jr_inv(R.log()); } private: /// Mutator of unit_quaternion is private to ensure class invariant. That is /// the quaternion must stay close to unit length. /// SOPHUS_FUNC QuaternionType& unit_quaternion_nonconst() { return static_cast(this)->unit_quaternion_nonconst(); } }; /// SO3 using default storage; derived from SO3Base. template class SO3 : public SO3Base> { public: using Base = SO3Base>; static int constexpr DoF = Base::DoF; static int constexpr num_parameters = Base::num_parameters; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using QuaternionMember = Eigen::Quaternion; /// ``Base`` is friend so unit_quaternion_nonconst can be accessed from /// ``Base``. friend class SO3Base>; EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Default constructor initializes unit quaternion to identity rotation. /// SOPHUS_FUNC SO3() : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {} /// Copy constructor /// SOPHUS_FUNC SO3(SO3 const& other) = default; /// Copy-like constructor from OtherDerived. /// template SOPHUS_FUNC SO3(SO3Base const& other) : unit_quaternion_(other.unit_quaternion()) {} /// Constructor from rotation matrix /// /// Precondition: rotation matrix needs to be orthogonal with determinant /// of 1. /// SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) { SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R * R.transpose()); SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %", R.determinant()); } /// Constructor from quaternion /// /// Precondition: quaternion must not be close to zero. /// template SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase const& quat) : unit_quaternion_(quat) { static_assert(std::is_same::Scalar, Scalar>::value, "Input must be of same scalar type"); Base::normalize(); } /// Accessor of unit quaternion. /// SOPHUS_FUNC QuaternionMember const& unit_quaternion() const { return unit_quaternion_; } /// Returns derivative of exp(x) wrt. x. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x(Tangent const& omega) { using std::cos; using std::exp; using std::sin; using std::sqrt; Scalar const c0 = omega[0] * omega[0]; Scalar const c1 = omega[1] * omega[1]; Scalar const c2 = omega[2] * omega[2]; Scalar const c3 = c0 + c1 + c2; if (c3 < Constants::epsilon()) { return Dx_exp_x_at_0(); } Scalar const c4 = sqrt(c3); Scalar const c5 = 1.0 / c4; Scalar const c6 = 0.5 * c4; Scalar const c7 = sin(c6); Scalar const c8 = c5 * c7; Scalar const c9 = pow(c3, -3.0L / 2.0L); Scalar const c10 = c7 * c9; Scalar const c11 = Scalar(1.0) / c3; Scalar const c12 = cos(c6); Scalar const c13 = Scalar(0.5) * c11 * c12; Scalar const c14 = c7 * c9 * omega[0]; Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0]; Scalar const c16 = -c14 * omega[1] + c15 * omega[1]; Scalar const c17 = -c14 * omega[2] + c15 * omega[2]; Scalar const c18 = omega[1] * omega[2]; Scalar const c19 = -c10 * c18 + c13 * c18; Scalar const c20 = Scalar(0.5) * c5 * c7; Sophus::Matrix J; J(0, 0) = -c0 * c10 + c0 * c13 + c8; J(0, 1) = c16; J(0, 2) = c17; J(1, 0) = c16; J(1, 1) = -c1 * c10 + c1 * c13 + c8; J(1, 2) = c19; J(2, 0) = c17; J(2, 1) = c19; J(2, 2) = -c10 * c2 + c13 * c2 + c8; J(3, 0) = -c20 * omega[0]; J(3, 1) = -c20 * omega[1]; J(3, 2) = -c20 * omega[2]; return J; } /// Returns derivative of exp(x) wrt. x_i at x=0. /// SOPHUS_FUNC static Sophus::Matrix Dx_exp_x_at_0() { Sophus::Matrix J; // clang-format off J << Scalar(0.5), Scalar(0), Scalar(0), Scalar(0), Scalar(0.5), Scalar(0), Scalar(0), Scalar(0), Scalar(0.5), Scalar(0), Scalar(0), Scalar(0); // clang-format on return J; } /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``. /// SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); } /// Group exponential /// /// This functions takes in an element of tangent space (= rotation vector /// ``omega``) and returns the corresponding element of the group SO(3). /// /// To be more specific, this function computes ``expmat(hat(omega))`` /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the /// hat()-operator of SO(3). /// SOPHUS_FUNC static SO3 exp(Tangent const& omega) { Scalar theta; return expAndTheta(omega, &theta); } /// As above, but also returns ``theta = |omega|`` as out-parameter. /// /// Precondition: ``theta`` must not be ``nullptr``. /// SOPHUS_FUNC static SO3 expAndTheta(Tangent const& omega, Scalar* theta) { SOPHUS_ENSURE(theta != nullptr, "must not be nullptr."); using std::abs; using std::cos; using std::sin; using std::sqrt; Scalar theta_sq = omega.squaredNorm(); Scalar imag_factor; Scalar real_factor; if (theta_sq < Constants::epsilon() * Constants::epsilon()) { *theta = Scalar(0); Scalar theta_po4 = theta_sq * theta_sq; imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq + Scalar(1.0 / 3840.0) * theta_po4; real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq + Scalar(1.0 / 384.0) * theta_po4; } else { *theta = sqrt(theta_sq); Scalar half_theta = Scalar(0.5) * (*theta); Scalar sin_half_theta = sin(half_theta); imag_factor = sin_half_theta / (*theta); real_factor = cos(half_theta); } SO3 q; q.unit_quaternion_nonconst() = QuaternionMember(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) < Sophus::Constants::epsilon(), "SO3::exp failed! omega: %, real: %, img: %", omega.transpose(), real_factor, imag_factor); return q; } /// Returns closest SO3 given arbitrary 3x3 matrix. /// template static SOPHUS_FUNC enable_if_t::value, SO3> fitToSO3(Transformation const& R) { return SO3(::Sophus::makeRotationMatrix(R)); } /// Returns the ith infinitesimal generators of SO(3). /// /// The infinitesimal generators of SO(3) are: /// /// ``` /// | 0 0 0 | /// G_0 = | 0 0 -1 | /// | 0 1 0 | /// /// | 0 0 1 | /// G_1 = | 0 0 0 | /// | -1 0 0 | /// /// | 0 -1 0 | /// G_2 = | 1 0 0 | /// | 0 0 0 | /// ``` /// /// Precondition: ``i`` must be 0, 1 or 2. /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= 2, "i should be in range [0,2]."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); } /// Lie bracket /// /// It computes the Lie bracket of SO(3). To be more specific, it computes /// /// ``[omega_1, omega_2]_so3 := vee([hat(omega_1), hat(omega_2)])`` /// /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the /// hat()-operator and ``vee(.)`` the vee()-operator of SO3. /// /// For the Lie algebra so3, the Lie bracket is simply the cross product: /// /// ``[omega_1, omega_2]_so3 = omega_1 x omega_2.`` /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1, Tangent const& omega2) { return omega1.cross(omega2); } /// Construct x-axis rotation. /// static SOPHUS_FUNC SO3 rotX(Scalar const& x) { return SO3::exp(Sophus::Vector3(x, Scalar(0), Scalar(0))); } /// Construct y-axis rotation. /// static SOPHUS_FUNC SO3 rotY(Scalar const& y) { return SO3::exp(Sophus::Vector3(Scalar(0), y, Scalar(0))); } /// Construct z-axis rotation. /// static SOPHUS_FUNC SO3 rotZ(Scalar const& z) { return SO3::exp(Sophus::Vector3(Scalar(0), Scalar(0), z)); } /// Draw uniform sample from SO(3) manifold. /// Based on: http://planning.cs.uiuc.edu/node198.html /// template static SO3 sampleUniform(UniformRandomBitGenerator& generator) { static_assert(IsUniformRandomBitGenerator::value, "generator must meet the UniformRandomBitGenerator concept"); std::uniform_real_distribution uniform(Scalar(0), Scalar(1)); std::uniform_real_distribution uniform_twopi(Scalar(0), 2 * Constants::pi()); const Scalar u1 = uniform(generator); const Scalar u2 = uniform_twopi(generator); const Scalar u3 = uniform_twopi(generator); const Scalar a = sqrt(1 - u1); const Scalar b = sqrt(u1); return SO3(QuaternionMember(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3))); } /// vee-operator /// /// It takes the 3x3-matrix representation ``Omega`` and maps it to the /// corresponding vector representation of Lie algebra. /// /// This is the inverse of the hat()-operator, see above. /// /// Precondition: ``Omega`` must have the following structure: /// /// | 0 -c b | /// | c 0 -a | /// | -b a 0 | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0)); } protected: /// Mutator of unit_quaternion is protected to ensure class invariant. /// SOPHUS_FUNC QuaternionMember& unit_quaternion_nonconst() { return unit_quaternion_; } QuaternionMember unit_quaternion_; }; } // namespace Sophus namespace Eigen { /// Specialization of Eigen::Map for ``SO3``; derived from SO3Base. /// /// Allows us to wrap SO3 objects around POD array (e.g. external c style /// quaternion). template class Map, Options> : public Sophus::SO3Base, Options>> { public: using Base = Sophus::SO3Base, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; /// ``Base`` is friend so unit_quaternion_nonconst can be accessed from /// ``Base``. friend class Sophus::SO3Base, Options>>; // LCOV_EXCL_START SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map) // LCOV_EXCL_STOP using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {} /// Accessor of unit quaternion. /// SOPHUS_FUNC Map, Options> const& unit_quaternion() const { return unit_quaternion_; } protected: /// Mutator of unit_quaternion is protected to ensure class invariant. /// SOPHUS_FUNC Map, Options>& unit_quaternion_nonconst() { return unit_quaternion_; } Map, Options> unit_quaternion_; }; /// Specialization of Eigen::Map for ``SO3 const``; derived from SO3Base. /// /// Allows us to wrap SO3 objects around POD array (e.g. external c style /// quaternion). template class Map const, Options> : public Sophus::SO3Base const, Options>> { public: using Base = Sophus::SO3Base const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint; using Base::operator*=; using Base::operator*; SOPHUS_FUNC Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {} /// Accessor of unit quaternion. /// SOPHUS_FUNC Map const, Options> const& unit_quaternion() const { return unit_quaternion_; } protected: /// Mutator of unit_quaternion is protected to ensure class invariant. /// Map const, Options> const unit_quaternion_; }; } // namespace Eigen #endif ================================================ FILE: thirdparty/Sophus/types.hpp ================================================ /// @file /// Common type aliases. #ifndef SOPHUS_TYPES_HPP #define SOPHUS_TYPES_HPP #include #include "common.hpp" namespace Sophus { template using Vector = Eigen::Matrix; template using Vector2 = Vector; using Vector2f = Vector2; using Vector2d = Vector2; template using Vector3 = Vector; using Vector3f = Vector3; using Vector3d = Vector3; template using Vector4 = Vector; using Vector4f = Vector4; using Vector4d = Vector4; template using Vector6 = Vector; using Vector6f = Vector6; using Vector6d = Vector6; template using Vector7 = Vector; using Vector7f = Vector7; using Vector7d = Vector7; template using Matrix = Eigen::Matrix; template using Matrix2 = Matrix; using Matrix2f = Matrix2; using Matrix2d = Matrix2; template using Matrix3 = Matrix; using Matrix3f = Matrix3; using Matrix3d = Matrix3; template using Matrix4 = Matrix; using Matrix4f = Matrix4; using Matrix4d = Matrix4; template using Matrix6 = Matrix; using Matrix6f = Matrix6; using Matrix6d = Matrix6; template using Matrix7 = Matrix; using Matrix7f = Matrix7; using Matrix7d = Matrix7; template using ParametrizedLine = Eigen::ParametrizedLine; template using ParametrizedLine3 = ParametrizedLine; using ParametrizedLine3f = ParametrizedLine3; using ParametrizedLine3d = ParametrizedLine3; template using ParametrizedLine2 = ParametrizedLine; using ParametrizedLine2f = ParametrizedLine2; using ParametrizedLine2d = ParametrizedLine2; namespace details { template class MaxMetric { public: static Scalar impl(Scalar s0, Scalar s1) { using std::abs; return abs(s0 - s1); } }; template class MaxMetric> { public: static Scalar impl(Matrix const& p0, Matrix const& p1) { return (p0 - p1).template lpNorm(); } }; template class SetToZero { public: static void impl(Scalar& s) { s = Scalar(0); } }; template class SetToZero> { public: static void impl(Matrix& v) { v.setZero(); } }; template class SetElementAt; template class SetElementAt { public: static void impl(Scalar& s, Scalar value, int at) { SOPHUS_ENSURE(at == 0, "is %", at); s = value; } }; template class SetElementAt, Scalar> { public: static void impl(Vector& v, Scalar value, int at) { SOPHUS_ENSURE(at >= 0 && at < N, "is %", at); v[at] = value; } }; template class SquaredNorm { public: static Scalar impl(Scalar const& s) { return s * s; } }; template class SquaredNorm> { public: static Scalar impl(Matrix const& s) { return s.squaredNorm(); } }; template class Transpose { public: static Scalar impl(Scalar const& s) { return s; } }; template class Transpose> { public: static Matrix impl(Matrix const& s) { return s.transpose(); } }; } // namespace details /// Returns maximum metric between two points ``p0`` and ``p1``, with ``p0, p1`` /// being matrices or a scalars. /// template auto maxMetric(T const& p0, T const& p1) -> decltype(details::MaxMetric::impl(p0, p1)) { return details::MaxMetric::impl(p0, p1); } /// Sets point ``p`` to zero, with ``p`` being a matrix or a scalar. /// template void setToZero(T& p) { return details::SetToZero::impl(p); } /// Sets ``i``th component of ``p`` to ``value``, with ``p`` being a /// matrix or a scalar. If ``p`` is a scalar, ``i`` must be ``0``. /// template void setElementAt(T& p, Scalar value, int i) { return details::SetElementAt::impl(p, value, i); } /// Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar. /// template auto squaredNorm(T const& p) -> decltype(details::SquaredNorm::impl(p)) { return details::SquaredNorm::impl(p); } /// Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a /// scalar. /// template auto transpose(T const& p) -> decltype(details::Transpose::impl(T())) { return details::Transpose::impl(p); } template struct IsFloatingPoint { static bool const value = std::is_floating_point::value; }; template struct IsFloatingPoint> { static bool const value = std::is_floating_point::value; }; template struct GetScalar { using Scalar = Scalar_; }; template struct GetScalar> { using Scalar = Scalar_; }; /// If the Vector type is of fixed size, then IsFixedSizeVector::value will be /// true. template ::type> struct IsFixedSizeVector : std::true_type {}; /// Planes in 3d are hyperplanes. template using Plane3 = Eigen::Hyperplane; using Plane3d = Plane3; using Plane3f = Plane3; /// Lines in 2d are hyperplanes. template using Line2 = Eigen::Hyperplane; using Line2d = Line2; using Line2f = Line2; } // namespace Sophus #endif // SOPHUS_TYPES_HPP ================================================ FILE: thirdparty/Sophus/velocities.hpp ================================================ #ifndef SOPHUS_VELOCITIES_HPP #define SOPHUS_VELOCITIES_HPP #include #include "num_diff.hpp" #include "se3.hpp" namespace Sophus { namespace experimental { // Experimental since the API will certainly change drastically in the future. // Transforms velocity vector by rotation ``foo_R_bar``. // // Note: vel_bar can be either a linear or a rotational velocity vector. // template Vector3 transformVelocity(SO3 const& foo_R_bar, Vector3 const& vel_bar) { // For rotational velocities note that: // // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) // = vee(hat(Adj(foo_R_bar) * vel_bar)) // = Adj(foo_R_bar) * vel_bar // = foo_R_bar * vel_bar. // return foo_R_bar * vel_bar; } // Transforms velocity vector by pose ``foo_T_bar``. // // Note: vel_bar can be either a linear or a rotational velocity vector. // template Vector3 transformVelocity(SE3 const& foo_T_bar, Vector3 const& vel_bar) { return transformVelocity(foo_T_bar.so3(), vel_bar); } // finite difference approximation of instantanious velocity in frame foo // template Vector3 finiteDifferenceRotationalVelocity( std::function(Scalar)> const& foo_R_bar, Scalar t, Scalar h = Constants::epsilon()) { // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor // // W = dR(t)/dt * R^{-1}(t) Matrix3 dR_dt_in_frame_foo = curveNumDiff( [&foo_R_bar](Scalar t0) -> Matrix3 { return foo_R_bar(t0).matrix(); }, t, h); // velocity tensor Matrix3 W_in_frame_foo = dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); return SO3::vee(W_in_frame_foo); } // finite difference approximation of instantanious velocity in frame foo // template Vector3 finiteDifferenceRotationalVelocity( std::function(Scalar)> const& foo_T_bar, Scalar t, Scalar h = Constants::epsilon()) { return finiteDifferenceRotationalVelocity( [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, h); } } // namespace experimental } // namespace Sophus #endif // SOPHUS_VELOCITIES_HPP ================================================ FILE: thirdparty/livox_ros_driver/CMakeLists.txt ================================================ # Copyright(c) 2020 livoxtech limited. cmake_minimum_required(VERSION 3.14) project(livox_ros_driver2) # Default to C99 if (NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif () # Default to C++14 if (NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif () list(INSERT CMAKE_MODULE_PATH 0 "${PROJECT_SOURCE_DIR}/cmake/modules") if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) endif () # Printf version info # include(cmake/version.cmake) project(${PROJECT_NAME} VERSION ${LIVOX_ROS_DRIVER2_VERSION} LANGUAGES CXX) message(STATUS "${PROJECT_NAME} version: ${LIVOX_ROS_DRIVER2_VERSION}") #--------------------------------------------------------------------------------------- # Add ROS Version MACRO #--------------------------------------------------------------------------------------- add_definitions(-DBUILDING_ROS2) # find dependencies # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() find_package(PCL REQUIRED) find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) # check apr find_package(PkgConfig) pkg_check_modules(APR apr-1) if (APR_FOUND) message(${APR_INCLUDE_DIRS}) message(${APR_LIBRARIES}) endif (APR_FOUND) # generate custom msg headers set(LIVOX_INTERFACES livox_interfaces2) rosidl_generate_interfaces(${LIVOX_INTERFACES} "msg/CustomPoint.msg" "msg/CustomMsg.msg" DEPENDENCIES builtin_interfaces std_msgs LIBRARY_NAME ${PROJECT_NAME} ) ## make sure the livox_lidar_sdk_shared library is installed # find_library(LIVOX_LIDAR_SDK_LIBRARY liblivox_lidar_sdk_shared.so /usr/local/lib REQUIRED) ## # find_path(LIVOX_LIDAR_SDK_INCLUDE_DIR # NAMES "livox_lidar_api.h" "livox_lidar_def.h" # REQUIRED) ## PCL library link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) # livox ros2 driver target # ament_auto_add_library(${PROJECT_NAME} SHARED # src/livox_ros_driver2.cpp # src/lddc.cpp # src/driver_node.cpp # src/lds.cpp # src/lds_lidar.cpp # # src/comm/comm.cpp # src/comm/ldq.cpp # src/comm/semaphore.cpp # src/comm/lidar_imu_data_queue.cpp # src/comm/cache_index.cpp # src/comm/pub_handler.cpp # # src/parse_cfg_file/parse_cfg_file.cpp # src/parse_cfg_file/parse_livox_lidar_cfg.cpp # # src/call_back/lidar_common_callback.cpp # src/call_back/livox_lidar_callback.cpp # ) # # target_include_directories(${PROJECT_NAME} PRIVATE ${livox_sdk_INCLUDE_DIRS}) # get include directories of custom msg headers # if (HUMBLE_ROS STREQUAL "humble") # rosidl_get_typesupport_target(cpp_typesupport_target # ${LIVOX_INTERFACES} "rosidl_typesupport_cpp") # target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}") # else () # set(LIVOX_INTERFACE_TARGET "${LIVOX_INTERFACES}__rosidl_typesupport_cpp") # add_dependencies(${PROJECT_NAME} ${LIVOX_INTERFACES}) # get_target_property(LIVOX_INTERFACES_INCLUDE_DIRECTORIES ${LIVOX_INTERFACE_TARGET} INTERFACE_INCLUDE_DIRECTORIES) # endif () # include file direcotry # target_include_directories(${PROJECT_NAME} PUBLIC # ${PCL_INCLUDE_DIRS} # ${APR_INCLUDE_DIRS} # ${LIVOX_LIDAR_SDK_INCLUDE_DIR} # ${LIVOX_INTERFACES_INCLUDE_DIRECTORIES} # for custom msgs # 3rdparty # src # ) # link libraries ## target_link_libraries(${PROJECT_NAME} ## ${LIVOX_LIDAR_SDK_LIBRARY} ## ${LIVOX_INTERFACE_TARGET} # for custom msgs ## ${PPT_LIBRARY} ## ${Boost_LIBRARY} ## ${PCL_LIBRARIES} ## ${APR_LIBRARIES} ## ) ## ## rclcpp_components_register_node(${PROJECT_NAME} ## PLUGIN "livox_ros::DriverNode" ## EXECUTABLE ${PROJECT_NAME}_node ## ) ## # ament_auto_package(INSTALL_TO_SHARE # config # launch_ROS2 # ) ================================================ FILE: thirdparty/livox_ros_driver/msg/CustomMsg.msg ================================================ # Livox publish pointcloud msg format. std_msgs/Header header # ROS standard message header uint64 timebase # The time of first point uint32 point_num # Total number of pointclouds uint8 lidar_id # Lidar device id number uint8[3] rsvd # Reserved use CustomPoint[] points # Pointcloud data ================================================ FILE: thirdparty/livox_ros_driver/msg/CustomPoint.msg ================================================ # Livox costom pointcloud format. uint32 offset_time # offset time relative to the base time float32 x # X axis, unit:m float32 y # Y axis, unit:m float32 z # Z axis, unit:m uint8 reflectivity # reflectivity, 0~255 uint8 tag # livox tag uint8 line # laser number in lidar ================================================ FILE: thirdparty/livox_ros_driver/package.xml ================================================ livox_ros_driver2 1.0.0 The ROS device driver for Livox 3D LiDARs, for ROS2 feng MIT ament_cmake_auto rosidl_default_generators rosidl_interface_packages rclcpp rclcpp_components std_msgs sensor_msgs rcutils pcl_conversions rcl_interfaces libpcl-all-dev rosbag2 rosidl_default_runtime ament_lint_auto ament_lint_common git apr ament_cmake