Showing preview only (1,275K chars total). Download the full file or copy to clipboard to get everything.
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: '^<ext/.*\.h>'
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<bool>` 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:
- Localization on VBR
- Map on VBR
- Point Cloud
- Grid Map
- Localization on the NCLT dataset
- Data on the Deep Robotics quadruped robot
- Tilted mounting demo
## 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
[](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<bool>在并行化时的问题
- 修正了几处可能导致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数据集上的建图：
- VBR上的定位
- VBR上的地图
- 点云
- 栅格
- NCLT 数据集上的定位
- 云深处四足机械狗上的数据
- 斜装的DEMO
## 编译
### 环境
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
[](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
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lightning</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="gao.xiang.thu@gmail.com">gaoxiang</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<depend>scrubber_common</depend>
<depend>agibot_robot</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<depend>message_filters</depend>
<depend>visualization_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
================================================
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 <gflags/gflags.h>
#include <glog/logging.h>
#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 <opencv2/opencv.hpp>
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::PangolinWindow>();
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 <gflags/gflags.h>
#include <glog/logging.h>
#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<std::string>("common", "lidar_topic");
std::string imu_topic = yaml.GetValue<std::string>("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 <gflags/gflags.h>
#include <glog/logging.h>
#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 <gflags/gflags.h>
#include <glog/logging.h>
#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::PangolinWindow>();
ui->Init();
lio.SetUI(ui);
auto loop = std::make_shared<LoopClosing>();
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 <gflags/gflags.h>
#include <glog/logging.h>
#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<std::string>("common", "lidar_topic");
std::string imu_topic = yaml.GetValue<std::string>("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 <gflags/gflags.h>
#include <glog/logging.h>
#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 <pangolin/display/display.h>
#include <pangolin/display/view.h>
#include <pangolin/gl/gldraw.h>
#include <pangolin/handler/handler.h>
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 <Eigen/Core>
#include <Eigen/Geometry>
#include <cfloat>
#include <memory>
#include <mutex>
#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<float, 4, 1>;
using Vec6f = Eigen::Matrix<float, 6, 1>;
using Vec2d = Eigen::Vector2d;
using Vec3d = Eigen::Vector3d;
using Vec4d = Eigen::Vector4d;
using Vec6d = Eigen::Matrix<double, 6, 1>;
using Vec7d = Eigen::Matrix<double, 7, 1>;
using Vec12d = Eigen::Matrix<double, 12, 1>;
using Vec15d = Eigen::Matrix<double, 15, 1>;
using VecXd = Eigen::Matrix<double, Eigen::Dynamic, 1>;
using Mat2f = Eigen::Matrix<float, 2, 2>;
using Mat3f = Eigen::Matrix<float, 3, 3>;
using Mat4f = Eigen::Matrix<float, 4, 4>;
using Mat6f = Eigen::Matrix<float, 6, 6>;
using Mat1d = Eigen::Matrix<double, 1, 1>;
using Mat2d = Eigen::Matrix<double, 2, 2>;
using Mat3d = Eigen::Matrix<double, 3, 3>;
using Mat4d = Eigen::Matrix<double, 4, 4>;
using Mat6d = Eigen::Matrix<double, 6, 6>;
using Mat12d = Eigen::Matrix<double, 12, 12>;
using Mat36f = Eigen::Matrix<float, 3, 6>;
using Mat23f = Eigen::Matrix<float, 2, 3>;
using Mat66f = Eigen::Matrix<float, 6, 6>;
using MatXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
template <int N, typename T = double>
using VectorN = Eigen::Matrix<T, N, 1, Eigen::ColMajor>;
using Vector2 = VectorN<2>;
using Vector3 = VectorN<3>;
using Vector4 = VectorN<4>;
using Vector6 = VectorN<6>;
using Vector7 = VectorN<7>;
using VectorX = VectorN<Eigen::Dynamic>;
template <int N, typename T = double>
using MatrixN = Eigen::Matrix<T, N, N, Eigen::ColMajor>;
using Matrix2 = MatrixN<2>;
using Matrix3 = MatrixN<3>;
using Matrix4 = MatrixN<4>;
using MatrixX = MatrixN<Eigen::Dynamic>;
using H6d = Eigen::Matrix<double, 1, 6>;
using H1d = Eigen::Matrix<double, 1, 1>;
using Vec2i = Eigen::Vector2i;
using Vec3i = Eigen::Vector3i;
using Aff3d = Eigen::Affine3d;
using Quatd = Eigen::Quaternion<double>;
using Trans3d = Eigen::Translation3d;
using AngAxisd = Eigen::AngleAxis<double>;
using Quat = Quatd;
using Aff3f = Eigen::Affine3f;
using Quatf = Eigen::Quaternion<float>;
using Trans3f = Eigen::Translation3f;
using AngAxisf = Eigen::AngleAxis<float>;
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<IMU>;
} // 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() {}
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 <deque>
#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<IMUPtr> imu_; // 两个scan之间的IMU测量
std::deque<OdomPtr> 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::MetaInfo> 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::MetaInfo> 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 <glog/logging.h>
#include <iomanip>
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<double, dim, 1>; // 矢量形式
using FullVectState = Eigen::Matrix<double, full_dim, 1>; // 全状态矢量形式
NavState() = default;
bool operator<(const NavState& other) { return timestamp_ < other.timestamp_; }
FullVectState ToState() {
FullVectState ret;
ret.block<kBlockDim, 1>(kPosIdx, 0) = pos_;
ret.block<kBlockDim, 1>(kRotIdx, 0) = rot_.log();
ret.block<kBlockDim, 1>(kVelIdx, 0) = vel_;
ret.block<kBlockDim, 1>(kBgIdx, 0) = bg_;
return ret;
}
void FromVectState(const FullVectState& state) {
pos_ = state.block<kBlockDim, 1>(kPosIdx, 0);
rot_ = SO3::exp(state.block<kBlockDim, 1>(kRotIdx, 0));
vel_ = state.block<kBlockDim, 1>(kVelIdx, 0);
bg_ = state.block<kBlockDim, 1>(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<double, full_dim, dim> df_dx(const Vec3d& acce) const {
Eigen::Matrix<double, full_dim, dim> cov = Eigen::Matrix<double, full_dim, dim>::Zero();
cov.block<kBlockDim, kBlockDim>(kPosIdx, kVelIdx) = Mat3d::Identity();
Vec3d acc = acce;
// Vec3d omega = gyro - bg_;
cov.block<kBlockDim, kBlockDim>(kVelIdx, kRotIdx) = -rot_.matrix() * SO3::hat(acc);
cov.block<kBlockDim, kBlockDim>(kRotIdx, kBgIdx) = -Eigen::Matrix3d::Identity();
return cov;
}
/// 运动方程对噪声的雅可比
inline Eigen::Matrix<double, full_dim, 12> df_dw() const {
Eigen::Matrix<double, full_dim, 12> cov = Eigen::Matrix<double, full_dim, 12>::Zero();
cov.block<kBlockDim, kBlockDim>(kVelIdx, 3) = -rot_.matrix();
cov.block<kBlockDim, kBlockDim>(kRotIdx, 0) = -Eigen::Matrix3d::Identity();
cov.block<kBlockDim, kBlockDim>(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<kBlockDim, 1>(kPosIdx, 0) = pos_ - other.pos_;
result.block<kBlockDim, 1>(kRotIdx, 0) = (other.rot_.inverse() * rot_).log();
result.block<kBlockDim, 1>(kVelIdx, 0) = vel_ - other.vel_;
result.block<kBlockDim, 1>(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<MetaInfo> vect_states_; // 矢量变量的维度
static const std::vector<MetaInfo> 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<Odom>;
} // namespace lightning
#endif // LIGHTNING_ODOM_H
================================================
FILE: src/common/options.cc
================================================
//
// Created by xiang on 24-4-8.
//
#include <common/options.h>
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 <string>
#include <common/constant.h>
#include <common/eigen_types.h>
#include <rclcpp/rclcpp.hpp>
/// 配置参数
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<double> Tol_ini;
Sophus::SE3d Toi;
try {
std::string front_type = yaml["frontend_type"].as<std::string>();
frontend_ = FrontEndType::FASTER_LIO;
process_cloud_in_step = yaml["process_cloud_in_step"].as<bool>();
online_mode = yaml["online_mode"].as<bool>();
with_ui_ = yaml["with_ui"].as<bool>();
enable_backend_ = yaml["enable_backend"].as<bool>();
enable_frontend_log_ = yaml["enable_frontend_log"].as<bool>();
enable_backend_log_ = yaml["enable_backend_log"].as<bool>();
is_vis_occupancy_map_ = yaml["is_vis_occupancy_map"].as<bool>();
enable_balm_ = yaml["enable_balm"].as<bool>();
point_cloud_topic_ = yaml["pointCloudTopic"].as<std::string>();
imu_topic_ = yaml["imuTopic"].as<std::string>();
odom_topic_ = yaml["odomTopic"].as<std::string>();
save_pcd_dir_ = yaml["savePCDDirectory"].as<std::string>();
baselink_frame_ = yaml["agi_sam"]["baselinkFrame"].as<std::string>();
odom_frame_ = yaml["agi_sam"]["odometryFrame"].as<std::string>();
map_frame_ = yaml["agi_sam"]["mapFrame"].as<std::string>();
save_map_resolution_ = yaml["agi_sam"]["save_map_resolution"].as<float>();
std::string sensorStr = yaml["agi_sam"]["sensor"].as<std::string>();
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<float>();
lidar_max_range_ = yaml["agi_sam"]["lidarMaxRange"].as<float>();
kf_dist_th_ = yaml["agi_sam"]["surroundingkeyframeAddingDistThreshold"].as<float>();
kf_angle_th_ = yaml["agi_sam"]["surroundingkeyframeAddingAngleThreshold"].as<float>();
imu_type_ = yaml["agi_sam"]["imuType"].as<int>();
use_fasterlio_undistort_ = yaml["use_fasterlio_undistort"].as<bool>();
relative_cloud_pt_time_ = yaml["relative_cloud_pt_time"].as<bool>();
Tol_ini = yaml["mapping"]["Tol"].as<std::vector<double>>();
auto extrinT = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
auto extrinR = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
lidar_T_wrt_IMU = math::VecFromArray<double>(extrinT);
lidar_R_wrt_IMU = math::MatFromArray<double>(extrinR);
auto Tol_eig = math::Mat4FromArray<double>(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 <string>
#include <vector>
#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 <pcl/point_cloud.h>
#include <pcl/point_types.h>
/// 地图点云的一些定义
/// 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<PointType>;
using CloudPtr = PointCloudType::Ptr;
using PointVec = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
inline Vec3f ToVec3f(const PointType& pt) { return pt.getVector3fMap(); }
inline Vec3d ToVec3d(const PointType& pt) { return pt.getVector3fMap().cast<double>(); }
/// 带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<FullPointType>;
using FullCloudPtr = FullPointCloudType::Ptr;
inline Vec3f ToVec3f(const FullPointType& pt) { return pt.getVector3fMap(); }
inline Vec3d ToVec3d(const FullPointType& pt) { return pt.getVector3fMap().cast<double>(); }
using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
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 <typename T>
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<double>;
using PoseRPYF = PoseRPY<float>;
}
#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<double, 3, 3> S2_hat() const {
Eigen::Matrix<double, 3, 3> 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<double, 3, 2> S2_Mx(const Eigen::Matrix<double, 2, 1> &delta) const {
Eigen::Matrix<double, 3, 2> res;
Eigen::Matrix<double, 3, 2> 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<double, 3, 2> S2_Bx() const {
Eigen::Matrix<double, 3, 2> 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<double, 3, 2>::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<double, 2, 3> S2_Nx_yy() const {
Eigen::Matrix<double, 2, 3> res;
Eigen::Matrix<double, 3, 2> 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<double, 3, 2> 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<double, 3, 2> 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 <mutex>
#include <thread>
namespace lightning {
using UL = std::unique_lock<std::mutex>;
}
#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 <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <map>
#include <pcl/filters/impl/voxel_grid.hpp>
#include <pcl/segmentation/impl/sac_segmentation.hpp>
#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<Keyframe::Ptr> 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<G2P5Map>(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<Keyframe::Ptr> 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<Keyframe::Ptr> &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<double>();
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<int>((floor)(min_x / r)) * r;
min_y = static_cast<int>((floor)(min_y / r)) * r;
max_x = static_cast<int>((ceil)(max_x / r)) * r;
max_y = static_cast<int>((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<Keyframe::Ptr> &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<std::map<double, double>> rays(360); // map键值：距离-相对高度（以距离排序）
std::vector<Vec2d> angle_distance_height(360, Vec2d::Zero()); // 每个角度上的距离-高度值
std::vector<Vec3d> 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<double> 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<Vec2d> &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<PointType> 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<bool>();
options_.min_th_floor_ = yaml["g2p5"]["min_th_floor"].as<float>();
options_.max_th_floor_ = yaml["g2p5"]["max_th_floor"].as<float>();
options_.lidar_height_ = yaml["g2p5"]["lidar_height"].as<float>();
options_.grid_map_resolution_ = yaml["g2p5"]["grid_map_resolution"].as<float>();
options_.default_floor_height_ = yaml["g2p5"]["floor_height"].as<float>();
G2P5Map::Options opt;
opt.resolution_ = options_.grid_map_resolution_;
frontend_map_ = std::make_shared<G2P5Map>(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 <thread>
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<void(G2P5MapPtr map)>;
/// 从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<Keyframe::Ptr> &kfs, G2P5MapPtr &map);
/// 对地图进行缩放
bool ResizeMap(const std::vector<Keyframe::Ptr> &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<Vec2d> &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<Keyframe::Ptr> 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<Keyframe::Ptr> 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 <malloc.h>
#include <cstdlib>
#include <execution>
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> G2P5Map::MakeDeepCopy() {
std::shared_ptr<G2P5Map> 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<Vec2i> updated_pts;
std::vector<float> 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<nav_msgs::msg::MapMetaData::_resolution_type>(options_.resolution_);
occu_map.info.width = static_cast<nav_msgs::msg::MapMetaData::_width_type>(image_width);
occu_map.info.height = static_cast<nav_msgs::msg::MapMetaData::_width_type>(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<cv::Vec3b>(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<cv::Vec3b>(extend_y, extend_x) == other_color) {
image.at<cv::Vec3b>(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 <bitset>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <opencv2/core.hpp>
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<G2P5Map> 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<G2P5Map> 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 <cstring>
#include <memory>
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 <algorithm>
#include <array>
#include <cstdint>
#include <limits>
#include <type_traits>
// 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<typename T, size_t N>
// std::array<T, N>
// IndexToPosition(std::array<T, N> 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<typename T, size_t N>
// std::array<T, N>
// PositionToIndex(std::array<T, N> 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 <typename T, size_t N>
std::array<T, N> UntransposeBits(std::array<T, N> const &in) {
const size_t bits = std::numeric_limits<T>::digits;
const T high_bit(T(1) << (bits - 1));
const size_t bit_count(bits * N);
std::array<T, N> 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 <typename T, size_t N>
std::array<T, N> TransposeBits(std::array<T, N> const &in) {
const size_t bits = std::numeric_limits<T>::digits;
const T high_bit(T(1) << (bits - 1));
const size_t bit_count(bits * N);
std::array<T, N> 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 <typename T, size_t N>
std::array<T, N> IndexToPosition(std::array<T, N> const &in) {
// First convert index to transpose.
std::array<T, N> 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 <typename T, size_t N>
std::array<T, N> PositionToIndex(std::array<T, N> const &in) {
const size_t bits = std::numeric_limits<T>::digits;
std::array<T, N> 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 <typename T, size_t N, size_t D>
T TransposeBits2(std::array<T, N> const &, std::integral_constant<size_t, D>, std::integral_constant<size_t, 0>) {
return T(0);
}
template <typename T, size_t N, size_t D, size_t B>
T TransposeBits2(std::array<T, N> const &in, std::integral_constant<size_t, D>, std::integral_constant<size_t, B>) {
const size_t size = std::numeric_limits<T>::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<size_t, D>(), std::integral_constant<size_t, B - 1>());
}
template <typename T, size_t N>
void TransposeBits(std::array<T, N> const &, std::array<T, N> &, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t D>
void TransposeBits(std::array<T, N> const &in, std::array<T, N> &out, std::integral_constant<size_t, D>) {
out[D - 1] = TransposeBits2(in, std::integral_constant<size_t, D - 1>(),
std::integral_constant<size_t, std::numeric_limits<T>::digits>());
TransposeBits(in, out, std::integral_constant<size_t, D - 1>());
}
template <typename T, size_t N, size_t D>
T UntransposeBits2(std::array<T, N> const &, std::integral_constant<size_t, D>, std::integral_constant<size_t, 0>) {
return T(0);
}
template <typename T, size_t N, size_t D, size_t B>
T UntransposeBits2(std::array<T, N> const &in, std::integral_constant<size_t, D>, std::integral_constant<size_t, B>) {
const size_t size = std::numeric_limits<T>::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<size_t, D>(), std::integral_constant<size_t, size_t(B - 1)>());
}
template <typename T, size_t N>
void UntransposeBits(std::array<T, N> const &, std::array<T, N> &, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t D>
void UntransposeBits(std::array<T, N> const &in, std::array<T, N> &out, std::integral_constant<size_t, D>) {
out[D - 1] = UntransposeBits2(in, std::integral_constant<size_t, D - 1>(),
std::integral_constant<size_t, std::numeric_limits<T>::digits>());
UntransposeBits(in, out, std::integral_constant<size_t, D - 1>());
}
template <typename T, size_t N>
void ApplyGrayCode1(std::array<T, N> const &in, std::array<T, N> &out, std::integral_constant<size_t, 0>) {
out[0] ^= in[N - 1] >> 1;
}
template <typename T, size_t N, size_t I>
void ApplyGrayCode1(std::array<T, N> const &in, std::array<T, N> &out, std::integral_constant<size_t, I>) {
out[I] ^= out[I - 1];
ApplyGrayCode1(in, out, std::integral_constant<size_t, I - 1>());
}
// Remove a gray code from a transposed vector
template <typename T, size_t N>
void RemoveGrayCode1(std::array<T, N> &, std::integral_constant<size_t, 0>) {}
// xor array values with previous values.
template <typename T, size_t N, size_t D>
void RemoveGrayCode1(std::array<T, N> &in, std::integral_constant<size_t, D>) {
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<size_t, D - 1>());
}
template <typename T>
T RemoveGrayCode2(T, std::integral_constant<size_t, 1>) {
return T(0);
}
template <typename T, size_t B>
T RemoveGrayCode2(T v, std::integral_constant<size_t, B>) {
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<size_t, B - 1>());
} else {
return RemoveGrayCode2(v, std::integral_constant<size_t, B - 1>());
}
}
template <typename T, size_t N, size_t B>
void GrayToHilbert2(std::array<T, N> &, std::integral_constant<size_t, B>, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t B, size_t I>
void GrayToHilbert2(std::array<T, N> &out, std::integral_constant<size_t, B>, std::integral_constant<size_t, I>) {
const size_t n(I - 1);
const T cur_bit(T(1) << (std::numeric_limits<T>::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<size_t, B>(), std::integral_constant<size_t, I - 1>());
}
template <typename T, size_t N>
void GrayToHilbert(std::array<T, N> &, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t B>
void GrayToHilbert(std::array<T, N> &out, std::integral_constant<size_t, B>) {
GrayToHilbert2(out, std::integral_constant<size_t, B>(), std::integral_constant<size_t, N>());
GrayToHilbert(out, std::integral_constant<size_t, B - 1>());
}
template <typename T, size_t N, size_t B>
void HilbertToGray2(std::array<T, N> &, std::integral_constant<size_t, B>, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t B, size_t I>
void HilbertToGray2(std::array<T, N> &out, std::integral_constant<size_t, B>, std::integral_constant<size_t, I>) {
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<size_t, B>(), std::integral_constant<size_t, I - 1>());
}
template <typename T, size_t N>
void HilbertToGray(std::array<T, N> &, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t B>
void HilbertToGray(std::array<T, N> &out, std::integral_constant<size_t, B>) {
HilbertToGray2(out, std::integral_constant<size_t, B>(), std::integral_constant<size_t, N>());
HilbertToGray(out, std::integral_constant<size_t, B - 1>());
}
template <typename T, size_t N>
void ApplyMaskToArray(std::array<T, N> &, T, std::integral_constant<size_t, 0>) {}
template <typename T, size_t N, size_t I>
void ApplyMaskToArray(std::array<T, N> &a, T mask, std::integral_constant<size_t, I>) {
a[I - 1] ^= mask;
ApplyMaskToArray(a, mask, std::integral_constant<size_t, I - 1>());
}
} // 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 <typename T, size_t N>
std::array<T, N> TransposeBits(std::array<T, N> const &in) {
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
tmp::TransposeBits(in, out, std::integral_constant<size_t, N>());
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 <typename T, size_t N>
std::array<T, N> UntransposeBits(std::array<T, N> const &in) {
std::array<T, N> out;
std::fill(out.begin(), out.end(), 0);
tmp::UntransposeBits(in, out, std::integral_constant<size_t, N>());
return out;
}
// Apply a gray code to a transformed vector.
template <typename T, size_t N>
std::array<T, N> ApplyGrayCode(std::array<T, N> const &in) {
std::array<T, N> out(in);
tmp::ApplyGrayCode1(in, out, std::integral_constant<size_t, N - 1>());
return out;
}
template <typename T, size_t N>
std::array<T, N> RemoveGrayCode(std::array<T, N> const &in) {
const size_t bits = std::numeric_limits<T>::digits;
std::array<T, N> out(in);
// Remove gray code from transposed vector.
{
// xor values with prev values.
tmp::RemoveGrayCode1(out, std::integral_constant<size_t, N - 1>());
// create a mask.
T t = tmp::RemoveGrayCode2(out[N - 1], std::integral_constant<size_t, bits>());
// Apply mask to output.
tmp::ApplyMaskToArray(out, t, std::integral_constant<size_t, N>());
}
return out;
}
// Generate code to convert from a transposed gray code to a hilbert
// code.
template <typename T, size_t N>
std::array<T, N> GrayToHilbert(std::array<T, N> const &in) {
std::array<T, N> out(in);
tmp::GrayToHilbert(out, std::integral_constant<size_t, std::numeric_limits<T>::digits - 1>());
return out;
}
// Generate code to convert from a hilbert code to a transposed gray
// code.
template <typename T, size_t N>
std::array<T, N> HilbertToGray(std::array<T, N> const &in) {
std::array<T, N> out(in);
tmp::HilbertToGray(out, std::integral_constant<size_t, std::numeric_limits<T>::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 <typename T, size_t N>
std::array<T, N> IndexToPosition(std::array<T, N> 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 <typename T, size_t N>
std::array<T, N> PositionToIndex(std::array<T, N> 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 <glog/logging.h>
#include <execution>
#include <list>
#include <thread>
#include <unordered_map>
#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 <IVoxNodeType node_type, typename PointT, int dim>
struct IVoxNodeTypeTraits {};
template <typename PointT, int dim>
struct IVoxNodeTypeTraits<IVoxNodeType::DEFAULT, PointT, dim> {
using NodeType = IVoxNode<PointT, dim>;
};
template <typename PointT, int dim>
struct IVoxNodeTypeTraits<IVoxNodeType::PHC, PointT, dim> {
using NodeType = IVoxNodePhc<PointT, dim>;
};
template <int dim = 3, IVoxNodeType node_type = IVoxNodeType::DEFAULT, typename PointType = pcl::PointXYZ>
class IVox {
public:
using KeyType = Eigen::Matrix<int, dim, 1>;
using PtType = Eigen::Matrix<float, dim, 1>;
using NodeType = typename IVoxNodeTypeTraits<node_type, PointType, dim>::NodeType;
using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
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<float> 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<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, math::hash_vec<dim>>
grids_map_; // voxel hash map
std::list<std::pair<KeyType, NodeType>> grids_cache_; // voxel cache
std::vector<KeyType> nearby_grids_; // nearbys
};
template <int dim, IVoxNodeType node_type, typename PointType>
bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointType& pt, PointType& closest_pt) {
std::vector<DistPoint> candidates;
auto key = Pos2Grid(math::ToEigen<float, dim>(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 <int dim, IVoxNodeType node_type, typename PointType>
bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num,
double max_range) {
std::vector<DistPoint> candidates;
candidates.reserve(max_num * nearby_grids_.size());
auto key = Pos2Grid(math::ToEigen<float, dim>(pt));
// #define INNER_TIMER
#ifdef INNER_TIMER
static std::unordered_map<std::string, std::vector<int64_t>> stats;
if (stats.empty()) {
stats["knn"] = std::vector<int64_t>();
stats["nth"] = std::vector<int64_t>();
}
#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<std::chrono::nanoseconds>(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<int64_t>& 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 <int dim, IVoxNodeType node_type, typename PointType>
size_t IVox<dim, node_type, PointType>::NumValidGrids() const {
return grids_map_.size();
}
template <int dim, IVoxNodeType node_type, typename PointType>
size_t IVox<dim, node_type, PointType>::NumPoints() const {
int ret = 0;
for (auto& g : grids_map_) {
ret += g.second->second.Size();
}
return ret;
}
template <int dim, IVoxNodeType node_type, typename PointType>
void IVox<dim, node_type, PointType>::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 <int dim, IVoxNodeType node_type, typename PointType>
bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud) {
std::vector<size_t> 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 <int dim, IVoxNodeType node_type, typename PointType>
void IVox<dim, node_type, PointType>::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<float, dim>(pt));
auto iter = grids_map_.find(key);
if (iter == grids_map_.end()) {
PointType center;
center.getVector3fMap() = key.template cast<float>() * 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 <int dim, IVoxNodeType node_type, typename PointType>
Eigen::Matrix<int, dim, 1> IVox<dim, node_type, PointType>::Pos2Grid(const IVox::PtType& pt) const {
return (pt * options_.inv_resolution_).array().round().template cast<int>();
}
template <int dim, IVoxNodeType node_type, typename PointType>
std::vector<float> IVox<dim, node_type, PointType>::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>{float(valid_num), ave, float(max), float(min), stddev};
}
} // namespace lightning
================================================
FILE: src/core/ivox3d/ivox3d_node.hpp
================================================
#include <pcl/common/centroid.h>
#include <algorithm>
#include <cmath>
#include <list>
#include <vector>
#include "core/ivox3d/hilbert.hpp"
#include "core/lightning_math.hpp"
namespace lightning {
template <typename PointT, int dim = 3>
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<DistPoint>& dis_points, const PointT& point, const int& K,
const double& max_range);
private:
std::vector<PointT> points_;
};
template <typename PointT, int dim = 3>
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<DistPoint>& 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<PhcCube> 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<float, dim, 1> min_cube_;
};
template <typename PointT, int dim>
struct IVoxNode<PointT, dim>::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 <typename PointT, int dim>
void IVoxNode<PointT, dim>::InsertPoint(const PointT& pt) {
points_.template emplace_back(pt);
if (points_.size() >= 10) {
points_.erase(points_.begin());
}
}
template <typename PointT, int dim>
bool IVoxNode<PointT, dim>::Empty() const {
return points_.empty();
}
template <typename PointT, int dim>
std::size_t IVoxNode<PointT, dim>::Size() const {
return points_.size();
}
template <typename PointT, int dim>
PointT IVoxNode<PointT, dim>::GetPoint(const std::size_t idx) const {
return points_[idx];
}
template <typename PointT, int dim>
int IVoxNode<PointT, dim>::KNNPointByCondition(std::vector<DistPoint>& 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<std::string, std::vector<int64_t>> stats;
if (stats.empty()) {
stats["dis"] = std::vector<int64_t>();
stats["put"] = std::vector<int64_t>();
stats["nth"] = std::vector<int64_t>();
}
#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<std::chrono::nanoseconds>(t1 - t0).count();
stats["dis"].emplace_back(dis);
auto put = std::chrono::duration_cast<std::chrono::nanoseconds>(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<std::chrono::nanoseconds>(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<int64_t>& 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 <typename PointT, int dim>
struct IVoxNodePhc<PointT, dim>::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 <typename PointT, int dim>
struct IVoxNodePhc<PointT, dim>::PhcCube {
uint32_t idx = 0;
pcl::CentroidPoint<PointT> 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 <typename PointT, int dim>
IVoxNodePhc<PointT, dim>::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 <typename PointT, int dim>
void IVoxNodePhc<PointT, dim>::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 <typename PointT, int dim>
void IVoxNodePhc<PointT, dim>::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 <typename PointT, int dim>
bool IVoxNodePhc<PointT, dim>::Empty() const {
return phc_cubes_.empty();
}
template <typename PointT, int dim>
std::size_t IVoxNodePhc<PointT, dim>::Size() const {
return phc_cubes_.size();
}
template <typename PointT, int dim>
PointT IVoxNodePhc<PointT, dim>::GetPoint(const std::size_t idx) const {
return phc_cubes_[idx].GetPoint();
}
template <typename PointT, int dim>
bool IVoxNodePhc<PointT, dim>::NNPoint(const Poi
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
SYMBOL INDEX (962 symbols across 193 files)
FILE: scripts/merge_bags.py
function merge_bags (line 12) | def merge_bags(input_bags, output_bag):
function convert_ros1_bag (line 48) | def convert_ros1_bag(input_file, output_file=None):
function decompress_db3_zstd (line 75) | def decompress_db3_zstd(input_file, output_file=None):
function find_ros1_bags (line 106) | def find_ros1_bags(root_folder):
function find_ros2_bags (line 127) | def find_ros2_bags(root_folder):
FILE: src/app/run_frontend_offline.cc
function main (line 21) | int main(int argc, char** argv) {
FILE: src/app/run_loc_offline.cc
function main (line 21) | int main(int argc, char** argv) {
FILE: src/app/run_loc_online.cc
function main (line 15) | int main(int argc, char** argv) {
FILE: src/app/run_loop_offline.cc
function main (line 18) | int main(int argc, char** argv) {
FILE: src/app/run_slam_offline.cc
function main (line 20) | int main(int argc, char** argv) {
FILE: src/app/run_slam_online.cc
function main (line 16) | int main(int argc, char** argv) {
FILE: src/app/test_ui.cc
function main (line 12) | int main(int argc, char** argv) {
FILE: src/common/constant.h
function namespace (line 10) | namespace lightning::constant {
FILE: src/common/eigen_types.h
function namespace (line 20) | namespace lightning {
FILE: src/common/functional_points.h
function namespace (line 8) | namespace lightning {
FILE: src/common/imu.h
function namespace (line 10) | namespace lightning {
FILE: src/common/keyframe.h
function namespace (line 13) | namespace lightning {
FILE: src/common/loop_candidate.h
function namespace (line 10) | namespace lightning {
FILE: src/common/measure_group.h
function namespace (line 14) | namespace lightning {
FILE: src/common/nav_state.cc
type lightning (line 7) | namespace lightning {
FILE: src/common/nav_state.h
function namespace (line 12) | namespace lightning {
function oplus (line 89) | void oplus(const FullVectState& vec, double dt) {
function VectState (line 102) | VectState boxminus(const NavState& other) {
function NavState (line 116) | NavState boxplus(const VectState& dx) {
type MetaInfo (line 129) | struct MetaInfo {
function SetPose (line 147) | inline void SetPose(const SE3& pose) {
function Vec3d (line 152) | inline Vec3d Getba() const { return Vec3d::Zero(); }
function SetVel (line 155) | void SetVel(const Vec3d& v) { vel_ = v; }
FILE: src/common/odom.h
function namespace (line 10) | namespace lightning {
FILE: src/common/options.cc
type lightning (line 7) | namespace lightning {
type debug (line 9) | namespace debug {
type lo (line 19) | namespace lo {
type fasterlio (line 32) | namespace fasterlio {
type map (line 37) | namespace map {
type ui (line 43) | namespace ui {
type pgo (line 49) | namespace pgo {
type lidar_loc (line 67) | namespace lidar_loc {
FILE: src/common/options.h
function namespace (line 17) | namespace lightning {
FILE: src/common/params.cc
type lightning (line 9) | namespace lightning {
FILE: src/common/params.h
function SensorType (line 18) | enum class SensorType {
FILE: src/common/point_def.h
type PointXYZIT (line 17) | struct PointXYZIT {
type PointRobotSense (line 27) | struct PointRobotSense {
function namespace (line 39) | namespace velodyne_ros {
FILE: src/common/pose_rpy.h
function namespace (line 8) | namespace lightning{
FILE: src/common/s2.hpp
type lightning (line 8) | namespace lightning {
type S2 (line 17) | struct S2 {
method S2 (line 26) | S2() { vec_ = length_ * Vec3d(1, 0, 0); }
method S2 (line 27) | S2(const Vec3d &vec) : vec_(vec) {
method S2_hat (line 34) | Eigen::Matrix<double, 3, 3> S2_hat() const {
method S2_Mx (line 43) | Eigen::Matrix<double, 3, 2> S2_Mx(const Eigen::Matrix<double, 2, 1> ...
method S2_Bx (line 97) | Eigen::Matrix<double, 3, 2> S2_Bx() const {
method S2_Nx_yy (line 116) | Eigen::Matrix<double, 2, 3> S2_Nx_yy() const {
method oplus (line 123) | void oplus(const Vec3d &delta, double scale = 1.0) { vec_ = math::ex...
method Vec2d (line 130) | Vec2d boxminus(const S2 &other) const {
method boxplus (line 156) | void boxplus(const Vec2d &delta, double scale = 1) {
FILE: src/common/std_types.h
function namespace (line 11) | namespace lightning {
FILE: src/common/timed_pose.h
function namespace (line 10) | namespace lightning {
FILE: src/core/g2p5/g2p5.cc
type lightning::g2p5 (line 18) | namespace lightning::g2p5 {
function G2P5MapPtr (line 263) | G2P5MapPtr G2P5::GetNewestMap() {
FILE: src/core/g2p5/g2p5.h
function namespace (line 15) | namespace lightning::g2p5 {
FILE: src/core/g2p5/g2p5_grid_data.h
function namespace (line 8) | namespace lightning::g2p5 {
FILE: src/core/g2p5/g2p5_map.cc
type lightning::g2p5 (line 11) | namespace lightning::g2p5 {
FILE: src/core/g2p5/g2p5_map.h
function namespace (line 16) | namespace lightning::g2p5 {
type std (line 114) | typedef std::shared_ptr<G2P5Map> G2P5MapPtr;
FILE: src/core/g2p5/g2p5_subgrid.cc
type lightning::g2p5 (line 10) | namespace lightning::g2p5 {
function SubGrid (line 47) | SubGrid &SubGrid::operator=(const SubGrid &other) {
FILE: src/core/g2p5/g2p5_subgrid.h
function namespace (line 11) | namespace lightning::g2p5 {
FILE: src/core/ivox3d/hilbert.hpp
type hilbert (line 80) | namespace hilbert {
type v1 (line 83) | namespace v1 {
type internal (line 84) | namespace internal {
function UntransposeBits (line 94) | std::array<T, N> UntransposeBits(std::array<T, N> const &in) {
function TransposeBits (line 127) | std::array<T, N> TransposeBits(std::array<T, N> const &in) {
function IndexToPosition (line 163) | std::array<T, N> IndexToPosition(std::array<T, N> const &in) {
function PositionToIndex (line 214) | std::array<T, N> PositionToIndex(std::array<T, N> const &in) {
type v2 (line 271) | namespace v2 {
type internal (line 272) | namespace internal {
type tmp (line 274) | namespace tmp {
function T (line 276) | T TransposeBits2(std::array<T, N> const &, std::integral_constan...
function T (line 281) | T TransposeBits2(std::array<T, N> const &in, std::integral_const...
function TransposeBits (line 297) | void TransposeBits(std::array<T, N> const &, std::array<T, N> &,...
function TransposeBits (line 300) | void TransposeBits(std::array<T, N> const &in, std::array<T, N> ...
function T (line 308) | T UntransposeBits2(std::array<T, N> const &, std::integral_const...
function T (line 313) | T UntransposeBits2(std::array<T, N> const &in, std::integral_con...
function UntransposeBits (line 330) | void UntransposeBits(std::array<T, N> const &, std::array<T, N> ...
function UntransposeBits (line 333) | void UntransposeBits(std::array<T, N> const &in, std::array<T, N...
function ApplyGrayCode1 (line 341) | void ApplyGrayCode1(std::array<T, N> const &in, std::array<T, N>...
function ApplyGrayCode1 (line 346) | void ApplyGrayCode1(std::array<T, N> const &in, std::array<T, N>...
function RemoveGrayCode1 (line 354) | void RemoveGrayCode1(std::array<T, N> &, std::integral_constant<...
function RemoveGrayCode1 (line 358) | void RemoveGrayCode1(std::array<T, N> &in, std::integral_constan...
function T (line 368) | T RemoveGrayCode2(T, std::integral_constant<size_t, 1>) {
function T (line 373) | T RemoveGrayCode2(T v, std::integral_constant<size_t, B>) {
function GrayToHilbert2 (line 385) | void GrayToHilbert2(std::array<T, N> &, std::integral_constant<s...
function GrayToHilbert2 (line 388) | void GrayToHilbert2(std::array<T, N> &out, std::integral_constan...
function GrayToHilbert (line 407) | void GrayToHilbert(std::array<T, N> &, std::integral_constant<si...
function GrayToHilbert (line 410) | void GrayToHilbert(std::array<T, N> &out, std::integral_constant...
function HilbertToGray2 (line 417) | void HilbertToGray2(std::array<T, N> &, std::integral_constant<s...
function HilbertToGray2 (line 420) | void HilbertToGray2(std::array<T, N> &out, std::integral_constan...
function HilbertToGray (line 439) | void HilbertToGray(std::array<T, N> &, std::integral_constant<si...
function HilbertToGray (line 442) | void HilbertToGray(std::array<T, N> &out, std::integral_constant...
function ApplyMaskToArray (line 449) | void ApplyMaskToArray(std::array<T, N> &, T, std::integral_const...
function ApplyMaskToArray (line 452) | void ApplyMaskToArray(std::array<T, N> &a, T mask, std::integral...
function TransposeBits (line 468) | std::array<T, N> TransposeBits(std::array<T, N> const &in) {
function UntransposeBits (line 486) | std::array<T, N> UntransposeBits(std::array<T, N> const &in) {
function ApplyGrayCode (line 498) | std::array<T, N> ApplyGrayCode(std::array<T, N> const &in) {
function RemoveGrayCode (line 507) | std::array<T, N> RemoveGrayCode(std::array<T, N> const &in) {
function GrayToHilbert (line 529) | std::array<T, N> GrayToHilbert(std::array<T, N> const &in) {
function HilbertToGray (line 540) | std::array<T, N> HilbertToGray(std::array<T, N> const &in) {
function IndexToPosition (line 560) | std::array<T, N> IndexToPosition(std::array<T, N> const &in) {
function PositionToIndex (line 572) | std::array<T, N> PositionToIndex(std::array<T, N> const &in) {
FILE: src/core/ivox3d/ivox3d.h
function namespace (line 16) | namespace lightning {
FILE: src/core/ivox3d/ivox3d_node.hpp
type lightning (line 10) | namespace lightning {
class IVoxNode (line 13) | class IVoxNode {
method IVoxNode (line 19) | IVoxNode() = default;
method IVoxNode (line 20) | IVoxNode(const PointT& center, const float& side_length) {}
class IVoxNodePhc (line 38) | class IVoxNodePhc {
type PhcCube (line 43) | struct PhcCube
method IVoxNodePhc (line 45) | IVoxNodePhc() = default;
type IVoxNode<PointT, dim>::DistPoint (line 78) | struct IVoxNode<PointT, dim>::DistPoint {
method DistPoint (line 83) | DistPoint() = default;
method DistPoint (line 84) | DistPoint(const double d, IVoxNode* n, const int i) : dist(d), node(...
method PointT (line 86) | PointT Get() { return node->GetPoint(idx); }
function PointT (line 112) | PointT IVoxNode<PointT, dim>::GetPoint(const std::size_t idx) const {
type IVoxNodePhc<PointT, dim>::DistPoint (line 184) | struct IVoxNodePhc<PointT, dim>::DistPoint {
method DistPoint (line 189) | DistPoint() {}
method DistPoint (line 190) | DistPoint(const double d, IVoxNodePhc* n, const int i) : dist(d), no...
method PointT (line 192) | PointT Get() { return node->GetPoint(idx); }
type IVoxNodePhc<PointT, dim>::PhcCube (line 200) | struct IVoxNodePhc<PointT, dim>::PhcCube {
method PhcCube (line 204) | PhcCube(uint32_t index, const PointT& pt) { mean.add(pt); }
method AddPoint (line 206) | void AddPoint(const PointT& pt) { mean.add(pt); }
method PointT (line 208) | PointT GetPoint() const {
function PointT (line 270) | PointT IVoxNodePhc<PointT, dim>::GetPoint(const std::size_t idx) const {
FILE: src/core/lightning_math.hpp
type lightning::math (line 26) | namespace lightning::math {
function SKEW_SYM_MATRIX (line 29) | inline Eigen::Matrix<T, 3, 3> SKEW_SYM_MATRIX(const Eigen::Matrix<T, 3...
function SKEW_SYM_MATRIX (line 36) | inline Eigen::Matrix<T, 3, 3> SKEW_SYM_MATRIX(const T& v1, const T& v2...
function Exp (line 43) | Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1>&& ang) {
function Exp (line 58) | Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1>& ang_vel, cons...
function Exp (line 78) | Eigen::Matrix<T, 3, 3> Exp(const T& v1, const T& v2, const T& v3) {
function Log (line 94) | Eigen::Matrix<T, 3, 1> Log(const Eigen::Matrix<T, 3, 3>& R) {
function RotMtoEuler (line 101) | Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3>& rot) {
function RpyToRotM2 (line 128) | Eigen::Matrix<T, 3, 3> RpyToRotM2(const T r, const T p, const T y) {
function VecFromArray (line 135) | inline Eigen::Matrix<S, 3, 1> VecFromArray(const std::vector<double>& ...
function VecFromArray (line 140) | inline Eigen::Matrix<S, 3, 1> VecFromArray(const boost::array<S, 3>& v) {
function MatFromArray (line 145) | inline Eigen::Matrix<S, 3, 3> MatFromArray(const std::vector<double>& ...
function MatFromArray (line 152) | inline Eigen::Matrix<S, 3, 3> MatFromArray(const boost::array<S, 9>& v) {
function T (line 159) | T rad2deg(const T& radians) {
function T (line 164) | T deg2rad(const T& degrees) {
function limit_in_range (line 177) | void limit_in_range(T&& num, T2&& min_limit, T2&& max_limit) {
function ComputeMeanAndCovDiag (line 197) | inline void ComputeMeanAndCovDiag(const C& data, D& mean, D& cov_diag,...
function HistoryMeanAndVar (line 226) | inline void HistoryMeanAndVar(size_t hist_n, float hist_mean, float hi...
function cos_sinc_sqrt (line 240) | inline std::pair<scalar, scalar> cos_sinc_sqrt(const scalar& x2) {
function SO3 (line 273) | inline SO3 exp(const Vec3d& vec, const double& scale = 1) {
function PseudoInverse (line 281) | inline Eigen::Matrix<double, 2, 3> PseudoInverse(const Eigen::Matrix<d...
function A_matrix (line 300) | inline Eigen::Matrix<double, 3, 3> A_matrix(const Vec3d& v) {
function A_inv (line 314) | inline Eigen::Matrix<double, 3, 3> A_inv(const Vec3d& v) {
type hash_vec (line 332) | struct hash_vec {
function esti_plane (line 358) | inline bool esti_plane(Eigen::Matrix<T, 4, 1>& pca_result, const Point...
function CloudPtr (line 417) | inline CloudPtr VoxelGrid(CloudPtr cloud, float voxel_size = 0.05) {
function ToSec (line 429) | inline double ToSec(uint64_t t) { return double(t) * 1e-9; }
function ToEigen (line 433) | inline Eigen::Matrix<T, dim, 1> ToEigen(const PointType& pt) {
function calc_dist (line 463) | inline float calc_dist(const PointType& p1, const PointType& p2) {
function calc_dist (line 467) | inline float calc_dist(const Eigen::Vector3f& p1, const Eigen::Vector3...
function distance2 (line 471) | inline double distance2(const PointT& pt1, const PointT& pt2) {
function Mat4FromArray (line 477) | inline Eigen::Matrix<S, 4, 4> Mat4FromArray(const std::vector<double>&...
type less_vec (line 485) | struct less_vec {
function ComputeMeanAndCov (line 507) | void ComputeMeanAndCov(const C& data, Eigen::Matrix<S, dim, 1>& mean, ...
function UpdateMeanAndCov (line 539) | void UpdateMeanAndCov(int hist_m, int curr_n, const Eigen::Matrix<S, D...
function KeepAngleInPI (line 552) | inline void KeepAngleInPI(double& angle) {
function KeepAngleIn2PI (line 561) | inline void KeepAngleIn2PI(double& angle) {
function FromSec (line 570) | inline builtin_interfaces::msg::Time FromSec(double t) {
function PoseRPYD (line 583) | inline PoseRPYD SE3ToRollPitchYaw(const SE3& pose) {
function SE3 (line 598) | inline SE3 XYZRPYToSE3(const PoseRPYD& pose) {
function PoseInterp (line 621) | inline bool PoseInterp(double query_time, C&& data, FT&& take_time_fun...
FILE: src/core/lio/anderson_acceleration.h
function namespace (line 12) | namespace lightning {
FILE: src/core/lio/eskf.cc
function SymmetrizeAndFloorCovariance (line 15) | void SymmetrizeAndFloorCovariance(CovType& P, double min_cov_diag) {
type lightning (line 36) | namespace lightning {
FILE: src/core/lio/eskf.hpp
type lightning (line 12) | namespace lightning {
class ESKF (line 23) | class ESKF {
type ObsType (line 33) | enum class ObsType {
method ESKF (line 43) | explicit ESKF(const NavState& x = NavState(), const CovType& P = Cov...
type CustomObservationModel (line 51) | struct CustomObservationModel {
type Options (line 69) | struct Options {
method Init (line 93) | void Init(Options options) {
method NavState (line 113) | const NavState& GetX() const { return x_; }
method CovType (line 114) | const CovType& GetP() const { return P_; }
method ChangeX (line 117) | void ChangeX(const NavState& state) { x_ = state; }
method ChangeP (line 118) | void ChangeP(const CovType& P) { P_ = P; }
method ChangeStamp (line 119) | void ChangeStamp(const double& stamp) { stamp_ = stamp; }
method SetUseAA (line 121) | void SetUseAA(bool use_aa) { use_aa_ = use_aa; }
method SetTime (line 122) | void SetTime(double timestamp) { x_.timestamp_ = timestamp; }
method GetIterations (line 125) | int GetIterations() const { return iterations_; }
method GetFinalRes (line 127) | double GetFinalRes() const { return final_res_; }
FILE: src/core/lio/imu_filter.h
function namespace (line 13) | namespace lightning {
FILE: src/core/lio/imu_processing.hpp
type lightning (line 22) | namespace lightning {
class ImuProcess (line 25) | class ImuProcess {
method IsIMUInited (line 41) | bool IsIMUInited() const { return imu_need_init_ == false; }
method SetUseIMUFilter (line 42) | void SetUseIMUFilter(bool b) { use_imu_filter_ = b; }
method GetMeanAccNorm (line 44) | double GetMeanAccNorm() const { return mean_acc_.norm(); }
FILE: src/core/lio/laser_mapping.cc
type lightning (line 16) | namespace lightning {
function CloudPtr (line 804) | CloudPtr LaserMapping::GetGlobalMap(bool use_lio_pose, bool use_voxel,...
function CloudPtr (line 862) | CloudPtr LaserMapping::GetRecentCloud() {
function CloudPtr (line 870) | CloudPtr LaserMapping::GetProjCloud() {
FILE: src/core/lio/laser_mapping.h
function namespace (line 22) | namespace ui {
type Options (line 33) | struct Options {
function SetUI (line 84) | void SetUI(std::shared_ptr<ui::PangolinWindow> ui) { ui_ = ui; }
function PointBodyToWorld (line 124) | inline void PointBodyToWorld(const PointType &pi, PointType &po) {
function CloudPtr (line 170) | CloudPtr scan_down_body_{new PointCloudType()}; // downsampled scan in...
FILE: src/core/lio/pointcloud_preprocess.cc
type lightning (line 6) | namespace lightning {
FILE: src/core/lio/pointcloud_preprocess.h
function LidarType (line 15) | enum class LidarType { AVIA = 1, VELO32 = 2, OUST64 = 3, ROBOSENSE = 4 };
FILE: src/core/localization/lidar_loc/lidar_loc.cc
type lightning::loc (line 23) | namespace lightning::loc {
function NavState (line 153) | NavState LidarLoc::GetState() {
FILE: src/core/localization/lidar_loc/lidar_loc.h
function namespace (line 17) | namespace lightning::ui {
function namespace (line 21) | namespace lightning::loc {
FILE: src/core/localization/lidar_loc/pclomp/ndt_omp.cpp
class pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> (line 4) | class pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>
class pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> (line 5) | class pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>
FILE: src/core/localization/lidar_loc/pclomp/ndt_omp.h
function namespace (line 49) | namespace pclomp {
function setStepSize (line 138) | inline void setStepSize(double step_size) { step_size_ = step_size; }
function setOulierRatio (line 148) | inline void setOulierRatio(double outlier_ratio) { outlier_ratio_ = outl...
function setNeighborhoodSearchMethod (line 150) | inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) { s...
function TargetGrid (line 162) | inline TargetGrid &getTargetGrid() { return (target_cells_); }
function convertTransform (line 168) | static void convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen...
function convertTransform (line 179) | static void convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen...
function AddTarget (line 194) | void AddTarget(PointCloudTargetPtr target) {
function initCompute (line 200) | bool initCompute() {
function ComputeTargetGrids (line 242) | void ComputeTargetGrids() {
function init (line 278) | void inline init() {
function setMaskXYZ (line 491) | inline void setMaskXYZ(bool new_value) { mask_xyz_ = new_value; }
function setMaskXY (line 492) | inline void setMaskXY(bool new_value) { mask_xy_ = new_value; }
function setMaskRP (line 493) | inline void setMaskRP(bool new_value) { mask_rp_ = new_value; }
function setMaskYaw (line 494) | inline void setMaskYaw(bool new_value) { mask_yaw_ = new_value; }
function setMaskRPY (line 495) | inline void setMaskRPY(bool new_value) { mask_rpy_ = new_value; }
function setMaskZ (line 496) | inline void setMaskZ(bool new_value) { mask_z_ = new_value; }
function setNoGuarnteedDecrease (line 497) | inline void setNoGuarnteedDecrease(bool new_value) { no_guarnteed_decrea...
FILE: src/core/localization/lidar_loc/pclomp/ndt_omp_impl.hpp
function omp_get_max_threads (line 249) | int omp_get_max_threads() { return 1; }
function omp_get_thread_num (line 250) | int omp_get_thread_num() { return 0; }
FILE: src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.cpp
class pclomp::VoxelGridCovariance<pcl::PointXYZ> (line 4) | class pclomp::VoxelGridCovariance<pcl::PointXYZ>
class pclomp::VoxelGridCovariance<pcl::PointXYZI> (line 5) | class pclomp::VoxelGridCovariance<pcl::PointXYZI>
FILE: src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.h
function namespace (line 49) | namespace pclomp {
type Leaf (line 135) | typedef const Leaf *LeafConstPtr;
function setMinPointPerVoxel (line 156) | inline void setMinPointPerVoxel(int min_points_per_voxel) {
function getMinPointPerVoxel (line 169) | inline int getMinPointPerVoxel() { return min_points_per_voxel_; }
function setCovEigValueInflationRatio (line 174) | inline void setCovEigValueInflationRatio(double min_covar_eigvalue_mult) {
function getCovEigValueInflationRatio (line 181) | inline double getCovEigValueInflationRatio() { return min_covar_eigvalue...
function Eigen (line 183) | inline Eigen::Vector3i Pt2Key(const Eigen::Vector3f &pt) const {
function HashMap (line 202) | inline const HashMap &getLeaves() { return leaves_; }
FILE: src/core/localization/localization.cpp
type lightning::loc (line 13) | namespace lightning::loc {
FILE: src/core/localization/localization.h
function namespace (line 12) | namespace lightning {
FILE: src/core/localization/localization_result.cc
type lightning::loc (line 8) | namespace lightning::loc {
function NavState (line 28) | NavState LocalizationResult::ToNavState() const {
FILE: src/core/localization/localization_result.h
function namespace (line 10) | namespace lightning::loc {
FILE: src/core/localization/pose_graph/pgo.cc
type lightning::loc (line 11) | namespace lightning::loc {
FILE: src/core/localization/pose_graph/pgo.h
function namespace (line 11) | namespace lightning::loc {
FILE: src/core/localization/pose_graph/pgo_impl.cc
type lightning::loc (line 11) | namespace lightning::loc {
function print_info (line 25) | std::string print_info(const std::vector<T>& edges, double th = 0) {
FILE: src/core/localization/pose_graph/pgo_impl.h
function namespace (line 21) | namespace lightning::loc {
FILE: src/core/localization/pose_graph/pose_extrapolator.cc
type lightning::loc (line 6) | namespace lightning::loc {
FILE: src/core/localization/pose_graph/pose_extrapolator.h
function namespace (line 11) | namespace lightning::loc {
FILE: src/core/localization/pose_graph/smoother.h
function namespace (line 12) | namespace lightning::loc {
FILE: src/core/loop_closing/loop_closing.cc
type lightning (line 20) | namespace lightning {
FILE: src/core/loop_closing/loop_closing.h
function namespace (line 15) | namespace lightning {
FILE: src/core/maps/tiled_map.cc
type lightning (line 11) | namespace lightning {
function CloudPtr (line 364) | CloudPtr TiledMap::GetAllMap() {
FILE: src/core/maps/tiled_map.h
function class (line 33) | class TiledMap {
function NearbyType (line 76) | enum class NearbyType {
function AddFP (line 172) | void AddFP(const FunctionalPoint& fp) { func_points_.emplace_back(fp); }
function CleanMapUpdate (line 182) | void CleanMapUpdate() {
FILE: src/core/maps/tiled_map_chunk.cc
type lightning (line 9) | namespace lightning {
FILE: src/core/maps/tiled_map_chunk.h
function namespace (line 11) | namespace lightning {
FILE: src/core/miao/core/common/config.h
type class (line 11) | enum class
function LinearSolverType (line 18) | enum class LinearSolverType {
FILE: src/core/miao/core/common/macros.h
function namespace (line 8) | namespace lightning::miao {
FILE: src/core/miao/core/common/math.h
function namespace (line 11) | namespace lightning::miao {
FILE: src/core/miao/core/common/string_tools.cc
type lightning::miao (line 11) | namespace lightning::miao {
function trim (line 13) | std::string trim(const std::string& s) {
function trimLeft (line 25) | std::string trimLeft(const std::string& s) {
function trimRight (line 39) | std::string trimRight(const std::string& s) {
function strToLower (line 53) | std::string strToLower(const std::string& s) {
function strToUpper (line 60) | std::string strToUpper(const std::string& s) {
function strExpandFilename (line 67) | std::string strExpandFilename(const std::string& filename) {
function strSplit (line 85) | std::vector<std::string> strSplit(const std::string& str, const std::s...
function strStartsWith (line 103) | bool strStartsWith(const std::string& s, const std::string& start) {
function strEndsWith (line 110) | bool strEndsWith(const std::string& s, const std::string& end) {
function readLine (line 118) | int readLine(std::istream& is, std::stringstream& currentLine) {
function skipLine (line 137) | void skipLine(std::istream& is) {
FILE: src/core/miao/core/common/string_tools.h
function namespace (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/graph/base_binary_edge.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/graph/base_edge.h
function namespace (line 11) | namespace lightning::miao {
FILE: src/core/miao/core/graph/base_fixed_sized_edge.h
function namespace (line 12) | namespace internal {
FILE: src/core/miao/core/graph/base_fixed_sized_edge.hpp
type lightning::miao (line 16) | namespace lightning::miao {
type MapHessianMemoryK (line 220) | struct MapHessianMemoryK {
FILE: src/core/miao/core/graph/base_unary_edge.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/graph/base_vec_vertex.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/graph/base_vertex.h
function namespace (line 15) | namespace lightning::miao {
FILE: src/core/miao/core/graph/edge.h
function namespace (line 16) | namespace lightning::miao {
FILE: src/core/miao/core/graph/graph.cc
type lightning::miao (line 14) | namespace lightning::miao {
FILE: src/core/miao/core/graph/graph.h
function namespace (line 19) | namespace lightning::miao {
FILE: src/core/miao/core/graph/optimizer.cc
type lightning::miao (line 15) | namespace lightning::miao {
FILE: src/core/miao/core/graph/optimizer.h
function namespace (line 14) | namespace lightning::miao {
FILE: src/core/miao/core/graph/vertex.h
function namespace (line 14) | namespace lightning::miao {
FILE: src/core/miao/core/math/marginal_covariance_cholesky.cc
type lightning::miao (line 7) | namespace lightning::miao {
type MatrixElem (line 9) | struct MatrixElem {
method MatrixElem (line 11) | MatrixElem(int r_, int c_) : r(r_), c(c_) {}
FILE: src/core/miao/core/math/marginal_covariance_cholesky.h
function namespace (line 14) | namespace lightning::miao {
FILE: src/core/miao/core/math/matrix_operations.h
function namespace (line 10) | namespace lightning::miao::internal {
FILE: src/core/miao/core/math/misc.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/algo_select.h
function namespace (line 23) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/opti_algo_dogleg.cc
type lightning::miao (line 16) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/opti_algo_dogleg.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/opti_algo_gauss_newton.cc
type lightning::miao (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/opti_algo_gauss_newton.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/opti_algo_lm.cc
type lightning::miao (line 19) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/opti_algo_lm.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/optimization_algorithm.cc
type lightning::miao (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/opti_algo/optimization_algorithm.h
function namespace (line 16) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/cauchy.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/dcs.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/fair.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/huber.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/mc_clure.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/pseudo_huber.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/robust_kernel.h
function namespace (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/saturated.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/tukey.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/robust_kernel/welsch.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/solver/block_solver.h
function namespace (line 20) | namespace lightning::miao {
FILE: src/core/miao/core/solver/block_solver.hpp
type lightning::miao (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/solver/linear_solver.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/solver/linear_solver_ccs.h
function namespace (line 13) | namespace lightning::miao {
FILE: src/core/miao/core/solver/linear_solver_dense.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/solver/linear_solver_eigen.h
function namespace (line 15) | namespace lightning::miao {
FILE: src/core/miao/core/solver/linear_solver_pcg.h
function namespace (line 11) | namespace lightning::miao {
FILE: src/core/miao/core/solver/linear_solver_pcg.hpp
type lightning::miao (line 3) | namespace lightning::miao {
type internal (line 4) | namespace internal {
function pcg_axy (line 10) | inline void pcg_axy(const MatrixType& A, const VectorX& x, int xoff,...
function pcg_axy (line 15) | inline void pcg_axy(const MatrixX& A, const VectorX& x, int xoff, Ve...
function pcg_axpy (line 20) | inline void pcg_axpy(const MatrixType& A, const VectorX& x, int xoff...
function pcg_axpy (line 25) | inline void pcg_axpy(const MatrixX& A, const VectorX& x, int xoff, V...
function pcg_atxpy (line 30) | inline void pcg_atxpy(const MatrixType& A, const VectorX& x, int xof...
function pcg_atxpy (line 35) | inline void pcg_atxpy(const MatrixX& A, const VectorX& x, int xoff, ...
FILE: src/core/miao/core/solver/solver.cc
type lightning::miao (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/solver/solver.h
function namespace (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/sparse/sparse_block_matrix.h
function namespace (line 15) | namespace lightning::miao {
FILE: src/core/miao/core/sparse/sparse_block_matrix.hpp
type lightning::miao (line 13) | namespace lightning::miao {
type TripletEntry (line 353) | struct TripletEntry {
method TripletEntry (line 356) | TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}
type TripletColSort (line 358) | struct TripletColSort {
FILE: src/core/miao/core/sparse/sparse_block_matrix_ccs.h
function namespace (line 15) | namespace lightning::miao {
FILE: src/core/miao/core/sparse/sparse_block_matrix_diagonal.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/sparse/sparse_block_matrix_hashmap.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/types/edge_se2.h
function namespace (line 11) | namespace lightning::miao {
FILE: src/core/miao/core/types/edge_se2_prior.h
function namespace (line 11) | namespace lightning::miao {
FILE: src/core/miao/core/types/edge_se3.h
function namespace (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/types/edge_se3_height_prior.h
function namespace (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/types/edge_se3_prior.h
function namespace (line 12) | namespace lightning::miao {
FILE: src/core/miao/core/types/vertex_pointxyz.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/types/vertex_se2.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/core/types/vertex_se3.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/miao/examples/bal/bal_example.cc
type BALCam (line 29) | struct BALCam {
class VertexCameraBAL (line 34) | class VertexCameraBAL : public miao::BaseVertex<9, BALCam> {
method VertexCameraBAL (line 37) | VertexCameraBAL() = default;
method OplusImpl (line 39) | void OplusImpl(const double* update) override {
class VertexPointBAL (line 52) | class VertexPointBAL : public miao::BaseVecVertex<3> {
method VertexPointBAL (line 55) | VertexPointBAL() = default;
class EdgeObservationBAL (line 80) | class EdgeObservationBAL : public miao::BaseBinaryEdge<2, Vec2d, VertexC...
method EdgeObservationBAL (line 83) | EdgeObservationBAL() {}
method ComputeError (line 85) | void ComputeError() override {
method LinearizeOplus (line 107) | void LinearizeOplus() override {
function main (line 192) | int main(int argc, char** argv) {
FILE: src/core/miao/examples/data_fitting/circle_fit.cc
class VertexCircle (line 18) | class VertexCircle : public miao::BaseVertex<3, Eigen::Vector3d> {
method VertexCircle (line 22) | VertexCircle() {}
method OplusImpl (line 24) | void OplusImpl(const double *update) override {
class EdgePointOnCircle (line 37) | class EdgePointOnCircle : public miao::BaseUnaryEdge<1, Eigen::Vector2d,...
method EIGEN_MAKE_ALIGNED_OPERATOR_NEW (line 39) | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
method ComputeError (line 43) | void ComputeError() override {
method LinearizeOplus (line 54) | void LinearizeOplus() override {
function errorOfSolution (line 66) | double errorOfSolution(int numPoints, const std::vector<Eigen::Vector2d>...
function main (line 81) | int main(int argc, char **argv) {
FILE: src/core/miao/examples/data_fitting/curve_fit.cc
class VertexParams (line 18) | class VertexParams : public miao::BaseVecVertex<3> {}
class EdgePointOnCurve (line 27) | class EdgePointOnCurve : public miao::BaseUnaryEdge<1, Eigen::Vector2d, ...
method EIGEN_MAKE_ALIGNED_OPERATOR_NEW (line 29) | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
method ComputeError (line 32) | void ComputeError() override {
method LinearizeOplus (line 40) | void LinearizeOplus() override {
function main (line 55) | int main(int argc, char** argv) {
FILE: src/core/miao/examples/icp/gicp_demo.cc
class EdgeGICP (line 21) | class EdgeGICP {
method EdgeGICP (line 35) | EdgeGICP() {
method makeRot0 (line 44) | void makeRot0() {
method makeRot1 (line 55) | void makeRot1() {
method Matrix3 (line 66) | Matrix3 prec0(double e) {
method Matrix3 (line 74) | Matrix3 prec1(double e) {
method Matrix3 (line 82) | Matrix3 cov0(double e) {
method Matrix3 (line 90) | Matrix3 cov1(double e) {
class Edge_V_V_GICP (line 102) | class Edge_V_V_GICP : public miao::BaseBinaryEdge<3, EdgeGICP, miao::Ver...
method Edge_V_V_GICP (line 105) | Edge_V_V_GICP() : pl_pl(false) {}
method ComputeError (line 112) | void ComputeError() override {
function main (line 139) | int main(int argc, char** argv) {
FILE: src/core/miao/examples/incremental/incremental.cc
function main (line 30) | int main(int argc, char** argv) {
FILE: src/core/miao/examples/pose_graph/pose_graph.cc
function main (line 24) | int main(int argc, char** argv) {
FILE: src/core/miao/examples/sba/sba_demo.cc
class VertexSCam (line 30) | class VertexSCam : public miao::VertexSE3 {
method EIGEN_MAKE_ALIGNED_OPERATOR_NEW (line 32) | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
method OplusImpl (line 37) | void OplusImpl(const double* update) override {
method transformW2F (line 55) | static void transformW2F(Eigen::Matrix<double, 3, 4>& m, const Vec3d& ...
method transformF2W (line 64) | static void transformF2W(Eigen::Matrix<double, 3, 4>& m, const Vec3d& ...
method setKcam (line 70) | static void setKcam(double fx, double fy, double cx, double cy, double...
method setTransform (line 81) | void setTransform() { w2n = estimate_.inverse().matrix().block<3, 4>(0...
method setProjection (line 85) | void setProjection() { w2i = Kcam * w2n; }
method setDr (line 88) | void setDr() {
method setAll (line 98) | void setAll() {
method mapPoint (line 105) | void mapPoint(Vec3d& res, const Vec3d& pt3) {
class Edge_XYZ_VSC (line 128) | class Edge_XYZ_VSC : public miao::BaseBinaryEdge<3, Vector3, miao::Verte...
method EIGEN_MAKE_ALIGNED_OPERATOR_NEW (line 130) | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
method ComputeError (line 134) | void ComputeError() override {
class Sample (line 149) | class Sample {
method uniform (line 151) | static int uniform(int from, int to) { return static_cast<int>(miao::S...
function main (line 154) | int main(int argc, char** argv) {
FILE: src/core/miao/examples/sphere/sphere.cc
function main (line 28) | int main(int argc, char** argv) {
FILE: src/core/miao/utils/sampler.cc
type lightning::miao (line 7) | namespace lightning::miao {
function sampleUniform (line 12) | double sampleUniform(double min, double max, std::mt19937* generator) {
function sampleGaussian (line 20) | double sampleGaussian(std::mt19937* generator) {
FILE: src/core/miao/utils/sampler.h
function namespace (line 14) | namespace lightning::miao {
FILE: src/core/miao/utils/tuple_tools.h
function namespace (line 10) | namespace lightning::miao {
FILE: src/core/system/async_message_process.h
function namespace (line 17) | namespace lightning::sys {
FILE: src/core/system/loc_system.cc
type lightning (line 10) | namespace lightning {
FILE: src/core/system/loc_system.h
function namespace (line 19) | namespace lightning {
FILE: src/core/system/slam.cc
type lightning (line 17) | namespace lightning {
FILE: src/core/system/slam.h
function namespace (line 20) | namespace lightning {
FILE: src/io/dataset_type.h
function DatasetType (line 12) | enum class DatasetType {
FILE: src/io/file_io.cc
type lightning (line 8) | namespace lightning {
function PathExists (line 9) | bool PathExists(const std::string& file_path) {
function RemoveIfExist (line 14) | bool RemoveIfExist(const std::string& path) {
function IsDirectory (line 23) | bool IsDirectory(const std::string& path) { return std::filesystem::is...
FILE: src/io/file_io.h
function namespace (line 11) | namespace lightning {
FILE: src/io/yaml_io.cc
type lightning (line 10) | namespace lightning {
FILE: src/io/yaml_io.h
function namespace (line 12) | namespace lightning {
FILE: src/ui/pangolin_window.cc
type lightning::ui (line 3) | namespace lightning::ui {
FILE: src/ui/pangolin_window.h
function namespace (line 13) | namespace lightning::ui {
FILE: src/ui/pangolin_window_impl.cc
type lightning::ui (line 12) | namespace lightning::ui {
FILE: src/ui/ui_car.cc
type lightning::ui (line 4) | namespace lightning::ui {
FILE: src/ui/ui_car.h
function namespace (line 5) | namespace lightning::ui {
FILE: src/ui/ui_cloud.cc
type lightning::ui (line 7) | namespace lightning::ui {
FILE: src/ui/ui_cloud.h
function namespace (line 6) | namespace lightning::ui {
FILE: src/ui/ui_trajectory.cc
type lightning::ui (line 5) | namespace lightning::ui {
FILE: src/ui/ui_trajectory.h
function namespace (line 7) | namespace lightning::ui {
FILE: src/utils/async_message_process.h
function namespace (line 18) | namespace lightning {
FILE: src/utils/pointcloud_utils.cc
type lightning (line 10) | namespace lightning {
function CloudPtr (line 13) | CloudPtr VoxelGrid(CloudPtr cloud, float voxel_size) {
function RemoveGround (line 24) | void RemoveGround(CloudPtr cloud, float z_min) {
FILE: src/utils/pointcloud_utils.h
function namespace (line 10) | namespace lightning {
FILE: src/utils/sync.h
function namespace (line 14) | namespace lightning {
FILE: src/utils/timer.cc
type lightning (line 11) | namespace lightning {
FILE: src/utils/timer.h
function namespace (line 15) | namespace lightning {
FILE: src/wrapper/bag_io.cc
type lightning (line 12) | namespace lightning {
FILE: src/wrapper/bag_io.h
function namespace (line 29) | namespace lightning {
FILE: src/wrapper/ros_utils.h
function namespace (line 13) | namespace lightning {
FILE: thirdparty/Sophus/average.hpp
type Sophus (line 17) | namespace Sophus {
function iterativeMean (line 24) | optional<typename SequenceContainer::value_type> iterativeMean(
function average (line 67) | enable_if_t<
function average (line 88) | enable_if_t<
type details (line 104) | namespace details {
function getUnitQuaternion (line 109) | Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
function getUnitQuaternion (line 114) | Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
function averageUnitQuaternion (line 120) | Eigen::Quaternion<Scalar> averageUnitQuaternion(
function average (line 162) | enable_if_t<
function average (line 172) | enable_if_t<
function average (line 191) | enable_if_t<
function average (line 202) | enable_if_t<
function average (line 211) | enable_if_t<
function average (line 220) | enable_if_t<
FILE: thirdparty/Sophus/common.hpp
type Sophus (line 71) | namespace Sophus {
function SOPHUS_FUNC (line 86) | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file,...
type Constants (line 110) | struct Constants {
method SOPHUS_FUNC (line 111) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 113) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 124) | struct Constants<float> {
method epsilon (line 125) | constexpr epsilon() {
method SOPHUS_FUNC (line 129) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 131) | constexpr pi() {
type nullopt_t (line 137) | struct nullopt_t {
method nullopt_t (line 138) | explicit constexpr nullopt_t() {}
class optional (line 149) | class optional {
method optional (line 151) | optional() : is_valid_(false) {}
method optional (line 153) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 155) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 159) | T const* operator->() const {
method T (line 164) | T* operator->() {
method T (line 169) | T const& operator*() const {
method T (line 174) | T& operator*() {
type IsUniformRandomBitGenerator (line 188) | struct IsUniformRandomBitGenerator {
type Sophus (line 84) | namespace Sophus {
function SOPHUS_FUNC (line 86) | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file,...
type Constants (line 110) | struct Constants {
method SOPHUS_FUNC (line 111) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 113) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 124) | struct Constants<float> {
method epsilon (line 125) | constexpr epsilon() {
method SOPHUS_FUNC (line 129) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 131) | constexpr pi() {
type nullopt_t (line 137) | struct nullopt_t {
method nullopt_t (line 138) | explicit constexpr nullopt_t() {}
class optional (line 149) | class optional {
method optional (line 151) | optional() : is_valid_(false) {}
method optional (line 153) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 155) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 159) | T const* operator->() const {
method T (line 164) | T* operator->() {
method T (line 169) | T const& operator*() const {
method T (line 174) | T& operator*() {
type IsUniformRandomBitGenerator (line 188) | struct IsUniformRandomBitGenerator {
type Sophus (line 107) | namespace Sophus {
function SOPHUS_FUNC (line 86) | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file,...
type Constants (line 110) | struct Constants {
method SOPHUS_FUNC (line 111) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 113) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 124) | struct Constants<float> {
method epsilon (line 125) | constexpr epsilon() {
method SOPHUS_FUNC (line 129) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 131) | constexpr pi() {
type nullopt_t (line 137) | struct nullopt_t {
method nullopt_t (line 138) | explicit constexpr nullopt_t() {}
class optional (line 149) | class optional {
method optional (line 151) | optional() : is_valid_(false) {}
method optional (line 153) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 155) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 159) | T const* operator->() const {
method T (line 164) | T* operator->() {
method T (line 169) | T const& operator*() const {
method T (line 174) | T& operator*() {
type IsUniformRandomBitGenerator (line 188) | struct IsUniformRandomBitGenerator {
FILE: thirdparty/Sophus/formatstring.hpp
type Sophus (line 9) | namespace Sophus {
type details (line 10) | namespace details {
class IsStreamable (line 14) | class IsStreamable {
class ArgToStream (line 29) | class ArgToStream {
method impl (line 31) | static void impl(std::stringstream& stream, T&& arg) {
function FormatStream (line 36) | inline void FormatStream(std::stringstream& stream, char const* text) {
function FormatStream (line 43) | void FormatStream(std::stringstream& stream, char const* text, T&& arg,
function FormatString (line 61) | std::string FormatString(char const* text, Args&&... args) {
function FormatString (line 67) | inline std::string FormatString() { return std::string(); }
FILE: thirdparty/Sophus/geometry.hpp
type Sophus (line 13) | namespace Sophus {
function normalFromSO2 (line 19) | Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
function SO2FromNormal (line 29) | SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
function normalFromSO3 (line 41) | Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
function rotationFromNormal (line 58) | Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
function SO3FromNormal (line 116) | SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
function lineFromSE2 (line 126) | Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
function SE2FromLine (line 135) | SE2<T> SE2FromLine(Line2<T> const& line_foo) {
function planeFromSE3 (line 148) | Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
function SE3FromPlane (line 157) | SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
function makeHyperplaneUnique (line 168) | Eigen::Hyperplane<T, N> makeHyperplaneUnique(
FILE: thirdparty/Sophus/interpolate.hpp
type Sophus (line 11) | namespace Sophus {
function interpolate (line 27) | enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
FILE: thirdparty/Sophus/interpolate_details.hpp
type Sophus (line 13) | namespace Sophus {
type interp_details (line 14) | namespace interp_details {
type Traits (line 17) | struct Traits
type Traits<SO2<Scalar>> (line 20) | struct Traits<SO2<Scalar>> {
method hasShortestPathAmbiguity (line 23) | static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
type Traits<RxSO2<Scalar>> (line 32) | struct Traits<RxSO2<Scalar>> {
method hasShortestPathAmbiguity (line 35) | static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_ba...
type Traits<SO3<Scalar>> (line 41) | struct Traits<SO3<Scalar>> {
method hasShortestPathAmbiguity (line 44) | static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
type Traits<RxSO3<Scalar>> (line 53) | struct Traits<RxSO3<Scalar>> {
method hasShortestPathAmbiguity (line 56) | static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_ba...
type Traits<SE2<Scalar>> (line 62) | struct Traits<SE2<Scalar>> {
method hasShortestPathAmbiguity (line 65) | static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
type Traits<SE3<Scalar>> (line 71) | struct Traits<SE3<Scalar>> {
method hasShortestPathAmbiguity (line 74) | static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
type Traits<Sim2<Scalar>> (line 80) | struct Traits<Sim2<Scalar>> {
method hasShortestPathAmbiguity (line 83) | static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
type Traits<Sim3<Scalar>> (line 91) | struct Traits<Sim3<Scalar>> {
method hasShortestPathAmbiguity (line 94) | static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
FILE: thirdparty/Sophus/num_diff.hpp
type Sophus (line 13) | namespace Sophus {
type details (line 15) | namespace details {
class Curve (line 17) | class Curve {
method num_diff (line 20) | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(cur...
class VectorField (line 33) | class VectorField {
method num_diff (line 35) | static Eigen::Matrix<Scalar, N, M> num_diff(
class VectorField<Scalar, N, 1> (line 56) | class VectorField<Scalar, N, 1> {
method num_diff (line 58) | static Eigen::Matrix<Scalar, N, 1> num_diff(
function curveNumDiff (line 72) | auto curveNumDiff(Fn curve, Scalar t,
function vectorFieldNumDiff (line 84) | Eigen::Matrix<Scalar, N, M> vectorFieldNumDiff(
FILE: thirdparty/Sophus/rotation_matrix.hpp
type Sophus (line 12) | namespace Sophus {
function SOPHUS_FUNC (line 17) | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
function SOPHUS_FUNC (line 33) | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> co...
function SOPHUS_FUNC (line 59) | SOPHUS_FUNC enable_if_t<
FILE: thirdparty/Sophus/rxso2.hpp
type Sophus (line 9) | namespace Sophus {
class RxSO2 (line 11) | class RxSO2
method RxSO2 (line 398) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 402) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 407) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 424) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 431) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 438) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 450) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 455) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method Dxi_exp_x_matrix_at_0 (line 459) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 472) | exp(Tangent const& a) {
method generator (line 497) | generator(int i) {
method hat (line 519) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 537) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
method RxSO2 (line 549) | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 568) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 574) | SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
function setComplex (line 79) | class RxSO2Base {
function SOPHUS_FUNC (line 310) | SOPHUS_FUNC ComplexType const& complex() const {
function SOPHUS_FUNC (line 316) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 324) | SOPHUS_FUNC
function SOPHUS_FUNC (line 329) | SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(th...
function SOPHUS_FUNC (line 335) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 341) | SOPHUS_FUNC void setScale(Scalar const& scale) {
function SOPHUS_FUNC (line 352) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 360) | SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
function SOPHUS_FUNC (line 367) | SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
class RxSO2 (line 379) | class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
method RxSO2 (line 398) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 402) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 407) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 424) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 431) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 438) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 450) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 455) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method Dxi_exp_x_matrix_at_0 (line 459) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 472) | exp(Tangent const& a) {
method generator (line 497) | generator(int i) {
method hat (line 519) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 537) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
method RxSO2 (line 549) | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 568) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 574) | SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
type Eigen (line 16) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::RxSO2<Scalar_, Options_>> (line 20) | struct traits<Sophus::RxSO2<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO2<Scalar_>, Options_>> (line 27) | struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO2<Scalar_> const, Options_>> (line 35) | struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
class Map<Sophus::RxSO2<Scalar_>, Options> (line 588) | class Map<Sophus::RxSO2<Scalar_>, Options>
method SOPHUS_FUNC (line 609) | SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {}
type Sophus (line 44) | namespace Sophus {
class RxSO2 (line 11) | class RxSO2
method RxSO2 (line 398) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 402) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 407) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 424) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 431) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 438) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 450) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 455) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method Dxi_exp_x_matrix_at_0 (line 459) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 472) | exp(Tangent const& a) {
method generator (line 497) | generator(int i) {
method hat (line 519) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 537) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
method RxSO2 (line 549) | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 568) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 574) | SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
function setComplex (line 79) | class RxSO2Base {
function SOPHUS_FUNC (line 310) | SOPHUS_FUNC ComplexType const& complex() const {
function SOPHUS_FUNC (line 316) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 324) | SOPHUS_FUNC
function SOPHUS_FUNC (line 329) | SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(th...
function SOPHUS_FUNC (line 335) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 341) | SOPHUS_FUNC void setScale(Scalar const& scale) {
function SOPHUS_FUNC (line 352) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 360) | SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
function SOPHUS_FUNC (line 367) | SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
class RxSO2 (line 379) | class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
method RxSO2 (line 398) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 402) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 407) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 424) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 431) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 438) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 450) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 455) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method Dxi_exp_x_matrix_at_0 (line 459) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 472) | exp(Tangent const& a) {
method generator (line 497) | generator(int i) {
method hat (line 519) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 537) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
method RxSO2 (line 549) | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 568) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 574) | SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
type Eigen (line 581) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::RxSO2<Scalar_, Options_>> (line 20) | struct traits<Sophus::RxSO2<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO2<Scalar_>, Options_>> (line 27) | struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO2<Scalar_> const, Options_>> (line 35) | struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
class Map<Sophus::RxSO2<Scalar_>, Options> (line 588) | class Map<Sophus::RxSO2<Scalar_>, Options>
method SOPHUS_FUNC (line 609) | SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {}
class Map<Sophus::RxSO2<Scalar_> const, Options> (line 631) | class Map<Sophus::RxSO2<Scalar_> const, Options>
method SOPHUS_FUNC (line 645) | SOPHUS_FUNC
FILE: thirdparty/Sophus/rxso3.hpp
type Sophus (line 9) | namespace Sophus {
class RxSO3 (line 11) | class RxSO3
method RxSO3 (line 435) | RxSO3()
method SOPHUS_FUNC (line 440) | SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
method SOPHUS_FUNC (line 445) | SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 462) | SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 474) | SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
method SOPHUS_FUNC (line 487) | SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 497) | SOPHUS_FUNC QuaternionMember const& quaternion() const { return quat...
method Dxi_exp_x_matrix_at_0 (line 501) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 514) | exp(Tangent const& a) {
method expAndTheta (line 523) | expAndTheta(Tangent const& a,
method generator (line 562) | generator(int i) {
method hat (line 584) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 603) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent cons...
method RxSO3 (line 618) | static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 638) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 644) | SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quatern...
function setQuaternion (line 70) | class RxSO3Base {
function SOPHUS_FUNC (line 339) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 345) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 353) | SOPHUS_FUNC
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 370) | SOPHUS_FUNC
function SOPHUS_FUNC (line 382) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 397) | SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
function SOPHUS_FUNC (line 404) | SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
class RxSO3 (line 416) | class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
method RxSO3 (line 435) | RxSO3()
method SOPHUS_FUNC (line 440) | SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
method SOPHUS_FUNC (line 445) | SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 462) | SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 474) | SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
method SOPHUS_FUNC (line 487) | SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 497) | SOPHUS_FUNC QuaternionMember const& quaternion() const { return quat...
method Dxi_exp_x_matrix_at_0 (line 501) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 514) | exp(Tangent const& a) {
method expAndTheta (line 523) | expAndTheta(Tangent const& a,
method generator (line 562) | generator(int i) {
method hat (line 584) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 603) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent cons...
method RxSO3 (line 618) | static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 638) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 644) | SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quatern...
type Eigen (line 16) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::RxSO3<Scalar_, Options_>> (line 20) | struct traits<Sophus::RxSO3<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO3<Scalar_>, Options_>> (line 27) | struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO3<Scalar_> const, Options_>> (line 35) | struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
class Map<Sophus::RxSO3<Scalar_>, Options> (line 658) | class Map<Sophus::RxSO3<Scalar_>, Options>
method SOPHUS_FUNC (line 679) | SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {}
type Sophus (line 44) | namespace Sophus {
class RxSO3 (line 11) | class RxSO3
method RxSO3 (line 435) | RxSO3()
method SOPHUS_FUNC (line 440) | SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
method SOPHUS_FUNC (line 445) | SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 462) | SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 474) | SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
method SOPHUS_FUNC (line 487) | SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 497) | SOPHUS_FUNC QuaternionMember const& quaternion() const { return quat...
method Dxi_exp_x_matrix_at_0 (line 501) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 514) | exp(Tangent const& a) {
method expAndTheta (line 523) | expAndTheta(Tangent const& a,
method generator (line 562) | generator(int i) {
method hat (line 584) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 603) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent cons...
method RxSO3 (line 618) | static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 638) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 644) | SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quatern...
function setQuaternion (line 70) | class RxSO3Base {
function SOPHUS_FUNC (line 339) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 345) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 353) | SOPHUS_FUNC
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 370) | SOPHUS_FUNC
function SOPHUS_FUNC (line 382) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 397) | SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
function SOPHUS_FUNC (line 404) | SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
class RxSO3 (line 416) | class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
method RxSO3 (line 435) | RxSO3()
method SOPHUS_FUNC (line 440) | SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
method SOPHUS_FUNC (line 445) | SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 462) | SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 474) | SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
method SOPHUS_FUNC (line 487) | SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 497) | SOPHUS_FUNC QuaternionMember const& quaternion() const { return quat...
method Dxi_exp_x_matrix_at_0 (line 501) | Dxi_exp_x_matrix_at_0(int i) {
method exp (line 514) | exp(Tangent const& a) {
method expAndTheta (line 523) | expAndTheta(Tangent const& a,
method generator (line 562) | generator(int i) {
method hat (line 584) | hat(Tangent const& a) {
method SOPHUS_FUNC (line 603) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent cons...
method RxSO3 (line 618) | static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 638) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
method SOPHUS_FUNC (line 644) | SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quatern...
type Eigen (line 651) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::RxSO3<Scalar_, Options_>> (line 20) | struct traits<Sophus::RxSO3<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO3<Scalar_>, Options_>> (line 27) | struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO3<Scalar_> const, Options_>> (line 35) | struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
class Map<Sophus::RxSO3<Scalar_>, Options> (line 658) | class Map<Sophus::RxSO3<Scalar_>, Options>
method SOPHUS_FUNC (line 679) | SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {}
class Map<Sophus::RxSO3<Scalar_> const, Options> (line 701) | class Map<Sophus::RxSO3<Scalar_> const, Options>
method SOPHUS_FUNC (line 715) | SOPHUS_FUNC
FILE: thirdparty/Sophus/se2.hpp
type Sophus (line 9) | namespace Sophus {
class SE2 (line 11) | class SE2
class SE2Base (line 58) | class SE2Base {
method SOPHUS_FUNC (line 103) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 116) | SOPHUS_FUNC SE2<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 123) | Dx_this_mul_exp_x_at_0()
method inverse (line 145) | inverse() const {
method SOPHUS_FUNC (line 160) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 188) | SOPHUS_FUNC void normalize() { so2().normalize(); }
method matrix (line 200) | matrix() const {
method matrix2x3 (line 210) | matrix2x3() const {
method SOPHUS_FUNC (line 219) | SOPHUS_FUNC SE2Base& operator=(SE2Base const& other) = default;
method SOPHUS_FUNC (line 224) | SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const&...
method SOPHUS_FUNC (line 233) | SOPHUS_FUNC SE2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 250) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 260) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 275) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 285) | SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const...
function SOPHUS_FUNC (line 311) | SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
function SOPHUS_FUNC (line 319) | SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC
function SOPHUS_FUNC (line 335) | SOPHUS_FUNC
function SOPHUS_FUNC (line 342) | SOPHUS_FUNC
function SOPHUS_FUNC (line 349) | SOPHUS_FUNC
function SOPHUS_FUNC (line 356) | SOPHUS_FUNC
type Eigen (line 16) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::SE2<Scalar_, Options>> (line 20) | struct traits<Sophus::SE2<Scalar_, Options>> {
type traits<Map<Sophus::SE2<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::SE2<Scalar_>, Options>>
type traits<Map<Sophus::SE2<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
class Map<Sophus::SE2<Scalar_>, Options> (line 749) | class Map<Sophus::SE2<Scalar_>, Options>
method SOPHUS_FUNC (line 774) | SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
function SOPHUS_FUNC (line 790) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
type Sophus (line 44) | namespace Sophus {
class SE2 (line 11) | class SE2
class SE2Base (line 58) | class SE2Base {
method SOPHUS_FUNC (line 103) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 116) | SOPHUS_FUNC SE2<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 123) | Dx_this_mul_exp_x_at_0()
method inverse (line 145) | inverse() const {
method SOPHUS_FUNC (line 160) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 188) | SOPHUS_FUNC void normalize() { so2().normalize(); }
method matrix (line 200) | matrix() const {
method matrix2x3 (line 210) | matrix2x3() const {
method SOPHUS_FUNC (line 219) | SOPHUS_FUNC SE2Base& operator=(SE2Base const& other) = default;
method SOPHUS_FUNC (line 224) | SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const&...
method SOPHUS_FUNC (line 233) | SOPHUS_FUNC SE2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 250) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 260) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 275) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 285) | SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const...
function SOPHUS_FUNC (line 311) | SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
function SOPHUS_FUNC (line 319) | SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC
function SOPHUS_FUNC (line 335) | SOPHUS_FUNC
function SOPHUS_FUNC (line 342) | SOPHUS_FUNC
function SOPHUS_FUNC (line 349) | SOPHUS_FUNC
function SOPHUS_FUNC (line 356) | SOPHUS_FUNC
function SOPHUS_FUNC (line 388) | SOPHUS_FUNC SE2(SE2 const& other) = default;
function SOPHUS_FUNC (line 446) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 453) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 460) | SOPHUS_FUNC SO2Member& so2() { return so2_; }
function SOPHUS_FUNC (line 464) | SOPHUS_FUNC SO2Member const& so2() const { return so2_; }
function SOPHUS_FUNC (line 468) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 472) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 478) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 530) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 544) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 560) | SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 586) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
function SOPHUS_FUNC (line 612) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 633) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 650) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function SOPHUS_FUNC (line 662) | static SOPHUS_FUNC SE2 rot(Scalar const& x) {
function SE2 (line 671) | static SE2 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 680) | static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
function SOPHUS_FUNC (line 684) | static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {
function SOPHUS_FUNC (line 690) | static SOPHUS_FUNC SE2 transX(Scalar const& x) {
function SOPHUS_FUNC (line 696) | static SOPHUS_FUNC SE2 transY(Scalar const& y) {
function SOPHUS_FUNC (line 713) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 743) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::SE2<Scalar_, Options>> (line 20) | struct traits<Sophus::SE2<Scalar_, Options>> {
type traits<Map<Sophus::SE2<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::SE2<Scalar_>, Options>>
type traits<Map<Sophus::SE2<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
class Map<Sophus::SE2<Scalar_>, Options> (line 749) | class Map<Sophus::SE2<Scalar_>, Options>
method SOPHUS_FUNC (line 774) | SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
function SOPHUS_FUNC (line 790) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
class Map<Sophus::SE2<Scalar_> const, Options> (line 803) | class Map<Sophus::SE2<Scalar_> const, Options>
FILE: thirdparty/Sophus/se3.hpp
type Sophus (line 9) | namespace Sophus {
class SE3 (line 11) | class SE3
class SE3Base (line 58) | class SE3Base {
method SOPHUS_FUNC (line 103) | SOPHUS_FUNC Adjoint Adj() const {
method Scalar (line 115) | Scalar angleX() const { return so3().angleX(); }
method Scalar (line 119) | Scalar angleY() const { return so3().angleY(); }
method Scalar (line 123) | Scalar angleZ() const { return so3().angleZ(); }
method SOPHUS_FUNC (line 128) | SOPHUS_FUNC SE3<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 135) | Dx_this_mul_exp_x_at_0()
method inverse (line 208) | inverse() const {
method SOPHUS_FUNC (line 223) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 263) | SOPHUS_FUNC void normalize() { so3().normalize(); }
method matrix (line 275) | matrix() const {
method matrix3x4 (line 285) | matrix3x4() const {
method SOPHUS_FUNC (line 294) | SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) = default;
method SOPHUS_FUNC (line 299) | SOPHUS_FUNC SE3Base<Derived>& operator=(SE3Base<OtherDerived> const&...
method SOPHUS_FUNC (line 308) | SOPHUS_FUNC SE3Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 325) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 335) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 350) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 360) | SOPHUS_FUNC SE3Base<Derived>& operator*=(SE3Base<OtherDerived> const...
method rotationMatrix (line 367) | rotationMatrix() const { return so3().matrix(); }
method SOPHUS_FUNC (line 371) | SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3...
method SOPHUS_FUNC (line 375) | SOPHUS_FUNC SO3Type const& so3() const {
method SOPHUS_FUNC (line 383) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
method SOPHUS_FUNC (line 391) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& R) {
function SOPHUS_FUNC (line 417) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 423) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
type Eigen (line 16) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::SE3<Scalar_, Options>> (line 20) | struct traits<Sophus::SE3<Scalar_, Options>> {
type traits<Map<Sophus::SE3<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::SE3<Scalar_>, Options>>
type traits<Map<Sophus::SE3<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
class Map<Sophus::SE3<Scalar_>, Options> (line 991) | class Map<Sophus::SE3<Scalar_>, Options>
method SOPHUS_FUNC (line 1015) | SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options>& so3() { return so3_; }
function SOPHUS_FUNC (line 1031) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar, Options>> const& translation()...
type Sophus (line 44) | namespace Sophus {
class SE3 (line 11) | class SE3
class SE3Base (line 58) | class SE3Base {
method SOPHUS_FUNC (line 103) | SOPHUS_FUNC Adjoint Adj() const {
method Scalar (line 115) | Scalar angleX() const { return so3().angleX(); }
method Scalar (line 119) | Scalar angleY() const { return so3().angleY(); }
method Scalar (line 123) | Scalar angleZ() const { return so3().angleZ(); }
method SOPHUS_FUNC (line 128) | SOPHUS_FUNC SE3<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 135) | Dx_this_mul_exp_x_at_0()
method inverse (line 208) | inverse() const {
method SOPHUS_FUNC (line 223) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 263) | SOPHUS_FUNC void normalize() { so3().normalize(); }
method matrix (line 275) | matrix() const {
method matrix3x4 (line 285) | matrix3x4() const {
method SOPHUS_FUNC (line 294) | SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) = default;
method SOPHUS_FUNC (line 299) | SOPHUS_FUNC SE3Base<Derived>& operator=(SE3Base<OtherDerived> const&...
method SOPHUS_FUNC (line 308) | SOPHUS_FUNC SE3Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 325) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 335) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 350) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 360) | SOPHUS_FUNC SE3Base<Derived>& operator*=(SE3Base<OtherDerived> const...
method rotationMatrix (line 367) | rotationMatrix() const { return so3().matrix(); }
method SOPHUS_FUNC (line 371) | SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3...
method SOPHUS_FUNC (line 375) | SOPHUS_FUNC SO3Type const& so3() const {
method SOPHUS_FUNC (line 383) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
method SOPHUS_FUNC (line 391) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& R) {
function SOPHUS_FUNC (line 417) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 423) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
function SOPHUS_FUNC (line 454) | SOPHUS_FUNC SE3(SE3 const& other) = default;
function SOPHUS_FUNC (line 513) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 520) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 527) | SOPHUS_FUNC SO3Member& so3() { return so3_; }
function SOPHUS_FUNC (line 531) | SOPHUS_FUNC SO3Member const& so3() const { return so3_; }
function SOPHUS_FUNC (line 535) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 539) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 545) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 727) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 747) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 763) | SOPHUS_FUNC static SE3<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 789) | SOPHUS_FUNC static enable_if_t<std::is_floating_point<S>::value, SE3>
function SOPHUS_FUNC (line 833) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 854) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 872) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function SOPHUS_FUNC (line 887) | static SOPHUS_FUNC SE3 rotX(Scalar const& x) {
function SOPHUS_FUNC (line 893) | static SOPHUS_FUNC SE3 rotY(Scalar const& y) {
function SOPHUS_FUNC (line 899) | static SOPHUS_FUNC SE3 rotZ(Scalar const& z) {
function SE3 (line 908) | static SE3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 918) | static SOPHUS_FUNC SE3 trans(T0 const& x, T1 const& y, T2 const& z) {
function SOPHUS_FUNC (line 922) | static SOPHUS_FUNC SE3 trans(Vector3<Scalar> const& xyz) {
function SOPHUS_FUNC (line 928) | static SOPHUS_FUNC SE3 transX(Scalar const& x) {
function SOPHUS_FUNC (line 934) | static SOPHUS_FUNC SE3 transY(Scalar const& y) {
function SOPHUS_FUNC (line 940) | static SOPHUS_FUNC SE3 transZ(Scalar const& z) {
function SOPHUS_FUNC (line 958) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 985) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::SE3<Scalar_, Options>> (line 20) | struct traits<Sophus::SE3<Scalar_, Options>> {
type traits<Map<Sophus::SE3<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::SE3<Scalar_>, Options>>
type traits<Map<Sophus::SE3<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
class Map<Sophus::SE3<Scalar_>, Options> (line 991) | class Map<Sophus::SE3<Scalar_>, Options>
method SOPHUS_FUNC (line 1015) | SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options>& so3() { return so3_; }
function SOPHUS_FUNC (line 1031) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar, Options>> const& translation()...
class Map<Sophus::SE3<Scalar_> const, Options> (line 1044) | class Map<Sophus::SE3<Scalar_> const, Options>
FILE: thirdparty/Sophus/sim2.hpp
type Sophus (line 10) | namespace Sophus {
class Sim2 (line 12) | class Sim2
class Sim2Base (line 59) | class Sim2Base {
method SOPHUS_FUNC (line 104) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 121) | SOPHUS_FUNC Sim2<NewScalarType> cast() const {
method inverse (line 128) | inverse() const {
method SOPHUS_FUNC (line 143) | SOPHUS_FUNC Tangent log() const {
method matrix (line 174) | matrix() const {
method matrix2x3 (line 184) | matrix2x3() const {
method SOPHUS_FUNC (line 193) | SOPHUS_FUNC Sim2Base& operator=(Sim2Base const& other) = default;
method SOPHUS_FUNC (line 198) | SOPHUS_FUNC Sim2Base<Derived>& operator=(
method SOPHUS_FUNC (line 211) | SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 228) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 238) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 253) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 285) | SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
function SOPHUS_FUNC (line 291) | SOPHUS_FUNC
function SOPHUS_FUNC (line 299) | SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 305) | SOPHUS_FUNC RxSO2Type& rxso2() {
function SOPHUS_FUNC (line 311) | SOPHUS_FUNC RxSO2Type const& rxso2() const {
function SOPHUS_FUNC (line 317) | SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); }
function SOPHUS_FUNC (line 321) | SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar>& R) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scal...
function SOPHUS_FUNC (line 337) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) {
function SOPHUS_FUNC (line 343) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 349) | SOPHUS_FUNC TranslationType const& translation() const {
type Eigen (line 17) | namespace Eigen {
type internal (line 18) | namespace internal {
type traits<Sophus::Sim2<Scalar_, Options>> (line 21) | struct traits<Sophus::Sim2<Scalar_, Options>> {
type traits<Map<Sophus::Sim2<Scalar_>, Options>> (line 28) | struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
type traits<Map<Sophus::Sim2<Scalar_> const, Options>> (line 36) | struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
class Map<Sophus::Sim2<Scalar_>, Options> (line 638) | class Map<Sophus::Sim2<Scalar_>, Options>
method SOPHUS_FUNC (line 662) | SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rx...
function SOPHUS_FUNC (line 677) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
type Sophus (line 45) | namespace Sophus {
class Sim2 (line 12) | class Sim2
class Sim2Base (line 59) | class Sim2Base {
method SOPHUS_FUNC (line 104) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 121) | SOPHUS_FUNC Sim2<NewScalarType> cast() const {
method inverse (line 128) | inverse() const {
method SOPHUS_FUNC (line 143) | SOPHUS_FUNC Tangent log() const {
method matrix (line 174) | matrix() const {
method matrix2x3 (line 184) | matrix2x3() const {
method SOPHUS_FUNC (line 193) | SOPHUS_FUNC Sim2Base& operator=(Sim2Base const& other) = default;
method SOPHUS_FUNC (line 198) | SOPHUS_FUNC Sim2Base<Derived>& operator=(
method SOPHUS_FUNC (line 211) | SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 228) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 238) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 253) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 285) | SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
function SOPHUS_FUNC (line 291) | SOPHUS_FUNC
function SOPHUS_FUNC (line 299) | SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 305) | SOPHUS_FUNC RxSO2Type& rxso2() {
function SOPHUS_FUNC (line 311) | SOPHUS_FUNC RxSO2Type const& rxso2() const {
function SOPHUS_FUNC (line 317) | SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); }
function SOPHUS_FUNC (line 321) | SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar>& R) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scal...
function SOPHUS_FUNC (line 337) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) {
function SOPHUS_FUNC (line 343) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 349) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 376) | SOPHUS_FUNC Sim2(Sim2 const& other) = default;
function SOPHUS_FUNC (line 425) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 432) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 439) | SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; }
function SOPHUS_FUNC (line 443) | SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; }
function SOPHUS_FUNC (line 447) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 451) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 457) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 482) | SOPHUS_FUNC static Sim2<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 520) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 541) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 559) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function Sim2 (line 585) | static Sim2 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 604) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 632) | namespace Eigen {
type internal (line 18) | namespace internal {
type traits<Sophus::Sim2<Scalar_, Options>> (line 21) | struct traits<Sophus::Sim2<Scalar_, Options>> {
type traits<Map<Sophus::Sim2<Scalar_>, Options>> (line 28) | struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
type traits<Map<Sophus::Sim2<Scalar_> const, Options>> (line 36) | struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
class Map<Sophus::Sim2<Scalar_>, Options> (line 638) | class Map<Sophus::Sim2<Scalar_>, Options>
method SOPHUS_FUNC (line 662) | SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rx...
function SOPHUS_FUNC (line 677) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
class Map<Sophus::Sim2<Scalar_> const, Options> (line 690) | class Map<Sophus::Sim2<Scalar_> const, Options>
FILE: thirdparty/Sophus/sim3.hpp
type Sophus (line 10) | namespace Sophus {
class Sim3 (line 12) | class Sim3
function setQuaternion (line 59) | class Sim3Base {
function SOPHUS_FUNC (line 293) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 299) | SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 305) | SOPHUS_FUNC RxSO3Type& rxso3() {
function SOPHUS_FUNC (line 311) | SOPHUS_FUNC RxSO3Type const& rxso3() const {
function SOPHUS_FUNC (line 317) | SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
function SOPHUS_FUNC (line 321) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scal...
function SOPHUS_FUNC (line 337) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
function SOPHUS_FUNC (line 343) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 349) | SOPHUS_FUNC TranslationType const& translation() const {
type Eigen (line 17) | namespace Eigen {
type internal (line 18) | namespace internal {
type traits<Sophus::Sim3<Scalar_, Options>> (line 21) | struct traits<Sophus::Sim3<Scalar_, Options>> {
type traits<Map<Sophus::Sim3<Scalar_>, Options>> (line 28) | struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
type traits<Map<Sophus::Sim3<Scalar_> const, Options>> (line 36) | struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
class Map<Sophus::Sim3<Scalar_>, Options> (line 655) | class Map<Sophus::Sim3<Scalar_>, Options>
method SOPHUS_FUNC (line 679) | SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rx...
function SOPHUS_FUNC (line 694) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation()...
type Sophus (line 45) | namespace Sophus {
class Sim3 (line 12) | class Sim3
function setQuaternion (line 59) | class Sim3Base {
function SOPHUS_FUNC (line 293) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 299) | SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 305) | SOPHUS_FUNC RxSO3Type& rxso3() {
function SOPHUS_FUNC (line 311) | SOPHUS_FUNC RxSO3Type const& rxso3() const {
function SOPHUS_FUNC (line 317) | SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
function SOPHUS_FUNC (line 321) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scal...
function SOPHUS_FUNC (line 337) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
function SOPHUS_FUNC (line 343) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 349) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 376) | SOPHUS_FUNC Sim3(Sim3 const& other) = default;
function SOPHUS_FUNC (line 427) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 434) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 441) | SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }
function SOPHUS_FUNC (line 445) | SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }
function SOPHUS_FUNC (line 449) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 453) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 459) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 476) | SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 536) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 557) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 575) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function Sim3 (line 600) | static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 621) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 649) | namespace Eigen {
type internal (line 18) | namespace internal {
type traits<Sophus::Sim3<Scalar_, Options>> (line 21) | struct traits<Sophus::Sim3<Scalar_, Options>> {
type traits<Map<Sophus::Sim3<Scalar_>, Options>> (line 28) | struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
type traits<Map<Sophus::Sim3<Scalar_> const, Options>> (line 36) | struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
class Map<Sophus::Sim3<Scalar_>, Options> (line 655) | class Map<Sophus::Sim3<Scalar_>, Options>
method SOPHUS_FUNC (line 679) | SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rx...
function SOPHUS_FUNC (line 694) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation()...
class Map<Sophus::Sim3<Scalar_> const, Options> (line 707) | class Map<Sophus::Sim3<Scalar_> const, Options>
FILE: thirdparty/Sophus/sim_details.hpp
type Sophus (line 6) | namespace Sophus {
type details (line 7) | namespace details {
function calcW (line 10) | Matrix<Scalar, N, N> calcW(Matrix<Scalar, N, N> const& Omega,
function calcWInv (line 52) | Matrix<Scalar, N, N> calcWInv(Matrix<Scalar, N, N> const& Omega,
FILE: thirdparty/Sophus/so2.hpp
type Sophus (line 17) | namespace Sophus {
class SO2 (line 19) | class SO2
function setComplex (line 80) | class SO2Base {
function SOPHUS_FUNC (line 322) | SOPHUS_FUNC
function SO2 (line 360) | SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
function SOPHUS_FUNC (line 364) | SOPHUS_FUNC SO2(SO2 const& other) = default;
type Eigen (line 24) | namespace Eigen {
type internal (line 25) | namespace internal {
type traits<Sophus::SO2<Scalar_, Options_>> (line 28) | struct traits<Sophus::SO2<Scalar_, Options_>> {
type traits<Map<Sophus::SO2<Scalar_>, Options_>> (line 35) | struct traits<Map<Sophus::SO2<Scalar_>, Options_>>
type traits<Map<Sophus::SO2<Scalar_> const, Options_>> (line 43) | struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>
class Map<Sophus::SO2<Scalar_>, Options> (line 548) | class Map<Sophus::SO2<Scalar_>, Options>
method SOPHUS_FUNC (line 570) | SOPHUS_FUNC
type Sophus (line 52) | namespace Sophus {
class SO2 (line 19) | class SO2
function setComplex (line 80) | class SO2Base {
function SOPHUS_FUNC (line 322) | SOPHUS_FUNC
function SO2 (line 360) | SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
function SOPHUS_FUNC (line 364) | SOPHUS_FUNC SO2(SO2 const& other) = default;
function SOPHUS_FUNC (line 388) | SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
function SOPHUS_FUNC (line 398) | SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
function SOPHUS_FUNC (line 407) | SOPHUS_FUNC explicit SO2(Scalar theta) {
function SOPHUS_FUNC (line 413) | SOPHUS_FUNC ComplexMember const& unit_complex() const {
function SOPHUS_FUNC (line 426) | SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
function SOPHUS_FUNC (line 434) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 443) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 450) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {
function SOPHUS_FUNC (line 461) | SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); }
function SOPHUS_FUNC (line 476) | SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
function SOPHUS_FUNC (line 489) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO2>
function SOPHUS_FUNC (line 499) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
function SO2 (line 506) | static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 541) | namespace Eigen {
type internal (line 25) | namespace internal {
type traits<Sophus::SO2<Scalar_, Options_>> (line 28) | struct traits<Sophus::SO2<Scalar_, Options_>> {
type traits<Map<Sophus::SO2<Scalar_>, Options_>> (line 35) | struct traits<Map<Sophus::SO2<Scalar_>, Options_>>
type traits<Map<Sophus::SO2<Scalar_> const, Options_>> (line 43) | struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>
class Map<Sophus::SO2<Scalar_>, Options> (line 548) | class Map<Sophus::SO2<Scalar_>, Options>
method SOPHUS_FUNC (line 570) | SOPHUS_FUNC
class Map<Sophus::SO2<Scalar_> const, Options> (line 596) | class Map<Sophus::SO2<Scalar_> const, Options>
method SOPHUS_FUNC (line 610) | SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {}
FILE: thirdparty/Sophus/so3.hpp
type Sophus (line 17) | namespace Sophus {
class SO3 (line 19) | class SO3
class SO3Base (line 75) | class SO3Base {
type TangentAndTheta (line 95) | struct TangentAndTheta {
method SOPHUS_FUNC (line 126) | SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
method hat (line 142) | hat(Tangent const& omega) {
method SOPHUS_FUNC (line 155) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX(...
method SOPHUS_FUNC (line 164) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY(...
method SOPHUS_FUNC (line 178) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ(...
method SOPHUS_FUNC (line 187) | SOPHUS_FUNC SO3<NewScalarType> cast() const {
method SOPHUS_FUNC (line 199) | SOPHUS_FUNC Scalar* data() { return unit_quaternion_nonconst().coeff...
method SOPHUS_FUNC (line 203) | SOPHUS_FUNC Scalar const* data() const { return unit_quaternion().co...
method Dx_this_mul_exp_x_at_0 (line 207) | Dx_this_mul_exp_x_at_0() const {
function SOPHUS_FUNC (line 254) | SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
function SOPHUS_FUNC (line 258) | SOPHUS_FUNC TangentAndTheta logAndTheta() const {
function SOPHUS_FUNC (line 306) | SOPHUS_FUNC void normalize() {
function SOPHUS_FUNC (line 318) | SOPHUS_FUNC Transformation matrix() const { return unit_quaternion().t...
function SOPHUS_FUNC (line 327) | SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& o...
function SOPHUS_FUNC (line 335) | SOPHUS_FUNC SO3Product<OtherDerived> operator*(SO3Base<OtherDerived> c...
function SOPHUS_FUNC (line 365) | SOPHUS_FUNC PointProduct<PointDerived> operator*(Eigen::MatrixBase<Poi...
function SOPHUS_FUNC (line 378) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(Eigen::Ma...
function SOPHUS_FUNC (line 390) | SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) ...
function SOPHUS_FUNC (line 397) | SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& ...
function SOPHUS_FUNC (line 406) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quater...
function SOPHUS_FUNC (line 413) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
function SOPHUS_FUNC (line 419) | SOPHUS_FUNC static Transformation jl(const Point& Omega) {
function SOPHUS_FUNC (line 433) | SOPHUS_FUNC static Transformation jl_inv(const Point& Omega) {
function SOPHUS_FUNC (line 447) | SOPHUS_FUNC static Transformation jr(const Point& Omega) { return jl(-...
function SOPHUS_FUNC (line 448) | SOPHUS_FUNC Transformation jr() { return jr(log()); }
function SOPHUS_FUNC (line 449) | SOPHUS_FUNC Transformation jr_inv() { return jl_inv(-log()); }
function SOPHUS_FUNC (line 450) | SOPHUS_FUNC static Transformation jr_inv(const Point& Omega) { return ...
function SOPHUS_FUNC (line 451) | SOPHUS_FUNC static Transformation jr_inv(const SO3Base<Derived>& R) { ...
function Dxi_exp_x_matrix_at_0 (line 464) | class SO3 : public SO3Base<SO3<Scalar_, Options>> {
function SOPHUS_FUNC (line 597) | SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
function SOPHUS_FUNC (line 606) | SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega, Scala...
function SOPHUS_FUNC (line 640) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3> ...
function SOPHUS_FUNC (line 664) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 685) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1, Tangent c...
function SOPHUS_FUNC (line 689) | static SOPHUS_FUNC SO3 rotX(Scalar const& x) { return SO3::exp(Sophus:...
function SOPHUS_FUNC (line 693) | static SOPHUS_FUNC SO3 rotY(Scalar const& y) { return SO3::exp(Sophus:...
function SOPHUS_FUNC (line 697) | static SOPHUS_FUNC SO3 rotZ(Scalar const& z) { return SO3::exp(Sophus:...
function SO3 (line 703) | static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 733) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 24) | namespace Eigen {
type internal (line 25) | namespace internal {
type traits<Sophus::SO3<Scalar_, Options_>> (line 28) | struct traits<Sophus::SO3<Scalar_, Options_>> {
type traits<Map<Sophus::SO3<Scalar_>, Options_>> (line 35) | struct traits<Map<Sophus::SO3<Scalar_>, Options_>> : traits<Sophus::...
type traits<Map<Sophus::SO3<Scalar_> const, Options_>> (line 42) | struct traits<Map<Sophus::SO3<Scalar_> const, Options_>> : traits<So...
class Map<Sophus::SO3<Scalar_>, Options> (line 753) | class Map<Sophus::SO3<Scalar_>, Options> : public Sophus::SO3Base<Map<...
method SOPHUS_FUNC (line 774) | SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
type Sophus (line 50) | namespace Sophus {
class SO3 (line 19) | class SO3
class SO3Base (line 75) | class SO3Base {
type TangentAndTheta (line 95) | struct TangentAndTheta {
method SOPHUS_FUNC (line 126) | SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
method hat (line 142) | hat(Tangent const& omega) {
method SOPHUS_FUNC (line 155) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX(...
method SOPHUS_FUNC (line 164) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY(...
method SOPHUS_FUNC (line 178) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ(...
method SOPHUS_FUNC (line 187) | SOPHUS_FUNC SO3<NewScalarType> cast() const {
method SOPHUS_FUNC (line 199) | SOPHUS_FUNC Scalar* data() { return unit_quaternion_nonconst().coeff...
method SOPHUS_FUNC (line 203) | SOPHUS_FUNC Scalar const* data() const { return unit_quaternion().co...
method Dx_this_mul_exp_x_at_0 (line 207) | Dx_this_mul_exp_x_at_0() const {
function SOPHUS_FUNC (line 254) | SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
function SOPHUS_FUNC (line 258) | SOPHUS_FUNC TangentAndTheta logAndTheta() const {
function SOPHUS_FUNC (line 306) | SOPHUS_FUNC void normalize() {
function SOPHUS_FUNC (line 318) | SOPHUS_FUNC Transformation matrix() const { return unit_quaternion().t...
function SOPHUS_FUNC (line 327) | SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& o...
function SOPHUS_FUNC (line 335) | SOPHUS_FUNC SO3Product<OtherDerived> operator*(SO3Base<OtherDerived> c...
function SOPHUS_FUNC (line 365) | SOPHUS_FUNC PointProduct<PointDerived> operator*(Eigen::MatrixBase<Poi...
function SOPHUS_FUNC (line 378) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(Eigen::Ma...
function SOPHUS_FUNC (line 390) | SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) ...
function SOPHUS_FUNC (line 397) | SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& ...
function SOPHUS_FUNC (line 406) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quater...
function SOPHUS_FUNC (line 413) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
function SOPHUS_FUNC (line 419) | SOPHUS_FUNC static Transformation jl(const Point& Omega) {
function SOPHUS_FUNC (line 433) | SOPHUS_FUNC static Transformation jl_inv(const Point& Omega) {
function SOPHUS_FUNC (line 447) | SOPHUS_FUNC static Transformation jr(const Point& Omega) { return jl(-...
function SOPHUS_FUNC (line 448) | SOPHUS_FUNC Transformation jr() { return jr(log()); }
function SOPHUS_FUNC (line 449) | SOPHUS_FUNC Transformation jr_inv() { return jl_inv(-log()); }
function SOPHUS_FUNC (line 450) | SOPHUS_FUNC static Transformation jr_inv(const Point& Omega) { return ...
function SOPHUS_FUNC (line 451) | SOPHUS_FUNC static Transformation jr_inv(const SO3Base<Derived>& R) { ...
function Dxi_exp_x_matrix_at_0 (line 464) | class SO3 : public SO3Base<SO3<Scalar_, Options>> {
function SOPHUS_FUNC (line 597) | SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
function SOPHUS_FUNC (line 606) | SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega, Scala...
function SOPHUS_FUNC (line 640) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3> ...
function SOPHUS_FUNC (line 664) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 685) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1, Tangent c...
function SOPHUS_FUNC (line 689) | static SOPHUS_FUNC SO3 rotX(Scalar const& x) { return SO3::exp(Sophus:...
function SOPHUS_FUNC (line 693) | static SOPHUS_FUNC SO3 rotY(Scalar const& y) { return SO3::exp(Sophus:...
function SOPHUS_FUNC (line 697) | static SOPHUS_FUNC SO3 rotZ(Scalar const& z) { return SO3::exp(Sophus:...
function SO3 (line 703) | static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 733) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 747) | namespace Eigen {
type internal (line 25) | namespace internal {
type traits<Sophus::SO3<Scalar_, Options_>> (line 28) | struct traits<Sophus::SO3<Scalar_, Options_>> {
type traits<Map<Sophus::SO3<Scalar_>, Options_>> (line 35) | struct traits<Map<Sophus::SO3<Scalar_>, Options_>> : traits<Sophus::...
type traits<Map<Sophus::SO3<Scalar_> const, Options_>> (line 42) | struct traits<Map<Sophus::SO3<Scalar_> const, Options_>> : traits<So...
class Map<Sophus::SO3<Scalar_>, Options> (line 753) | class Map<Sophus::SO3<Scalar_>, Options> : public Sophus::SO3Base<Map<...
method SOPHUS_FUNC (line 774) | SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
class Map<Sophus::SO3<Scalar_> const, Options> (line 793) | class Map<Sophus::SO3<Scalar_> const, Options> : public Sophus::SO3Base<...
method SOPHUS_FUNC (line 806) | SOPHUS_FUNC Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {}
FILE: thirdparty/Sophus/types.hpp
type Sophus (line 10) | namespace Sophus {
type details (line 81) | namespace details {
class MaxMetric (line 83) | class MaxMetric {
method Scalar (line 85) | static Scalar impl(Scalar s0, Scalar s1) {
class MaxMetric<Matrix<Scalar, M, N>> (line 92) | class MaxMetric<Matrix<Scalar, M, N>> {
method Scalar (line 94) | static Scalar impl(Matrix<Scalar, M, N> const& p0,
class SetToZero (line 101) | class SetToZero {
method impl (line 103) | static void impl(Scalar& s) { s = Scalar(0); }
class SetToZero<Matrix<Scalar, M, N>> (line 107) | class SetToZero<Matrix<Scalar, M, N>> {
method impl (line 109) | static void impl(Matrix<Scalar, M, N>& v) { v.setZero(); }
class SetElementAt (line 113) | class SetElementAt
class SetElementAt<Scalar, Scalar> (line 116) | class SetElementAt<Scalar, Scalar> {
method impl (line 118) | static void impl(Scalar& s, Scalar value, int at) {
class SetElementAt<Vector<Scalar, N>, Scalar> (line 125) | class SetElementAt<Vector<Scalar, N>, Scalar> {
method impl (line 127) | static void impl(Vector<Scalar, N>& v, Scalar value, int at) {
class SquaredNorm (line 134) | class SquaredNorm {
method Scalar (line 136) | static Scalar impl(Scalar const& s) { return s * s; }
class SquaredNorm<Matrix<Scalar, N, 1>> (line 140) | class SquaredNorm<Matrix<Scalar, N, 1>> {
method Scalar (line 142) | static Scalar impl(Matrix<Scalar, N, 1> const& s) { return s.squar...
class Transpose (line 146) | class Transpose {
method Scalar (line 148) | static Scalar impl(Scalar const& s) { return s; }
class Transpose<Matrix<Scalar, M, N>> (line 152) | class Transpose<Matrix<Scalar, M, N>> {
method impl (line 154) | static Matrix<Scalar, M, N> impl(Matrix<Scalar, M, N> const& s) {
function maxMetric (line 164) | auto maxMetric(T const& p0, T const& p1)
function setToZero (line 172) | void setToZero(T& p) {
function setElementAt (line 180) | void setElementAt(T& p, Scalar value, int i) {
function squaredNorm (line 187) | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm<T>::impl...
function transpose (line 195) | auto transpose(T const& p) -> decltype(details::Transpose<T>::impl(T()...
type IsFloatingPoint (line 200) | struct IsFloatingPoint {
type IsFloatingPoint<Matrix<Scalar, M, N>> (line 205) | struct IsFloatingPoint<Matrix<Scalar, M, N>> {
type GetScalar (line 210) | struct GetScalar {
type GetScalar<Matrix<Scalar_, M, N>> (line 215) | struct GetScalar<Matrix<Scalar_, M, N>> {
type IsFixedSizeVector (line 225) | struct IsFixedSizeVector : std::true_type {}
FILE: thirdparty/Sophus/velocities.hpp
type Sophus (line 9) | namespace Sophus {
type experimental (line 10) | namespace experimental {
function transformVelocity (line 18) | Vector3<Scalar> transformVelocity(SO3<Scalar> const& foo_R_bar,
function transformVelocity (line 35) | Vector3<Scalar> transformVelocity(SE3<Scalar> const& foo_T_bar,
function finiteDifferenceRotationalVelocity (line 43) | Vector3<Scalar> finiteDifferenceRotationalVelocity(
function finiteDifferenceRotationalVelocity (line 63) | Vector3<Scalar> finiteDifferenceRotationalVelocity(
Condensed preview — 235 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,333K chars).
[
{
"path": ".clang-format",
"chars": 3298,
"preview": "---\nLanguage: Cpp\n# BasedOnStyle: Google\nAccessModifierOffset: -1\nAlignAfterOpenBracket: Align\nAlignConsecutiveA"
},
{
"path": ".gitignore",
"chars": 106,
"preview": "*build\n*install\n*log\n*.vscode\n*libs\n**data\n**/.vscode/\nbin\n\n**cmake-build-debug**\n**cmake-build-release**\n"
},
{
"path": "CMakeLists.txt",
"chars": 1731,
"preview": "cmake_minimum_required(VERSION 3.16)\n\nproject(lightning)\n\n# Default to C++17\nif (NOT CMAKE_CXX_STANDARD)\n set(CMAKE_C"
},
{
"path": "README.md",
"chars": 11753,
"preview": "# Lightning-LM\n\nEnglish | [中文](./README_CN.md)\n\nLightning-Speed Lidar Localization and Mapping\n\nLightning-LM is a comple"
},
{
"path": "README_CN.md",
"chars": 6020,
"preview": "# Lightning-LM\n\n[English](./README.md) | 中文\n\nLightning-Speed Lidar Localization and Mapping\n\nLightning-LM 是一个完整的激光建图+定位模"
},
{
"path": "cmake/packages.cmake",
"chars": 1925,
"preview": "find_package(glog REQUIRED)\nfind_package(Eigen3 REQUIRED)\nfind_package(PCL REQUIRED)\nfind_package(yaml-cpp REQUIRED)\nfin"
},
{
"path": "config/default.yaml",
"chars": 3483,
"preview": "# 针对NCLT的配置\ncommon:\n dataset: \"nclt\"\n lidar_topic: \"points_raw\"\n livox_lidar_topic: \"/livox/lidar\"\n imu_topic: \"/liv"
},
{
"path": "config/default_livox.yaml",
"chars": 3483,
"preview": "# 针对NCLT的配置\ncommon:\n dataset: \"nclt\"\n lidar_topic: \"points_raw\"\n livox_lidar_topic: \"/livox/lidar\"\n imu_topic: \"/liv"
},
{
"path": "config/default_nclt.yaml",
"chars": 3456,
"preview": "# 针对NCLT的配置\ncommon:\n dataset: \"nclt\"\n lidar_topic: \"points_raw\"\n livox_lidar_topic: \"/livox/lidar\"\n imu_topic: \"imu_"
},
{
"path": "config/default_robosense.yaml",
"chars": 3444,
"preview": "# 针对NCLT的配置\ncommon:\n dataset: \"robosense\"\n lidar_topic: \"/LIDAR/POINTS\"\n livox_lidar_topic: \"/livox/lidar\"\n imu_topi"
},
{
"path": "config/default_utbm.yaml",
"chars": 3587,
"preview": "# 针对utbm的配置\ncommon:\n dataset: \"utbm\"\n lidar_topic: \"/velodyne_points\"\n livox_lidar_topic: \"/livox/lidar\"\n imu_topic:"
},
{
"path": "config/default_vbr.yaml",
"chars": 3461,
"preview": "# 针对NCLT的配置\ncommon:\n dataset: \"vbr\"\n lidar_topic: \"/ouster/points\"\n livox_lidar_topic: \"/livox/lidar\"\n imu_topic: \"/"
},
{
"path": "doc/g2p5_vbr.pgm",
"chars": 9590,
"preview": "P5\n2272 2528\n255\n\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000\u0000"
},
{
"path": "docker/Dockerfile",
"chars": 3048,
"preview": "FROM ros:humble-perception\n\nLABEL maintainer=\"pengfei.guo2001@gmail.com\"\nLABEL description=\"A docker for lightning-lm\"\n\n"
},
{
"path": "docker/README.md",
"chars": 954,
"preview": "# lightning-lm Docker\n\n这是为 lightning-lm 项目准备的 Docker 指南，分为构建和运行两部分\n\n## 构建\n\n可以在本地构建或者在云端构建然后拉取，如果需要在本地构建，则在安装好docker环境之后执"
},
{
"path": "package.xml",
"chars": 1277,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "pcd/.gitkeep",
"chars": 0,
"preview": ""
},
{
"path": "scripts/install_dep.sh",
"chars": 127,
"preview": "sudo apt install libopencv-dev libpcl-dev pcl-tools libyaml-cpp-dev libgoogle-glog-dev libgflags-dev ros-humble-pcl-conv"
},
{
"path": "scripts/merge_bags.py",
"chars": 4696,
"preview": "import os.path\n\nimport sys\nimport rclpy\nfrom orca.orca_platform import datadir\nfrom rosbag2_py import SequentialReader, "
},
{
"path": "scripts/save_default_map.sh",
"chars": 79,
"preview": "ros2 service call /lightning/save_map lightning/srv/SaveMap \"{map_id: new_map}\""
},
{
"path": "src/CMakeLists.txt",
"chars": 2033,
"preview": "add_library(${PROJECT_NAME}.libs SHARED\n common/nav_state.cc\n common/constant.h\n common/options.cc\n"
},
{
"path": "src/app/CMakeLists.txt",
"chars": 1228,
"preview": "add_executable(run_frontend_offline\n run_frontend_offline.cc\n)\n\ntarget_link_libraries(run_frontend_offline\n "
},
{
"path": "src/app/run_frontend_offline.cc",
"chars": 2710,
"preview": "//\n// Created by xiang on 25-3-18.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/g2p5/g2p5.h\""
},
{
"path": "src/app/run_loc_offline.cc",
"chars": 2159,
"preview": "//\n// Created by xiang on 25-3-18.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/localization"
},
{
"path": "src/app/run_loc_online.cc",
"chars": 773,
"preview": "//\n// Created by xiang on 25-3-18.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/system/loc_s"
},
{
"path": "src/app/run_loop_offline.cc",
"chars": 2000,
"preview": "//\n// Created by xiang on 25-3-18.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/lio/laser_ma"
},
{
"path": "src/app/run_slam_offline.cc",
"chars": 2129,
"preview": "//\n// Created by xiang on 25-3-18.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/system/slam."
},
{
"path": "src/app/run_slam_online.cc",
"chars": 920,
"preview": "//\n// Created by xiang on 25-3-18.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/system/slam."
},
{
"path": "src/app/test_ui.cc",
"chars": 566,
"preview": "//\n// Created by xiang on 25-8-27.\n//\n\n#include \"ui/pangolin_window.h\"\n\n#include <pangolin/display/display.h>\n#include <"
},
{
"path": "src/common/constant.h",
"chars": 581,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_CONSTANT_H\n#define LIGHTNING_CONSTANT_H\n\n#include \"common/eigen"
},
{
"path": "src/common/eigen_types.h",
"chars": 2658,
"preview": "//\n// Created by gaoxiang on 2020/8/10.\n//\n\n#pragma once\n\n#ifndef LIGHTNING_EIGEN_TYPES_H\n#define LIGHTNING_EIGEN_TYPES_"
},
{
"path": "src/common/functional_points.h",
"chars": 330,
"preview": "//\n// Created by xiang on 23-10-12.\n//\n#pragma once\n\n#include \"common/eigen_types.h\"\n\nnamespace lightning {\n\n/// 功能点定义\ns"
},
{
"path": "src/common/imu.h",
"chars": 418,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_IMU_H\n#define LIGHTNING_IMU_H\n\n#include \"common/eigen_types.h\"\n"
},
{
"path": "src/common/keyframe.h",
"chars": 1596,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_KEYFRAME_H\n#define LIGHTNING_KEYFRAME_H\n\n#include \"common/eigen"
},
{
"path": "src/common/loop_candidate.h",
"chars": 467,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_LOOP_CANDIDATE_H\n#define LIGHTNING_LOOP_CANDIDATE_H\n\n#include \""
},
{
"path": "src/common/measure_group.h",
"chars": 763,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_MEASURE_GROUP_H\n#define LIGHTNING_MEASURE_GROUP_H\n\n#include <de"
},
{
"path": "src/common/nav_state.cc",
"chars": 568,
"preview": "//\n// Created by xiang on 2022/6/21.\n//\n\n#include \"common/nav_state.h\"\n\nnamespace lightning {\n/// 矢量变量的维度\nconst std::vec"
},
{
"path": "src/common/nav_state.h",
"chars": 5760,
"preview": "//\n// Created by xiang on 2022/2/15.\n//\n\n#pragma once\n\n#include \"common/eigen_types.h\"\n\n#include <glog/logging.h>\n#inclu"
},
{
"path": "src/common/odom.h",
"chars": 396,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_ODOM_H\n#define LIGHTNING_ODOM_H\n\n#include \"common/eigen_types.h"
},
{
"path": "src/common/options.cc",
"chars": 2224,
"preview": "//\n// Created by xiang on 24-4-8.\n//\n\n#include <common/options.h>\n\nnamespace lightning {\n\nnamespace debug {\n\n/// debug a"
},
{
"path": "src/common/options.h",
"chars": 3247,
"preview": "//\n// Created by xiang on 24-4-8.\n//\n\n#pragma once\n#ifndef LIGHTNING_MODULE_OPTIONS_H\n#define LIGHTNING_MODULE_OPTIONS_H"
},
{
"path": "src/common/params.cc",
"chars": 3900,
"preview": "//\n// Created by xiang on 25-3-26.\n//\n\n#include \"common/params.h\"\n#include \"core/lightning_math.hpp\"\n#include \"io/yaml_i"
},
{
"path": "src/common/params.h",
"chars": 2773,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_PARAMS_H\n#define LIGHTNING_PARAMS_H\n\n#include <string>\n#include"
},
{
"path": "src/common/point_def.h",
"chars": 3664,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#pragma once\n#ifndef LIGHTNING_POINT_DEF_H\n#define LIGHTNING_POINT_DEF_H\n\n#includ"
},
{
"path": "src/common/pose_rpy.h",
"chars": 505,
"preview": "//\n// Created by xiang on 25-7-7.\n//\n\n#ifndef LIGHTNING_POSE_RPY_H\n#define LIGHTNING_POSE_RPY_H\n\nnamespace lightning{\n\n/"
},
{
"path": "src/common/s2.hpp",
"chars": 6023,
"preview": "//\n// Created by xiang on 2022/2/15.\n//\n#pragma once\n\n#include \"core/lightning_math.hpp\"\n\nnamespace lightning {\n\n/**\n * "
},
{
"path": "src/common/std_types.h",
"chars": 237,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_STD_TYPES_H\n#define LIGHTNING_STD_TYPES_H\n\n#include <mutex>\n#in"
},
{
"path": "src/common/timed_pose.h",
"chars": 499,
"preview": "//\n// Created by xiang on 25-7-7.\n//\n\n#ifndef LIGHTNING_TIMED_POSE_H\n#define LIGHTNING_TIMED_POSE_H\n\n#include \"common/ei"
},
{
"path": "src/core/g2p5/g2p5.cc",
"chars": 14308,
"preview": "//\n// Created by xiang on 25-6-23.\n//\n\n#include \"core/g2p5/g2p5.h\"\n#include \"common/constant.h\"\n\n#include <pcl/ModelCoef"
},
{
"path": "src/core/g2p5/g2p5.h",
"chars": 4066,
"preview": "//\n// Created by xiang on 25-6-23.\n//\n\n#ifndef LIGHTNING_G2P5_H\n#define LIGHTNING_G2P5_H\n\n#include \"common/eigen_types.h"
},
{
"path": "src/core/g2p5/g2p5_grid_data.h",
"chars": 543,
"preview": "//\n// Created by xiang on 25-6-24.\n//\n\n#ifndef LIGHTNING_G2P5_GRID_DATA_H\n#define LIGHTNING_G2P5_GRID_DATA_H\n\nnamespace "
},
{
"path": "src/core/g2p5/g2p5_map.cc",
"chars": 12587,
"preview": "//\n// Created by xiang on 25-6-23.\n//\n\n#include \"core/g2p5/g2p5_map.h\"\n\n#include <malloc.h>\n#include <cstdlib>\n#include "
},
{
"path": "src/core/g2p5/g2p5_map.h",
"chars": 3171,
"preview": "//\n// Created by xiang on 25-6-23.\n//\n\n#ifndef LIGHTNING_G2P5_MAP_H\n#define LIGHTNING_G2P5_MAP_H\n\n#include \"common/eigen"
},
{
"path": "src/core/g2p5/g2p5_subgrid.cc",
"chars": 1252,
"preview": "//\n// Created by xiang on 25-6-24.\n//\n\n#include \"core/g2p5/g2p5_subgrid.h\"\n\n#include <cstring>\n#include <memory>\n\nnamesp"
},
{
"path": "src/core/g2p5/g2p5_subgrid.h",
"chars": 2331,
"preview": "//\n// Created by xiang on 25-6-24.\n//\n\n#ifndef LIGHTNING_G2P5_SUBGRID_H\n#define LIGHTNING_G2P5_SUBGRID_H\n\n#include \"comm"
},
{
"path": "src/core/ivox3d/hilbert.hpp",
"chars": 18077,
"preview": "// 2021-09-18, https://github.com/spectral3d/hilbert_hpp is under MIT license.\n\n// Copyright (c) 2019 David Beynon\n//\n//"
},
{
"path": "src/core/ivox3d/ivox3d.h",
"chars": 11272,
"preview": "//\n// Created by xiang on 2021/9/16.\n//\n#pragma once\n\n#include <glog/logging.h>\n#include <execution>\n#include <list>\n#in"
},
{
"path": "src/core/ivox3d/ivox3d_node.hpp",
"chars": 12638,
"preview": "#include <pcl/common/centroid.h>\n#include <algorithm>\n#include <cmath>\n#include <list>\n#include <vector>\n\n#include \"core"
},
{
"path": "src/core/lightning_math.hpp",
"chars": 24791,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_LIGHTNING_MATH_HPP\n#define LIGHTNING_LIGHTNING_MATH_HPP\n\n#pragm"
},
{
"path": "src/core/lio/anderson_acceleration.h",
"chars": 4005,
"preview": "#ifndef ANDERSONACCELERATION_H_\n#define ANDERSONACCELERATION_H_\n\n#include <omp.h>\n#include <algorithm>\n#include <cassert"
},
{
"path": "src/core/lio/eskf.cc",
"chars": 12188,
"preview": "//\n// Created by xiang on 2022/2/15.\n//\n\n#include \"core/lio/eskf.hpp\"\n#include \"core/lightning_math.hpp\"\n\n#include <Eige"
},
{
"path": "src/core/lio/eskf.hpp",
"chars": 4900,
"preview": "//\n// Created by xiang on 2022/2/15.\n//\n\n#ifndef FUSION_ESKF_HPP\n#define FUSION_ESKF_HPP\n\n#include \"common/eigen_types.h"
},
{
"path": "src/core/lio/imu_filter.h",
"chars": 7024,
"preview": "//\n// Created by user on 2026/3/18.\n//\n\n#ifndef LIGHTNING_IMU_FILTER_H\n#define LIGHTNING_IMU_FILTER_H\n\n#include \"common/"
},
{
"path": "src/core/lio/imu_processing.hpp",
"chars": 12065,
"preview": "#pragma once\n\n#ifndef FASTER_LIO_IMU_PROCESSING_H\n#define FASTER_LIO_IMU_PROCESSING_H\n\n#include <glog/logging.h>\n#includ"
},
{
"path": "src/core/lio/laser_mapping.cc",
"chars": 30521,
"preview": "#include <pcl/common/transforms.h>\n#include <yaml-cpp/yaml.h>\n#include <fstream>\n\n#include \"common/options.h\"\n#include \""
},
{
"path": "src/core/lio/laser_mapping.h",
"chars": 6739,
"preview": "#ifndef FASTER_LIO_LASER_MAPPING_H\n#define FASTER_LIO_LASER_MAPPING_H\n\n#include <pcl/filters/voxel_grid.h>\n#include <con"
},
{
"path": "src/core/lio/pointcloud_preprocess.cc",
"chars": 8424,
"preview": "#include \"pointcloud_preprocess.h\"\n#include <execution>\n\n#include <glog/logging.h>\n\nnamespace lightning {\n\nvoid PointClo"
},
{
"path": "src/core/lio/pointcloud_preprocess.h",
"chars": 2018,
"preview": "#ifndef FASTER_LIO_POINTCLOUD_PROCESSING_H\n#define FASTER_LIO_POINTCLOUD_PROCESSING_H\n\n#include <pcl_conversions/pcl_con"
},
{
"path": "src/core/lio/pose6d.h",
"chars": 669,
"preview": "//\n// Created by xiang on 25-4-14.\n//\n\n#ifndef FASTER_LIO_POSE6D_H\n#define FASTER_LIO_POSE6D_H\n\n#include \"common/eigen_t"
},
{
"path": "src/core/localization/lidar_loc/lidar_loc.cc",
"chars": 35277,
"preview": "#include <algorithm>\n#include <execution>\n\n#include <pcl/common/transforms.h>\n#include <pcl/filters/passthrough.h>\n#incl"
},
{
"path": "src/core/localization/lidar_loc/lidar_loc.h",
"chars": 7404,
"preview": "#pragma once\n\n#include <pcl/registration/icp.h>\n#include <chrono>\n#include <deque>\n#include <iostream>\n#include <sensor_"
},
{
"path": "src/core/localization/lidar_loc/pclomp/ndt_omp.cpp",
"chars": 218,
"preview": "#include \"ndt_omp.h\"\n#include \"ndt_omp_impl.hpp\"\n\ntemplate class pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl"
},
{
"path": "src/core/localization/lidar_loc/pclomp/ndt_omp.h",
"chars": 25378,
"preview": "/*\n * Software License Agreement (BSD License)\n *\n * Point Cloud Library (PCL) - www.pointclouds.org\n * Copyright (c) "
},
{
"path": "src/core/localization/lidar_loc/pclomp/ndt_omp_impl.hpp",
"chars": 42554,
"preview": "#include \"ndt_omp.h\"\n/*\n * Software License Agreement (BSD License)\n *\n * Point Cloud Library (PCL) - www.pointclouds.o"
},
{
"path": "src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.cpp",
"chars": 205,
"preview": "#include \"voxel_grid_covariance_omp.h\"\n#include \"voxel_grid_covariance_omp_impl.hpp\"\n\ntemplate class pclomp::VoxelGridCo"
},
{
"path": "src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp.h",
"chars": 9358,
"preview": "/*\n * Software License Agreement (BSD License)\n *\n * Point Cloud Library (PCL) - www.pointclouds.org\n * Copyright (c) "
},
{
"path": "src/core/localization/lidar_loc/pclomp/voxel_grid_covariance_omp_impl.hpp",
"chars": 8332,
"preview": "/*\n * Software License Agreement (BSD License)\n *\n * Point Cloud Library (PCL) - www.pointclouds.org\n * Copyright (c) "
},
{
"path": "src/core/localization/localization.cpp",
"chars": 11037,
"preview": "#include <pcl/common/transforms.h>\n#include <pcl_conversions/pcl_conversions.h>\n\n#include \"core/localization/lidar_loc/l"
},
{
"path": "src/core/localization/localization.h",
"chars": 4121,
"preview": "#pragma once\n\n#include \"geometry_msgs/msg/transform_stamped.hpp\"\n#include \"std_msgs/msg/int32.hpp\"\n\n#include \"common/imu"
},
{
"path": "src/core/localization/localization_result.cc",
"chars": 1192,
"preview": "//\n// Created by xiang on 24-4-11.\n//\n\n#include \"localization_result.h\"\n#include \"core/lightning_math.hpp\"\n\nnamespace li"
},
{
"path": "src/core/localization/localization_result.h",
"chars": 2197,
"preview": "//\n// Created by xiang on 2021/11/17.\n//\n#pragma once\n\n#include <geometry_msgs/msg/transform_stamped.hpp>\n#include \"comm"
},
{
"path": "src/core/localization/pose_graph/pgo.cc",
"chars": 19030,
"preview": "#include \"pgo.h\"\n#include \"pgo_impl.h\"\n\n#include <boost/format.hpp>\n\n#include <glog/logging.h>\n\n#include \"common/options"
},
{
"path": "src/core/localization/pose_graph/pgo.h",
"chars": 2846,
"preview": "#pragma once\n\n#include \"common/eigen_types.h\"\n#include \"common/nav_state.h\"\n#include \"core/localization/localization_res"
},
{
"path": "src/core/localization/pose_graph/pgo_impl.cc",
"chars": 32910,
"preview": "#include \"pgo_impl.h\"\n#include \"core/opti_algo/algo_select.h\"\n#include \"core/robust_kernel/robust_kernel_all.h\"\n#include"
},
{
"path": "src/core/localization/pose_graph/pgo_impl.h",
"chars": 10066,
"preview": "#pragma once\n\n#include <boost/format.hpp>\n#include <deque>\n#include <functional>\n#include <map>\n#include <mutex>\n#includ"
},
{
"path": "src/core/localization/pose_graph/pose_extrapolator.cc",
"chars": 4300,
"preview": "#include \"core/localization/pose_graph/pose_extrapolator.h\"\n#include \"core/lightning_math.hpp\"\n\n#include <memory>\n\nnames"
},
{
"path": "src/core/localization/pose_graph/pose_extrapolator.h",
"chars": 2127,
"preview": "#ifndef TEAPROT_POSE_EXTRAPOLATOR_H_\n#define TEAPROT_POSE_EXTRAPOLATOR_H_\n\n#include <deque>\n\n#include \"common/eigen_type"
},
{
"path": "src/core/localization/pose_graph/smoother.h",
"chars": 3774,
"preview": "//\n// Created by xiang on 23-2-24.\n//\n\n#pragma once\n\n#include <deque>\n\n#include \"common/eigen_types.h\"\n#include \"common/"
},
{
"path": "src/core/loop_closing/loop_closing.cc",
"chars": 10524,
"preview": "//\n// Created by xiang on 25-4-21.\n//\n\n#include \"core/loop_closing/loop_closing.h\"\n#include \"common/keyframe.h\"\n#include"
},
{
"path": "src/core/loop_closing/loop_closing.h",
"chars": 2432,
"preview": "//\n// Created by xiang on 25-4-21.\n//\n\n#ifndef LIGHTNING_LOOP_CLOSING_H\n#define LIGHTNING_LOOP_CLOSING_H\n\n#include \"comm"
},
{
"path": "src/core/maps/tiled_map.cc",
"chars": 19463,
"preview": "//\n// Created by xiang on 23-2-7.\n//\n\n#include \"core/maps/tiled_map.h\"\n#include \"io/file_io.h\"\n\n#include <pcl/io/pcd_io."
},
{
"path": "src/core/maps/tiled_map.h",
"chars": 8810,
"preview": "//\n// Created by xiang on 23-2-7.\n//\n\n#ifndef LIGHNING_TILED_MAP_H\n#define LIGHNING_TILED_MAP_H\n\n#include <opencv2/core/"
},
{
"path": "src/core/maps/tiled_map_chunk.cc",
"chars": 588,
"preview": "//\n// Created by xiang on 23-2-7.\n//\n\n#include \"tiled_map_chunk.h\"\n\n#include <pcl/io/pcd_io.h>\n\nnamespace lightning {\n\nv"
},
{
"path": "src/core/maps/tiled_map_chunk.h",
"chars": 1029,
"preview": "//\n// Created by xiang on 23-2-7.\n//\n\n#ifndef LIGHTNING_TILED_MAP_CHUNK_H\n#define LIGHTNING_TILED_MAP_CHUNK_H\n\n#include "
},
{
"path": "src/core/miao/CMakeLists.txt",
"chars": 76,
"preview": "add_subdirectory(core)\n# add_subdirectory(examples)\nadd_subdirectory(utils)\n"
},
{
"path": "src/core/miao/core/CMakeLists.txt",
"chars": 498,
"preview": "add_library(miao.core SHARED\n graph/graph.cc\n graph/optimizer.cc\n\n common/string_tools.cc\n\n "
},
{
"path": "src/core/miao/core/common/config.h",
"chars": 1881,
"preview": "//\n// Created by xiang on 24-6-4.\n//\n\n#ifndef MIAO_CONFIG_H\n#define MIAO_CONFIG_H\n\nnamespace lightning::miao {\n\n/// 优化器使"
},
{
"path": "src/core/miao/core/common/macros.h",
"chars": 337,
"preview": "//\n// Created by xiang on 24-3-19.\n//\n\n#ifndef MIAO_MACROS_H\n#define MIAO_MACROS_H\n\nnamespace lightning::miao {\n\n/// 禁止拷"
},
{
"path": "src/core/miao/core/common/math.h",
"chars": 11016,
"preview": "//\n// Created by xiang on 24-6-17.\n//\n\n#ifndef MIAO_MATH_H\n#define MIAO_MATH_H\n\n#include <cmath>\n#include \"common/eigen_"
},
{
"path": "src/core/miao/core/common/string_tools.cc",
"chars": 3413,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#include \"string_tools.h\"\n#include <algorithm>\n#include <vector>\n\n#include <glog/"
},
{
"path": "src/core/miao/core/common/string_tools.h",
"chars": 2367,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_STRING_TOOLS_H\n#define MIAO_STRING_TOOLS_H\n\n#include <sstream>\n#incl"
},
{
"path": "src/core/miao/core/graph/base_binary_edge.h",
"chars": 895,
"preview": "//\n// Created by xiang on 24-4-23.\n//\n\n#ifndef MIAO_BASE_BINARY_EDGE_H\n#define MIAO_BASE_BINARY_EDGE_H\n\n#include \"base_f"
},
{
"path": "src/core/miao/core/graph/base_edge.h",
"chars": 1982,
"preview": "//\n// Created by xiang on 24-4-17.\n//\n\n#ifndef MIAO_BASE_EDGE_H\n#define MIAO_BASE_EDGE_H\n\n#include \"common/eigen_types.h"
},
{
"path": "src/core/miao/core/graph/base_fixed_sized_edge.h",
"chars": 8250,
"preview": "//\n// Created by xiang on 24-4-18.\n//\n\n#ifndef MIAO_BASE_FIXED_SIZED_EDGE_H\n#define MIAO_BASE_FIXED_SIZED_EDGE_H\n\n#inclu"
},
{
"path": "src/core/miao/core/graph/base_fixed_sized_edge.hpp",
"chars": 8684,
"preview": "//\n// Created by xiang on 24-5-8.\n//\n\n#ifndef BASE_FIXED_SIZED_EDGE_HPP\n#define BASE_FIXED_SIZED_EDGE_HPP\n\n#include \"bas"
},
{
"path": "src/core/miao/core/graph/base_unary_edge.h",
"chars": 637,
"preview": "//\n// Created by xiang on 24-4-23.\n//\n\n#ifndef MIAO_BASE_UNARY_EDGE_H\n#define MIAO_BASE_UNARY_EDGE_H\n\n#include \"base_fix"
},
{
"path": "src/core/miao/core/graph/base_vec_vertex.h",
"chars": 658,
"preview": "//\n// Created by xiang on 24-6-4.\n//\n\n#ifndef MIAO_BASE_VEC_VERTEX_H\n#define MIAO_BASE_VEC_VERTEX_H\n\n#include \"core/grap"
},
{
"path": "src/core/miao/core/graph/base_vertex.h",
"chars": 4139,
"preview": "//\n// Created by xiang on 24-3-19.\n//\n\n#ifndef MIAO_BASE_VERTEX_H\n#define MIAO_BASE_VERTEX_H\n\n#include \"common/eigen_typ"
},
{
"path": "src/core/miao/core/graph/edge.h",
"chars": 3581,
"preview": "//\n// Created by xiang on 24-3-19.\n//\n\n#ifndef MIAO_EDGE_H\n#define MIAO_EDGE_H\n\n#include <algorithm>\n#include <cassert>\n"
},
{
"path": "src/core/miao/core/graph/graph.cc",
"chars": 3594,
"preview": "//\n// Created by xiang on 24-3-19.\n//\n\n#include \"graph.h\"\n#include \"core/common/string_tools.h\"\n#include \"core/graph/edg"
},
{
"path": "src/core/miao/core/graph/graph.h",
"chars": 2188,
"preview": "//\n// Created by xiang on 24-3-19.\n//\n\n#ifndef MIAO_GRAPH_H\n#define MIAO_GRAPH_H\n\n#include <algorithm>\n#include <bitset>"
},
{
"path": "src/core/miao/core/graph/optimizer.cc",
"chars": 10078,
"preview": "//\n// Created by xiang on 24-4-23.\n//\n\n#include \"optimizer.h\"\n#include \"vertex.h\"\n\n#include \"core/graph/edge.h\"\n#include"
},
{
"path": "src/core/miao/core/graph/optimizer.h",
"chars": 5500,
"preview": "//\n// Created by xiang on 24-4-23.\n//\n\n#ifndef MIAO_OPTIMIZER_H\n#define MIAO_OPTIMIZER_H\n\n#include \"core/common/config.h"
},
{
"path": "src/core/miao/core/graph/vertex.h",
"chars": 4289,
"preview": "//\n// Created by xiang on 24-3-19.\n//\n\n#ifndef MIAO_VERTEX_H\n#define MIAO_VERTEX_H\n\n#include <Eigen/Core>\n#include <memo"
},
{
"path": "src/core/miao/core/math/marginal_covariance_cholesky.cc",
"chars": 6724,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#include \"marginal_covariance_cholesky.h\"\n\nnamespace lightning::miao {\n\nstruct Ma"
},
{
"path": "src/core/miao/core/math/marginal_covariance_cholesky.h",
"chars": 2838,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#ifndef MIAO_MARGINAL_COVARIANCE_CHOLESKY_H\n#define MIAO_MARGINAL_COVARIANCE_CHOL"
},
{
"path": "src/core/miao/core/math/matrix_operations.h",
"chars": 2125,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_MATRIX_OPERATIONS_H\n#define MIAO_MATRIX_OPERATIONS_H\n\n#include \"comm"
},
{
"path": "src/core/miao/core/math/misc.h",
"chars": 366,
"preview": "//\n// Created by xiang on 24-5-8.\n//\n\n#ifndef MISC_H\n#define MISC_H\n\n#include <cmath>\n\nnamespace lightning::miao {\n\n/**\n"
},
{
"path": "src/core/miao/core/opti_algo/algo_select.h",
"chars": 2721,
"preview": "//\n// Created by xiang on 24-5-14.\n//\n\n#ifndef MIAO_ALGO_SELECT_H\n#define MIAO_ALGO_SELECT_H\n\n#include \"optimization_alg"
},
{
"path": "src/core/miao/core/opti_algo/opti_algo_dogleg.cc",
"chars": 5877,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#include \"opti_algo_dogleg.h\"\n#include \"core/graph/optimizer.h\"\n#include \"core/gr"
},
{
"path": "src/core/miao/core/opti_algo/opti_algo_dogleg.h",
"chars": 1458,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_OPTI_ALGO_DOGLEG_H\n#define MIAO_OPTI_ALGO_DOGLEG_H\n\n#include \"optimi"
},
{
"path": "src/core/miao/core/opti_algo/opti_algo_gauss_newton.cc",
"chars": 1152,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n#include \"opti_algo_gauss_newton.h\"\n#include \"core/graph/optimizer.h\"\n#include \"co"
},
{
"path": "src/core/miao/core/opti_algo/opti_algo_gauss_newton.h",
"chars": 761,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#ifndef MIAO_OPTI_ALGO_GAUSS_NEWTON_H\n#define MIAO_OPTI_ALGO_GAUSS_NEWTON_H\n\n#inc"
},
{
"path": "src/core/miao/core/opti_algo/opti_algo_lm.cc",
"chars": 4166,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#include \"opti_algo_lm.h\"\n#include \"core/graph/edge.h\"\n#include \"core/graph/optim"
},
{
"path": "src/core/miao/core/opti_algo/opti_algo_lm.h",
"chars": 1418,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_OPTI_ALGO_LM_H\n#define MIAO_OPTI_ALGO_LM_H\n\n#include \"optimization_a"
},
{
"path": "src/core/miao/core/opti_algo/optimization_algorithm.cc",
"chars": 900,
"preview": "//\n// Created by xiang on 24-4-25.\n//\n\n#include \"optimization_algorithm.h\"\n#include \"core/graph/optimizer.h\"\n#include \"c"
},
{
"path": "src/core/miao/core/opti_algo/optimization_algorithm.h",
"chars": 1386,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_OPTIMIZATION_ALGORITHM_H\n#define MIAO_OPTIMIZATION_ALGORITHM_H\n\n#inc"
},
{
"path": "src/core/miao/core/robust_kernel/cauchy.h",
"chars": 664,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_CAUCHY_H\n#define MIAO_CAUCHY_H\n\n#include \"robust_kernel.h\"\n\nnamespac"
},
{
"path": "src/core/miao/core/robust_kernel/dcs.h",
"chars": 968,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_DCS_H\n#define MIAO_DCS_H\n\n#include \"robust_kernel.h\"\n\nnamespace ligh"
},
{
"path": "src/core/miao/core/robust_kernel/fair.h",
"chars": 965,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_FAIR_H\n#define MIAO_FAIR_H\n\n#include \"robust_kernel.h\"\n\nnamespace li"
},
{
"path": "src/core/miao/core/robust_kernel/huber.h",
"chars": 1135,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_HUBER_H\n#define MIAO_HUBER_H\n\n#include \"robust_kernel.h\"\n\nnamespace "
},
{
"path": "src/core/miao/core/robust_kernel/mc_clure.h",
"chars": 769,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_MC_CLURE_H\n#define MIAO_MC_CLURE_H\n\n#include \"robust_kernel.h\"\n\nname"
},
{
"path": "src/core/miao/core/robust_kernel/pseudo_huber.h",
"chars": 857,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_PSEUDO_HUBER_H\n#define MIAO_PSEUDO_HUBER_H\n\n#include \"robust_kernel."
},
{
"path": "src/core/miao/core/robust_kernel/robust_kernel.h",
"chars": 1439,
"preview": "//\n// Created by xiang on 24-4-25.\n//\n\n#ifndef MIAO_ROBUST_KERNEL_H\n#define MIAO_ROBUST_KERNEL_H\n\n#include \"common/eigen"
},
{
"path": "src/core/miao/core/robust_kernel/robust_kernel_all.h",
"chars": 354,
"preview": "//\n// Created by xiang on 24-5-22.\n//\n\n#ifndef MIAO_ROBUST_KERNEL_ALL_H\n#define MIAO_ROBUST_KERNEL_ALL_H\n\n#include \"robu"
},
{
"path": "src/core/miao/core/robust_kernel/saturated.h",
"chars": 694,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_SATURATED_H\n#define MIAO_SATURATED_H\n\n#include \"robust_kernel.h\"\n\nna"
},
{
"path": "src/core/miao/core/robust_kernel/tukey.h",
"chars": 954,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_TUKEY_H\n#define MIAO_TUKEY_H\n\n#include \"robust_kernel.h\"\n\nnamespace "
},
{
"path": "src/core/miao/core/robust_kernel/welsch.h",
"chars": 714,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_WELSCH_H\n#define MIAO_WELSCH_H\n\n#include \"robust_kernel.h\"\n\nnamespac"
},
{
"path": "src/core/miao/core/solver/block_solver.h",
"chars": 5588,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#ifndef MIAO_BLOCK_SOLVER_H\n#define MIAO_BLOCK_SOLVER_H\n\n/// 模板化的solver\n/// Block"
},
{
"path": "src/core/miao/core/solver/block_solver.hpp",
"chars": 20672,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#include <glog/logging.h>\n\n#include <execution>\n#include \"core/graph/edge.h\"\n#inc"
},
{
"path": "src/core/miao/core/solver/linear_solver.h",
"chars": 2491,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#ifndef MIAO_LINEAR_SOLVER_H\n#define MIAO_LINEAR_SOLVER_H\n\n#include \"core/sparse/"
},
{
"path": "src/core/miao/core/solver/linear_solver_ccs.h",
"chars": 1147,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#ifndef MIAO_LINEAR_SOLVER_CCS_H\n#define MIAO_LINEAR_SOLVER_CCS_H\n\n#include \"core"
},
{
"path": "src/core/miao/core/solver/linear_solver_dense.h",
"chars": 2529,
"preview": "//\n// Created by xiang on 24-4-28.\n//\n\n#ifndef MIAO_LINEAR_SOLVER_DENSE_H\n#define MIAO_LINEAR_SOLVER_DENSE_H\n\n#include \""
},
{
"path": "src/core/miao/core/solver/linear_solver_eigen.h",
"chars": 5287,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_LINEAR_SOLVER_EIGEN_H\n#define MIAO_LINEAR_SOLVER_EIGEN_H\n\n#include <"
},
{
"path": "src/core/miao/core/solver/linear_solver_pcg.h",
"chars": 1807,
"preview": "//\n// Created by xiang on 24-5-13.\n//\n\n#ifndef MIAO_LINEAR_SOLVER_PCG_H\n#define MIAO_LINEAR_SOLVER_PCG_H\n\n#include \"core"
},
{
"path": "src/core/miao/core/solver/linear_solver_pcg.hpp",
"chars": 7643,
"preview": "#include <cassert>\n\nnamespace lightning::miao {\nnamespace internal {\n\n/**\n * y=Ax\n */\ntemplate <typename MatrixType>\ninl"
},
{
"path": "src/core/miao/core/solver/solver.cc",
"chars": 342,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#include \"solver.h\"\n\n#include <algorithm>\n#include <cstring>\n\nnamespace lightning"
},
{
"path": "src/core/miao/core/solver/solver.h",
"chars": 3148,
"preview": "//\n// Created by xiang on 24-4-25.\n//\n\n#ifndef MIAO_SOLVER_H\n#define MIAO_SOLVER_H\n\n#include \"common/eigen_types.h\"\n#inc"
},
{
"path": "src/core/miao/core/sparse/sparse_block_matrix.h",
"chars": 7019,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_SPARSE_BLOCK_MATRIX_H\n#define MIAO_SPARSE_BLOCK_MATRIX_H\n\n#include \""
},
{
"path": "src/core/miao/core/sparse/sparse_block_matrix.hpp",
"chars": 15361,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_SPARSE_BLCOK_MATRIX_HPP\n#define MIAO_SPARSE_BLCOK_MATRIX_HPP\n\n#inclu"
},
{
"path": "src/core/miao/core/sparse/sparse_block_matrix_ccs.h",
"chars": 6641,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_SPARSE_BLOCK_MATRIX_CCS_H\n#define MIAO_SPARSE_BLOCK_MATRIX_CCS_H\n\n#i"
},
{
"path": "src/core/miao/core/sparse/sparse_block_matrix_diagonal.h",
"chars": 2483,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#ifndef MIAO_SPARSE_BLOCK_MATRIX_DIAGONAL_H\n#define MIAO_SPARSE_BLOCK_MATRIX_DIAG"
},
{
"path": "src/core/miao/core/sparse/sparse_block_matrix_hashmap.h",
"chars": 3185,
"preview": "//\n// Created by xiang on 24-4-24.\n//\n\n#ifndef MIAO_SPARSE_BLOCK_HASHMAP_H\n#define MIAO_SPARSE_BLOCK_HASHMAP_H\n\n#include"
},
{
"path": "src/core/miao/core/types/edge_se2.h",
"chars": 1492,
"preview": "//\n// Created by xiang on 24-6-17.\n//\n\n#ifndef MIAO_EDGE_SE2_H\n#define MIAO_EDGE_SE2_H\n\n#include \"core/graph/base_binary"
},
{
"path": "src/core/miao/core/types/edge_se2_prior.h",
"chars": 861,
"preview": "//\n// Created by xiang on 24-7-1.\n//\n\n#ifndef MIAO_EDGE_SE2_PRIOR_H\n#define MIAO_EDGE_SE2_PRIOR_H\n\n#include \"core/graph/"
},
{
"path": "src/core/miao/core/types/edge_se3.h",
"chars": 1121,
"preview": "//\n// Created by xiang on 24-6-4.\n//\n\n#ifndef MIAO_EDGE_SE3_H\n#define MIAO_EDGE_SE3_H\n\n#include \"core/common/math.h\"\n#in"
},
{
"path": "src/core/miao/core/types/edge_se3_height_prior.h",
"chars": 752,
"preview": "//\n// Created by xiang on 25-11-13.\n//\n\n#ifndef LIGHTNING_EDGE_SE3_HEIGHT_PRIOR_H\n#define LIGHTNING_EDGE_SE3_HEIGHT_PRIO"
},
{
"path": "src/core/miao/core/types/edge_se3_prior.h",
"chars": 859,
"preview": "//\n// Created by xiang on 25-7-14.\n//\n\n#ifndef MIAO_EDGE_SE3_PRIOR_H\n#define MIAO_EDGE_SE3_PRIOR_H\n\n#include \"core/commo"
},
{
"path": "src/core/miao/core/types/vertex_pointxyz.h",
"chars": 290,
"preview": "//\n// Created by xiang on 24-5-22.\n//\n\n#ifndef MIAO_VERTEX_POINTXYZ_H\n#define MIAO_VERTEX_POINTXYZ_H\n\n#include \"core/gra"
},
{
"path": "src/core/miao/core/types/vertex_se2.h",
"chars": 553,
"preview": "//\n// Created by xiang on 24-6-17.\n//\n\n#ifndef MIAO_VERTEX_SE2_H\n#define MIAO_VERTEX_SE2_H\n\n#include \"core/graph/base_ve"
},
{
"path": "src/core/miao/core/types/vertex_se3.h",
"chars": 700,
"preview": "//\n// Created by xiang on 24-5-20.\n//\n\n#ifndef MIAO_VERTEX_SE3_H\n#define MIAO_VERTEX_SE3_H\n\n#include \"core/graph/base_ve"
},
{
"path": "src/core/miao/examples/CMakeLists.txt",
"chars": 181,
"preview": "add_subdirectory(data_fitting)\nadd_subdirectory(icp)\nadd_subdirectory(sba)\nadd_subdirectory(bal)\nadd_subdirectory(sphere"
},
{
"path": "src/core/miao/examples/bal/CMakeLists.txt",
"chars": 158,
"preview": "add_executable(bal_example bal_example.cc)\ntarget_link_libraries(bal_example\n ${PROJECT_NAME}.core\n ${PROJ"
},
{
"path": "src/core/miao/examples/bal/bal_example.cc",
"chars": 10238,
"preview": "//\n// Created by xiang on 24-6-3.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n#include <random>\n\n#include \""
},
{
"path": "src/core/miao/examples/data_fitting/CMakeLists.txt",
"chars": 486,
"preview": "add_executable(circle_fit circle_fit.cc)\nset_target_properties(circle_fit PROPERTIES OUTPUT_NAME circle_fit)\n\ntarget_lin"
},
{
"path": "src/core/miao/examples/data_fitting/circle_fit.cc",
"chars": 5257,
"preview": "//\n// Created by xiang on 24-4-26.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n\n#include \"core/graph/base_u"
},
{
"path": "src/core/miao/examples/data_fitting/curve_fit.cc",
"chars": 3737,
"preview": "//\n// Created by xiang on 24-5-14.\n//\n\n#include <Eigen/Core>\n#include <iostream>\n\n#include \"core/graph/base_unary_edge.h"
},
{
"path": "src/core/miao/examples/icp/CMakeLists.txt",
"chars": 151,
"preview": "add_executable(gicp_demo gicp_demo.cc)\ntarget_link_libraries(gicp_demo\n ${PROJECT_NAME}.core\n ${PROJECT_NA"
},
{
"path": "src/core/miao/examples/icp/gicp_demo.cc",
"chars": 7401,
"preview": "//\n// Created by xiang on 24-5-20.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n#include <cstdint>\n#include "
},
{
"path": "src/core/miao/examples/incremental/CMakeLists.txt",
"chars": 158,
"preview": "add_executable(incremental incremental.cc)\ntarget_link_libraries(incremental\n ${PROJECT_NAME}.core\n ${PROJ"
},
{
"path": "src/core/miao/examples/incremental/incremental.cc",
"chars": 6466,
"preview": "//\n// Created by xiang on 24-6-4.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n#include <cstdint>\n#include <"
},
{
"path": "src/core/miao/examples/pose_graph/CMakeLists.txt",
"chars": 155,
"preview": "add_executable(pose_graph pose_graph.cc)\ntarget_link_libraries(pose_graph\n ${PROJECT_NAME}.core\n ${PROJECT"
},
{
"path": "src/core/miao/examples/pose_graph/pose_graph.cc",
"chars": 7803,
"preview": "//\n// Created by xiang on 24-6-14.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n#include <fstream>\n\n#include"
},
{
"path": "src/core/miao/examples/sba/CMakeLists.txt",
"chars": 235,
"preview": "add_executable(sba_demo sba_demo.cc)\nset_target_properties(sba_demo PROPERTIES OUTPUT_NAME sba_demo)\n\ntarget_link_librar"
},
{
"path": "src/core/miao/examples/sba/sba_demo.cc",
"chars": 10224,
"preview": "//\n// Created by xiang on 24-5-21.\n//\n\n#include <unordered_map>\n#include <unordered_set>\n\n#include \"core/graph/base_bina"
},
{
"path": "src/core/miao/examples/sphere/CMakeLists.txt",
"chars": 143,
"preview": "add_executable(sphere sphere.cc)\ntarget_link_libraries(sphere\n ${PROJECT_NAME}.core\n ${PROJECT_NAME}.utils"
},
{
"path": "src/core/miao/examples/sphere/sphere.cc",
"chars": 5518,
"preview": "//\n// Created by xiang on 24-6-4.\n//\n\n#include <gflags/gflags.h>\n#include <glog/logging.h>\n#include <cstdint>\n#include <"
},
{
"path": "src/core/miao/utils/CMakeLists.txt",
"chars": 50,
"preview": "add_library(miao.utils SHARED\n sampler.cc\n)"
},
{
"path": "src/core/miao/utils/sampler.cc",
"chars": 677,
"preview": "//\n// Created by xiang on 24-5-8.\n//\n\n#include \"sampler.h\"\n\nnamespace lightning::miao {\nstatic std::normal_distribution<"
},
{
"path": "src/core/miao/utils/sampler.h",
"chars": 2631,
"preview": "//\n// Created by xiang on 24-5-8.\n//\n\n#ifndef SAMPLER_H\n#define SAMPLER_H\n\n#include <ctime>\n#include <memory>\n\n#include "
},
{
"path": "src/core/miao/utils/tuple_tools.h",
"chars": 548,
"preview": "//\n// Created by xiang on 24-5-11.\n//\n\n#ifndef MIAO_TUPLE_TOOLS_H\n#define MIAO_TUPLE_TOOLS_H\n\n#include <tuple>\n\nnamespac"
},
{
"path": "src/core/system/async_message_process.h",
"chars": 3373,
"preview": "//\n// Created by xiang on 2022/2/9.\n//\n\n#ifndef ASYNC_MESSAGE_PROCESS_H\n#define ASYNC_MESSAGE_PROCESS_H\n\n#include <condi"
},
{
"path": "src/core/system/loc_system.cc",
"chars": 3359,
"preview": "//\n// Created by xiang on 25-9-12.\n//\n\n#include \"core/system/loc_system.h\"\n#include \"core/localization/localization.h\"\n#"
},
{
"path": "src/core/system/loc_system.h",
"chars": 1789,
"preview": "//\n// Created by xiang on 25-9-8.\n//\n\n#ifndef LIGHTNING_LOC_SYSTEM_H\n#define LIGHTNING_LOC_SYSTEM_H\n\n#include <tf2_ros/t"
},
{
"path": "src/core/system/slam.cc",
"chars": 9766,
"preview": "//\n// Created by xiang on 25-5-6.\n//\n\n#include \"core/system/slam.h\"\n#include \"core/g2p5/g2p5.h\"\n#include \"core/lio/laser"
},
{
"path": "src/core/system/slam.h",
"chars": 2734,
"preview": "//\n// Created by xiang on 25-5-6.\n//\n\n#ifndef LIGHTNING_SLAM_H\n#define LIGHTNING_SLAM_H\n\n#include <rclcpp/rclcpp.hpp>\n#i"
},
{
"path": "src/io/dataset_type.h",
"chars": 1660,
"preview": "//\n// Created by xiang on 25-3-24.\n//\n\n#ifndef LIGHTNING_DATASET_TYPE_H\n#define LIGHTNING_DATASET_TYPE_H\n\n#include <stri"
},
{
"path": "src/io/file_io.cc",
"chars": 568,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#include \"io/file_io.h\"\n#include <filesystem>\n\nnamespace lightning {\nbool PathExi"
},
{
"path": "src/io/file_io.h",
"chars": 521,
"preview": "//\n// Created by xiang on 25-3-12.\n//\n\n#ifndef LIGHTNING_FILE_IO_H\n#define LIGHTNING_FILE_IO_H\n\n#include \"common/eigen_t"
},
{
"path": "src/io/yaml_io.cc",
"chars": 619,
"preview": "//\n// Created by gaoxiang on 2020/5/13.\n//\n\n#include \"io/yaml_io.h\"\n\n#include <glog/logging.h>\n#include <fstream>\n\nnames"
},
{
"path": "src/io/yaml_io.h",
"chars": 1580,
"preview": "//\n// Created by gaoxiang on 2020/5/13.\n//\n\n#ifndef LIGHTNING_YAML_IO_H\n#define LIGHTNING_YAML_IO_H\n\n#include <yaml-cpp/"
},
{
"path": "src/ui/pangolin_window.cc",
"chars": 3273,
"preview": "#include \"ui/pangolin_window_impl.h\"\n\nnamespace lightning::ui {\n\nPangolinWindow::PangolinWindow() { impl_ = std::make_sh"
},
{
"path": "src/ui/pangolin_window.h",
"chars": 1536,
"preview": "#pragma once\n\n#include <map>\n#include <memory>\n#include <queue>\n\n#include \"common/eigen_types.h\"\n#include \"common/keyfra"
},
{
"path": "src/ui/pangolin_window_impl.cc",
"chars": 15938,
"preview": "#include <pangolin/display/default_font.h>\n#include <iomanip>\n#include <sstream>\n#include <string>\n#include <thread>\n\n#i"
},
{
"path": "src/ui/pangolin_window_impl.h",
"chars": 5303,
"preview": "#pragma once\n\n#include <pangolin/pangolin.h>\n\n#include <pcl/filters/voxel_grid.h>\n#include <pcl/point_cloud.h>\n#include "
},
{
"path": "src/ui/ui_car.cc",
"chars": 1068,
"preview": "#include \"ui/ui_car.h\"\n#include <GL/gl.h>\n\nnamespace lightning::ui {\n\nstd::vector<Vec3f> UiCar::car_vertices_ = {\n //"
},
{
"path": "src/ui/ui_car.h",
"chars": 407,
"preview": "#pragma once\n\n#include \"common/eigen_types.h\"\n\nnamespace lightning::ui {\n\n/// 在UI里显示的小车\nclass UiCar {\n public:\n UiC"
},
{
"path": "src/ui/ui_cloud.cc",
"chars": 3439,
"preview": "#include \"ui/ui_cloud.h\"\n#include \"common/options.h\"\n\n#include <GL/gl.h>\n#include <numeric>\n\nnamespace lightning::ui {\n\n"
},
{
"path": "src/ui/ui_cloud.h",
"chars": 1711,
"preview": "#pragma once\n\n#include \"common/eigen_types.h\"\n#include \"common/point_def.h\"\n\nnamespace lightning::ui {\n\n/// 在UI中使用的点云\n//"
},
{
"path": "src/ui/ui_trajectory.cc",
"chars": 579,
"preview": "#include \"ui/ui_trajectory.h\"\n\n#include <GL/gl.h>\n\nnamespace lightning::ui {\n\nvoid UiTrajectory::AddPt(const SE3& pose) "
}
]
// ... and 35 more files (download for full content)
About this extraction
This page contains the full source code of the gaoxiang12/lightning-lm GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 235 files (1.1 MB), approximately 350.4k tokens, and a symbol index with 962 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.