Showing preview only (822K chars total). Download the full file or copy to clipboard to get everything.
Repository: PJLab-ADG/Livox-Mapping
Branch: main
Commit: 38bc18f15ad8
Files: 73
Total size: 786.6 KB
Directory structure:
gitextract_r29jj8ts/
├── .gitignore
├── LICENSE
├── LIO-Livox/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── README.md
│ ├── cmake/
│ │ ├── FindEigen3.cmake
│ │ └── FindSuiteSparse.cmake
│ ├── config/
│ │ └── horizon_config.yaml
│ ├── include/
│ │ ├── Estimator/
│ │ │ └── Estimator.h
│ │ ├── IMUIntegrator/
│ │ │ └── IMUIntegrator.h
│ │ ├── LidarFeatureExtractor/
│ │ │ └── LidarFeatureExtractor.h
│ │ ├── MapManager/
│ │ │ └── Map_Manager.h
│ │ ├── segment/
│ │ │ ├── pointsCorrect.hpp
│ │ │ └── segment.hpp
│ │ ├── sophus/
│ │ │ ├── average.hpp
│ │ │ ├── common.hpp
│ │ │ ├── example_ensure_handler.cpp
│ │ │ ├── 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
│ │ │ ├── test_macros.hpp
│ │ │ ├── types.hpp
│ │ │ └── velocities.hpp
│ │ └── utils/
│ │ ├── ceresfunc.h
│ │ ├── math_utils.hpp
│ │ └── pcl_utils.hpp
│ ├── launch/
│ │ └── livox_odometry.launch
│ ├── package.xml
│ ├── rviz_cfg/
│ │ └── lio.rviz
│ └── src/
│ ├── lio/
│ │ ├── Estimator.cpp
│ │ ├── IMUIntegrator.cpp
│ │ ├── LidarFeatureExtractor.cpp
│ │ ├── Map_Manager.cpp
│ │ ├── PoseEstimation.cpp
│ │ ├── ScanRegistration.cpp
│ │ └── ceresfunc.cpp
│ └── segment/
│ ├── pointsCorrect.cpp
│ └── segment.cpp
├── README.md
├── SC-PGO/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── include/
│ │ ├── livox_mapping/
│ │ │ ├── common.h
│ │ │ └── tic_toc.h
│ │ └── scancontext/
│ │ ├── KDTreeVectorOfVectorsAdaptor.h
│ │ ├── Scancontext.cpp
│ │ ├── Scancontext.h
│ │ └── nanoflann.hpp
│ ├── launch/
│ │ └── livox_mapping.launch
│ ├── package.xml
│ ├── rviz_cfg/
│ │ └── livox_mapping.rviz
│ ├── src/
│ │ └── laserPosegraphOptimization.cpp
│ └── utils/
│ ├── moving_object_removal/
│ │ ├── README.md
│ │ └── move_object_removal_livox.py
│ └── python/
│ ├── bone_table.npy
│ ├── jet_table.npy
│ ├── makeMergedMap.py
│ └── pypcdMyUtils.py
└── ad_localization_msgs/
├── CMakeLists.txt
├── msg/
│ └── NavStateInfo.msg
└── package.xml
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
.vscode/
__pycache__/
================================================
FILE: LICENSE
================================================
BSD 3-Clause License
Copyright (c) 2021, PJLab-ADG
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: LIO-Livox/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(livox_odometry)
SET(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(catkin REQUIRED COMPONENTS
message_generation
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
tf
tf_conversions
rosbag
livox_ros_driver
eigen_conversions
pcl_conversions
pcl_ros
message_filters
std_srvs)
find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(Ceres REQUIRED)
find_package(OpenCV REQUIRED)
find_package(SuiteSparse REQUIRED)
include_directories(include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${SUITESPARSE_INCLUDE_DIRS})
##################
## ROS messages ##
##################
generate_messages(DEPENDENCIES std_msgs)
catkin_package(CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
pcl_conversions pcl_ros message_filters std_srvs tf_conversions tf
eigen_conversions DEPENDS PCL OpenCV INCLUDE_DIRS include)
add_executable (ScanRegistration
src/lio/ScanRegistration.cpp
src/lio/LidarFeatureExtractor.cpp
src/segment/segment.cpp
src/segment/pointsCorrect.cpp)
target_link_libraries(ScanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable (PoseEstimation
src/lio/PoseEstimation.cpp
src/lio/Estimator.cpp
src/lio/IMUIntegrator.cpp
src/lio/ceresfunc.cpp
src/lio/Map_Manager.cpp)
target_link_libraries(PoseEstimation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
================================================
FILE: LIO-Livox/LICENSE
================================================
BSD 3-Clause License
Copyright (c) 2021, LIVOX
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
================================================
FILE: LIO-Livox/README.md
================================================
# LIO-Livox (A Robust LiDAR-Inertial Odometry for Livox LiDAR)
This respository implements a robust LiDAR-inertial odometry system for Livox LiDAR.
The system uses only a single Livox LiDAR with a built-in IMU. It has a robust initialization module,
which is independent to the sensor motion. **It can be initialized with the static state, dynamic state, and the mixture of static and dynamic state.**
The system achieves super robust performance. **It can pass through a 4km-tunnel and run on the highway with a very high speed (about 80km/h) using a single Livox Horizon.**
Moreover, **it is also robust to dynamic objects**, such as cars, bicycles, and pedestrains. It obtains high precision of localization even in traffic jams.
**The mapping result is precise even most of the FOV is occluded by vehicles.**
Videos of the demonstration of the system can be found on Youtube and Bilibili. *NOTE: Images are only used for demonstration, not used in the system.*
**Developer**: [Livox](www.livoxtech.com)
<div align="center">
<img src="./doc/tunnel.gif" width="850px">
<img src="./doc/urban_dynamic.gif" width="850px">
<img src="./doc/urban_complex.gif" width="850px">
</div>
## System achritecture
<div align="center">
<img src="./doc/system.png" width="1000px">
</div>
The system consists of two ros nodes: ScanRegistartion and PoseEstimation.
* The class "LidarFeatureExtractor" of the node "ScanRegistartion" extracts corner features, surface features, and irregular features from the raw point cloud.
* In the node "PoseEstimation", the main thread aims to estimate sensor poses, while another thread in the class "Estimator" uses the class "MapManager" to build and manage feature maps.
The system is mainly designed for car platforms in the large scale outdoor environment.
Users can easily run the system with a Livox Horizon LiDAR.Horizon
The system starts with the node "ScanRegistartion", where feature points are extracted. Before the feature extraction, dynamic objects are removed from the raw point cloud, since in urban scenes there are usually many dynamic objects, which
affect system robustness and precision. For the dynamic objects filter, we use a fast point cloud segmentation method. The Euclidean clustering is applied to group points into some clusters. The raw point cloud is divided into ground points, background points, and foreground points.
Foreground points are considered as dynamic objects, which are excluded form the feature extraction process. Due to the dynamic objects filter, the system obtains high robustness in dynamic scenes.
In open scenarios, usually few features can be extracted, leading to degeneracy on certain degrees of freedom. To tackle this problem, we developed a feature extraction process to make the distribution of feature points wide and uniform.
A uniform and wide distribution provides more constraints on all 6 degrees of freedom, which is helpful for eliminating degeneracy. Besides, some irregular points also provides information in feature-less
scenes. Therefore, we also extract irregular features as a class for the point cloud registration.
Feature points are classifed into three types, corner features, surface features, and irregular features, according to their
local geometry properties. We first extract points with large curvature and isolated points on each scan line as corner points. Then principal components analysis (PCA) is performed to classify surface features and irregular features, as shown in the following figure.
For points with different distance, thresholds are set to different values, in order to make the distribution of points in space as uniform as possible.
<div align="center">
<img src="./doc/feature extraction.png" height="400px">
</div>
In the node "PoseEstimation", the motion distortion of the point cloud is compensated using IMU preintegration or constant velocity model. Then the IMU initialization module is performed. If the Initialization
is successfully finished, the system will switch to the LIO mode. Otherwise, it run with LO mode and initialize IMU states. In the LO mode, we use a frame-to-model point cloud registration to estimate the sensor pose.
Inspired by ORB-SLAM3, a maximum a posteriori (MAP) estimation method is adopted to jointly initialize IMU biases, velocities, and the gravity direction.
This method doesn't need a careful initialization process. **The system can be initialized with an arbitrary motion.** This method takes into account sensor uncertainty, which obtains the optimum in the sense of maximum posterior probability.
It achieves efficient, robust, and accurate performance.
After the initialization, a tightly coupled slding window based sensor fusion module is performed to estimate IMU poses, biases, and velocities within the sliding window.
Simultaneously, an extra thread builds and maintains the global map in parallel.
## Prerequisites
* [Ubuntu](http://ubuntu.com) (tested on 16.04 and 18.04)
* [ROS](http://wiki.ros.org/ROS/Installation) (tested with Kinetic and Melodic)
* [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page)
* [Ceres Solver](http://ceres-solver.org/installation.html)
* [PCL](http://www.pointclouds.org/downloads/linux.html)
* [livox_ros_driver](https://github.com/Livox-SDK/livox_ros_driver)
* Suitesparse
```
sudo apt-get install libsuitesparse-dev
```
## Compilation
```
cd ~/catkin_ws/src
git clone https://github.com/Livox-SDK/LIO-Livox
cd ..
catkin_make
```
## Run with bag files:
### Run the launch file:
```
cd ~/catkin_ws
source devel/setup.bash
roslaunch lio_livox horizon.launch
```
#### Play your bag files:
```
rosbag play YOUR_ROSBAG.bag
```
## Run with your device:
### Run your LiDAR with livox_ros_driver
```
cd ~/catkin_ws
source devel/setup.bash
roslaunch livox_ros_driver livox_lidar_msg.launch
```
### Run the launch file:
```
cd ~/catkin_ws
source devel/setup.bash
roslaunch lio_livox horizon.launch
```
## Notes:
The current version of the system is only adopted for Livox Horizon. In theory, it should be able to run directly with a Livox Avia, but we haven't done enough tests.
Besides, the system doesn't provide a interface of Livox mid series. If you want use mid-40 or mid-70, you can try [livox_mapping](https://github.com/Livox-SDK/livox_mapping).
The topic of point cloud messages is /livox/lidar and its type is livox_ros_driver/CustomMsg. \
The topic of IMU messages is /livox/imu and its type is sensor_msgs/Imu.
There are some parameters in launch files:
* IMU_Mode: choose IMU information fusion strategy, there are 3 modes:
- 0 - without using IMU information, pure LiDAR odometry, motion distortion is removed using a constant velocity model
- 1 - using IMU preintegration to remove motion distortion
- 2 - tightly coupling IMU and LiDAR information
* Extrinsic_Tlb: extrinsic parameter between LiDAR and IMU, which uses SE3 form. If you want to use an external IMU, you need to calibrate your own sensor suite
and change this parameter to your extrinsic parameter.
There are also some parameters in the config file:
* Use_seg: choose the segmentation mode for dynamic objects filtering, there are 2 modes:
- 0 - without using the segmentation method, you can choose this mode if there is few dynamic objects in your data
- 1 - using the segmentation method to remove dynamic objects
## Acknowledgements
Thanks for following work:
* [LOAM](https://github.com/cuitaixiang/LOAM_NOTED) (LOAM: Lidar Odometry and Mapping in Real-time)
* [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) (VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator)
* [LIO-mapping](https://github.com/hyye/lio-mapping) (Tightly Coupled 3D Lidar Inertial Odometry and Mapping)
* [ORB-SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) (ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM)
* [LiLi-OM](https://github.com/KIT-ISAS/lili-om) (Towards High-Performance Solid-State-LiDAR-Inertial Odometry and Mapping)
* [MSCKF_VIO](https://github.com/KumarRobotics/msckf_vio) (Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight)
* [horizon_highway_slam](https://github.com/Livox-SDK/horizon_highway_slam)
* [livox_mapping](https://github.com/Livox-SDK/livox_mapping)
* [livox_horizon_loam](https://github.com/Livox-SDK/livox_horizon_loam)
## Support
You can get support from Livox with the following methods:
* Send email to [cs@livoxtech.com](cs@livoxtech.com) with a clear description of your problem and your setup
* Report issues on github
================================================
FILE: LIO-Livox/cmake/FindEigen3.cmake
================================================
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
# specific additional paths for some OS
if (WIN32)
set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
endif(WIN32)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${EIGEN_ADDITIONAL_SEARCH_PATHS}
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)
================================================
FILE: LIO-Livox/cmake/FindSuiteSparse.cmake
================================================
# Ceres Solver - A fast non-linear least squares minimizer
# Copyright 2015 Google Inc. All rights reserved.
# http://ceres-solver.org/
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
# * Neither the name of Google Inc. nor the names of its contributors may be
# used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: alexs.mac@gmail.com (Alex Stewart)
#
# FindSuiteSparse.cmake - Find SuiteSparse libraries & dependencies.
#
# This module defines the following variables:
#
# SUITESPARSE_FOUND: TRUE iff SuiteSparse and all dependencies have been found.
# SUITESPARSE_INCLUDE_DIRS: Include directories for all SuiteSparse components.
# SUITESPARSE_LIBRARIES: Libraries for all SuiteSparse component libraries and
# dependencies.
# SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or
# SuiteSparse_config.h (>= v4).
# SUITESPARSE_MAIN_VERSION: Equal to 4 if SUITESPARSE_VERSION = 4.2.1
# SUITESPARSE_SUB_VERSION: Equal to 2 if SUITESPARSE_VERSION = 4.2.1
# SUITESPARSE_SUBSUB_VERSION: Equal to 1 if SUITESPARSE_VERSION = 4.2.1
#
# SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION: TRUE iff running
# on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system
# install, in which case found version of SuiteSparse cannot be used to link
# a shared library due to a bug (static linking is unaffected).
#
# The following variables control the behaviour of this module:
#
# SUITESPARSE_INCLUDE_DIR_HINTS: List of additional directories in which to
# search for SuiteSparse includes,
# e.g: /timbuktu/include.
# SUITESPARSE_LIBRARY_DIR_HINTS: List of additional directories in which to
# search for SuiteSparse libraries,
# e.g: /timbuktu/lib.
#
# The following variables define the presence / includes & libraries for the
# SuiteSparse components searched for, the SUITESPARSE_XX variables are the
# union of the variables for all components.
#
# == Symmetric Approximate Minimum Degree (AMD)
# AMD_FOUND
# AMD_INCLUDE_DIR
# AMD_LIBRARY
#
# == Constrained Approximate Minimum Degree (CAMD)
# CAMD_FOUND
# CAMD_INCLUDE_DIR
# CAMD_LIBRARY
#
# == Column Approximate Minimum Degree (COLAMD)
# COLAMD_FOUND
# COLAMD_INCLUDE_DIR
# COLAMD_LIBRARY
#
# Constrained Column Approximate Minimum Degree (CCOLAMD)
# CCOLAMD_FOUND
# CCOLAMD_INCLUDE_DIR
# CCOLAMD_LIBRARY
#
# == Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD)
# CHOLMOD_FOUND
# CHOLMOD_INCLUDE_DIR
# CHOLMOD_LIBRARY
#
# == Multifrontal Sparse QR (SuiteSparseQR)
# SUITESPARSEQR_FOUND
# SUITESPARSEQR_INCLUDE_DIR
# SUITESPARSEQR_LIBRARY
#
# == Common configuration for all but CSparse (SuiteSparse version >= 4).
# SUITESPARSE_CONFIG_FOUND
# SUITESPARSE_CONFIG_INCLUDE_DIR
# SUITESPARSE_CONFIG_LIBRARY
#
# == Common configuration for all but CSparse (SuiteSparse version < 4).
# UFCONFIG_FOUND
# UFCONFIG_INCLUDE_DIR
#
# Optional SuiteSparse Dependencies:
#
# == Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS)
# METIS_FOUND
# METIS_LIBRARY
#
# == Intel Thread Building Blocks (TBB)
# TBB_FOUND
# TBB_LIBRARY
# TBB_MALLOC_FOUND
# TBB_MALLOC_LIBRARY
# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
# FindSuiteSparse was invoked.
macro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX)
if (MSVC)
set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
endif (MSVC)
endmacro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX)
# Called if we failed to find SuiteSparse or any of it's required dependencies,
# unsets all public (designed to be used externally) variables and reports
# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
macro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG)
unset(SUITESPARSE_FOUND)
unset(SUITESPARSE_INCLUDE_DIRS)
unset(SUITESPARSE_LIBRARIES)
unset(SUITESPARSE_VERSION)
unset(SUITESPARSE_MAIN_VERSION)
unset(SUITESPARSE_SUB_VERSION)
unset(SUITESPARSE_SUBSUB_VERSION)
# Do NOT unset SUITESPARSE_FOUND_REQUIRED_VARS here, as it is used by
# FindPackageHandleStandardArgs() to generate the automatic error message on
# failure which highlights which components are missing.
suitesparse_reset_find_library_prefix()
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if (SuiteSparse_FIND_QUIETLY)
message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN})
elseif (SuiteSparse_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN})
else()
# Neither QUIETLY nor REQUIRED, use no priority which emits a message
# but continues configuration and allows generation.
message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN})
endif (SuiteSparse_FIND_QUIETLY)
# Do not call return(), s/t we keep processing if not called with REQUIRED
# and report all missing components, rather than bailing after failing to find
# the first.
endmacro(SUITESPARSE_REPORT_NOT_FOUND)
# Protect against any alternative find_package scripts for this library having
# been called previously (in a client project) which set SUITESPARSE_FOUND, but
# not the other variables we require / set here which could cause the search
# logic here to fail.
unset(SUITESPARSE_FOUND)
# Handle possible presence of lib prefix for libraries on MSVC, see
# also SUITESPARSE_RESET_FIND_LIBRARY_PREFIX().
if (MSVC)
# Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES
# s/t we can set it back before returning.
set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}")
# The empty string in this list is important, it represents the case when
# the libraries have no prefix (shared libraries / DLLs).
set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}")
endif (MSVC)
# Specify search directories for include files and libraries (this is the union
# of the search directories for all OSs). Search user-specified hint
# directories first if supplied, and search user-installed locations first
# so that we prefer user installs to system installs where both exist.
list(APPEND SUITESPARSE_CHECK_INCLUDE_DIRS
/opt/local/include
/opt/local/include/ufsparse # Mac OS X
/usr/local/homebrew/include # Mac OS X
/usr/local/include
/usr/include)
list(APPEND SUITESPARSE_CHECK_LIBRARY_DIRS
/opt/local/lib
/opt/local/lib/ufsparse # Mac OS X
/usr/local/homebrew/lib # Mac OS X
/usr/local/lib
/usr/lib)
# Additional suffixes to try appending to each search path.
list(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES
suitesparse) # Windows/Ubuntu
# Wrappers to find_path/library that pass the SuiteSparse search hints/paths.
#
# suitesparse_find_component(<component> [FILES name1 [name2 ...]]
# [LIBRARIES name1 [name2 ...]]
# [REQUIRED])
macro(suitesparse_find_component COMPONENT)
include(CMakeParseArguments)
set(OPTIONS REQUIRED)
set(MULTI_VALUE_ARGS FILES LIBRARIES)
cmake_parse_arguments(SUITESPARSE_FIND_${COMPONENT}
"${OPTIONS}" "" "${MULTI_VALUE_ARGS}" ${ARGN})
if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED)
list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${COMPONENT}_FOUND)
endif()
set(${COMPONENT}_FOUND TRUE)
if (SUITESPARSE_FIND_${COMPONENT}_FILES)
find_path(${COMPONENT}_INCLUDE_DIR
NAMES ${SUITESPARSE_FIND_${COMPONENT}_FILES}
HINTS ${SUITESPARSE_INCLUDE_DIR_HINTS}
PATHS ${SUITESPARSE_CHECK_INCLUDE_DIRS}
PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES})
if (${COMPONENT}_INCLUDE_DIR)
message(STATUS "Found ${COMPONENT} headers in: "
"${${COMPONENT}_INCLUDE_DIR}")
mark_as_advanced(${COMPONENT}_INCLUDE_DIR)
else()
# Specified headers not found.
set(${COMPONENT}_FOUND FALSE)
if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED)
suitesparse_report_not_found(
"Did not find ${COMPONENT} header (required SuiteSparse component).")
else()
message(STATUS "Did not find ${COMPONENT} header (optional "
"SuiteSparse component).")
# Hide optional vars from CMake GUI even if not found.
mark_as_advanced(${COMPONENT}_INCLUDE_DIR)
endif()
endif()
endif()
if (SUITESPARSE_FIND_${COMPONENT}_LIBRARIES)
find_library(${COMPONENT}_LIBRARY
NAMES ${SUITESPARSE_FIND_${COMPONENT}_LIBRARIES}
HINTS ${SUITESPARSE_LIBRARY_DIR_HINTS}
PATHS ${SUITESPARSE_CHECK_LIBRARY_DIRS}
PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES})
if (${COMPONENT}_LIBRARY)
message(STATUS "Found ${COMPONENT} library: ${${COMPONENT}_LIBRARY}")
mark_as_advanced(${COMPONENT}_LIBRARY)
else ()
# Specified libraries not found.
set(${COMPONENT}_FOUND FALSE)
if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED)
suitesparse_report_not_found(
"Did not find ${COMPONENT} library (required SuiteSparse component).")
else()
message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse "
"dependency)")
# Hide optional vars from CMake GUI even if not found.
mark_as_advanced(${COMPONENT}_LIBRARY)
endif()
endif()
endif()
endmacro()
# Given the number of components of SuiteSparse, and to ensure that the
# automatic failure message generated by FindPackageHandleStandardArgs()
# when not all required components are found is helpful, we maintain a list
# of all variables that must be defined for SuiteSparse to be considered found.
unset(SUITESPARSE_FOUND_REQUIRED_VARS)
# BLAS.
find_package(BLAS QUIET)
if (NOT BLAS_FOUND)
suitesparse_report_not_found(
"Did not find BLAS library (required for SuiteSparse).")
endif (NOT BLAS_FOUND)
list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS BLAS_FOUND)
# LAPACK.
find_package(LAPACK QUIET)
if (NOT LAPACK_FOUND)
suitesparse_report_not_found(
"Did not find LAPACK library (required for SuiteSparse).")
endif (NOT LAPACK_FOUND)
list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS LAPACK_FOUND)
suitesparse_find_component(AMD REQUIRED FILES amd.h LIBRARIES amd)
suitesparse_find_component(CAMD REQUIRED FILES camd.h LIBRARIES camd)
suitesparse_find_component(COLAMD REQUIRED FILES colamd.h LIBRARIES colamd)
suitesparse_find_component(CCOLAMD REQUIRED FILES ccolamd.h LIBRARIES ccolamd)
suitesparse_find_component(CHOLMOD REQUIRED FILES cholmod.h LIBRARIES cholmod)
suitesparse_find_component(
SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp LIBRARIES spqr)
if (SUITESPARSEQR_FOUND)
# SuiteSparseQR may be compiled with Intel Threading Building Blocks,
# we assume that if TBB is installed, SuiteSparseQR was compiled with
# support for it, this will do no harm if it wasn't.
find_package(TBB QUIET)
if (TBB_FOUND)
message(STATUS "Found Intel Thread Building Blocks (TBB) library "
"(${TBB_VERSION}) assuming SuiteSparseQR was compiled "
"with TBB.")
# Add the TBB libraries to the SuiteSparseQR libraries (the only
# libraries to optionally depend on TBB).
list(APPEND SUITESPARSEQR_LIBRARY ${TBB_LIBRARIES})
else()
message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was "
"not compiled with TBB.")
endif()
endif(SUITESPARSEQR_FOUND)
# UFconfig / SuiteSparse_config.
#
# If SuiteSparse version is >= 4 then SuiteSparse_config is required.
# For SuiteSparse 3, UFconfig.h is required.
suitesparse_find_component(
SUITESPARSE_CONFIG FILES SuiteSparse_config.h LIBRARIES suitesparseconfig)
if (SUITESPARSE_CONFIG_FOUND)
# SuiteSparse_config (SuiteSparse version >= 4) requires librt library for
# timing by default when compiled on Linux or Unix, but not on OSX (which
# does not have librt).
if (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE)
suitesparse_find_component(LIBRT LIBRARIES rt)
if (LIBRT_FOUND)
message(STATUS "Adding librt: ${LIBRT_LIBRARY} to "
"SuiteSparse_config libraries (required on Linux & Unix [not OSX] if "
"SuiteSparse is compiled with timing).")
list(APPEND SUITESPARSE_CONFIG_LIBRARY ${LIBRT_LIBRARY})
else()
message(STATUS "Could not find librt, but found SuiteSparse_config, "
"assuming that SuiteSparse was compiled without timing.")
endif ()
endif (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE)
else()
# Failed to find SuiteSparse_config (>= v4 installs), instead look for
# UFconfig header which should be present in < v4 installs.
suitesparse_find_component(UFCONFIG FILES UFconfig.h)
endif ()
if (NOT SUITESPARSE_CONFIG_FOUND AND
NOT UFCONFIG_FOUND)
suitesparse_report_not_found(
"Failed to find either: SuiteSparse_config header & library (should be "
"present in all SuiteSparse >= v4 installs), or UFconfig header (should "
"be present in all SuiteSparse < v4 installs).")
endif()
# Extract the SuiteSparse version from the appropriate header (UFconfig.h for
# <= v3, SuiteSparse_config.h for >= v4).
list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS SUITESPARSE_VERSION)
if (UFCONFIG_FOUND)
# SuiteSparse version <= 3.
set(SUITESPARSE_VERSION_FILE ${UFCONFIG_INCLUDE_DIR}/UFconfig.h)
if (NOT EXISTS ${SUITESPARSE_VERSION_FILE})
suitesparse_report_not_found(
"Could not find file: ${SUITESPARSE_VERSION_FILE} containing version "
"information for <= v3 SuiteSparse installs, but UFconfig was found "
"(only present in <= v3 installs).")
else (NOT EXISTS ${SUITESPARSE_VERSION_FILE})
file(READ ${SUITESPARSE_VERSION_FILE} UFCONFIG_CONTENTS)
string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+"
SUITESPARSE_MAIN_VERSION "${UFCONFIG_CONTENTS}")
string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1"
SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}")
string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+"
SUITESPARSE_SUB_VERSION "${UFCONFIG_CONTENTS}")
string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1"
SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}")
string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+"
SUITESPARSE_SUBSUB_VERSION "${UFCONFIG_CONTENTS}")
string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1"
SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}")
# This is on a single line s/t CMake does not interpret it as a list of
# elements and insert ';' separators which would result in 4.;2.;1 nonsense.
set(SUITESPARSE_VERSION
"${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}")
endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE})
endif (UFCONFIG_FOUND)
if (SUITESPARSE_CONFIG_FOUND)
# SuiteSparse version >= 4.
set(SUITESPARSE_VERSION_FILE
${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h)
if (NOT EXISTS ${SUITESPARSE_VERSION_FILE})
suitesparse_report_not_found(
"Could not find file: ${SUITESPARSE_VERSION_FILE} containing version "
"information for >= v4 SuiteSparse installs, but SuiteSparse_config was "
"found (only present in >= v4 installs).")
else (NOT EXISTS ${SUITESPARSE_VERSION_FILE})
file(READ ${SUITESPARSE_VERSION_FILE} SUITESPARSE_CONFIG_CONTENTS)
string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+"
SUITESPARSE_MAIN_VERSION "${SUITESPARSE_CONFIG_CONTENTS}")
string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1"
SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}")
string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+"
SUITESPARSE_SUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}")
string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1"
SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}")
string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+"
SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}")
string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1"
SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}")
# This is on a single line s/t CMake does not interpret it as a list of
# elements and insert ';' separators which would result in 4.;2.;1 nonsense.
set(SUITESPARSE_VERSION
"${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}")
endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE})
endif (SUITESPARSE_CONFIG_FOUND)
# METIS (Optional dependency).
suitesparse_find_component(METIS LIBRARIES metis)
# Only mark SuiteSparse as found if all required components and dependencies
# have been found.
set(SUITESPARSE_FOUND TRUE)
foreach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS})
if (NOT ${REQUIRED_VAR})
set(SUITESPARSE_FOUND FALSE)
endif (NOT ${REQUIRED_VAR})
endforeach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS})
if (SUITESPARSE_FOUND)
list(APPEND SUITESPARSE_INCLUDE_DIRS
${AMD_INCLUDE_DIR}
${CAMD_INCLUDE_DIR}
${COLAMD_INCLUDE_DIR}
${CCOLAMD_INCLUDE_DIR}
${CHOLMOD_INCLUDE_DIR}
${SUITESPARSEQR_INCLUDE_DIR})
# Handle config separately, as otherwise at least one of them will be set
# to NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail.
if (SUITESPARSE_CONFIG_FOUND)
list(APPEND SUITESPARSE_INCLUDE_DIRS
${SUITESPARSE_CONFIG_INCLUDE_DIR})
endif (SUITESPARSE_CONFIG_FOUND)
if (UFCONFIG_FOUND)
list(APPEND SUITESPARSE_INCLUDE_DIRS
${UFCONFIG_INCLUDE_DIR})
endif (UFCONFIG_FOUND)
# As SuiteSparse includes are often all in the same directory, remove any
# repetitions.
list(REMOVE_DUPLICATES SUITESPARSE_INCLUDE_DIRS)
# Important: The ordering of these libraries is *NOT* arbitrary, as these
# could potentially be static libraries their link ordering is important.
list(APPEND SUITESPARSE_LIBRARIES
${SUITESPARSEQR_LIBRARY}
${CHOLMOD_LIBRARY}
${CCOLAMD_LIBRARY}
${CAMD_LIBRARY}
${COLAMD_LIBRARY}
${AMD_LIBRARY}
${LAPACK_LIBRARIES}
${BLAS_LIBRARIES})
if (SUITESPARSE_CONFIG_FOUND)
list(APPEND SUITESPARSE_LIBRARIES
${SUITESPARSE_CONFIG_LIBRARY})
endif (SUITESPARSE_CONFIG_FOUND)
if (METIS_FOUND)
list(APPEND SUITESPARSE_LIBRARIES
${METIS_LIBRARY})
endif (METIS_FOUND)
endif()
# Determine if we are running on Ubuntu with the package install of SuiteSparse
# which is broken and does not support linking a shared library.
set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION FALSE)
if (CMAKE_SYSTEM_NAME MATCHES "Linux" AND
SUITESPARSE_VERSION VERSION_EQUAL 3.4.0)
find_program(LSB_RELEASE_EXECUTABLE lsb_release)
if (LSB_RELEASE_EXECUTABLE)
# Any even moderately recent Ubuntu release (likely to be affected by
# this bug) should have lsb_release, if it isn't present we are likely
# on a different Linux distribution (should be fine).
execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si
OUTPUT_VARIABLE LSB_DISTRIBUTOR_ID
OUTPUT_STRIP_TRAILING_WHITESPACE)
if (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND
SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd")
# We are on Ubuntu, and the SuiteSparse version matches the broken
# system install version and is a system install.
set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION TRUE)
message(STATUS "Found system install of SuiteSparse "
"${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug "
"preventing linking of shared libraries (static linking unaffected).")
endif (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND
SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd")
endif (LSB_RELEASE_EXECUTABLE)
endif (CMAKE_SYSTEM_NAME MATCHES "Linux" AND
SUITESPARSE_VERSION VERSION_EQUAL 3.4.0)
suitesparse_reset_find_library_prefix()
# Handle REQUIRED and QUIET arguments to FIND_PACKAGE
include(FindPackageHandleStandardArgs)
if (SUITESPARSE_FOUND)
find_package_handle_standard_args(SuiteSparse
REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS}
VERSION_VAR SUITESPARSE_VERSION
FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.")
else (SUITESPARSE_FOUND)
# Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to
# find SuiteSparse to avoid a confusing autogenerated failure message
# that states 'not found (missing: FOO) (found version: x.y.z)'.
find_package_handle_standard_args(SuiteSparse
REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS}
FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.")
endif (SUITESPARSE_FOUND)
================================================
FILE: LIO-Livox/config/horizon_config.yaml
================================================
%YAML:1.0
# switches
Lidar_Type: 0 # 0-horizon
Used_Line: 6 # lines used for lio, set to 1~6
Feature_Mode: 0 # 0(false) or 1(true)
NumCurvSize: 2
DistanceFaraway: 100 # [m] <DistanceFaraway near / >DistanceFaraway far
NumFlat: 3 # nums of one part's flat feature
PartNum: 150 # nums of one scan's parts
FlatThreshold: 0.02 # cloud curvature threshold of flat feature
BreakCornerDis: 1 # break distance of break points
LidarNearestDis: 1.0 # if(depth < LidarNearestDis) do not use this point
KdTreeCornerOutlierDis: 0.2 # corner filter threshold
Use_seg: 1 # use segment algorithm
map_skip_frame: 2
================================================
FILE: LIO-Livox/include/Estimator/Estimator.h
================================================
#ifndef LIO_LIVOX_ESTIMATOR_H
#define LIO_LIVOX_ESTIMATOR_H
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/NavSatFix.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <Eigen/Core>
#include <sensor_msgs/Imu.h>
#include <queue>
#include <iterator>
#include <future>
#include "MapManager/Map_Manager.h"
#include "utils/ceresfunc.h"
#include "utils/pcl_utils.hpp"
#include "IMUIntegrator/IMUIntegrator.h"
#include <chrono>
class Estimator{
typedef pcl::PointXYZINormal PointType;
public:
/** \brief slide window size */
static const int SLIDEWINDOWSIZE = 10;
/** \brief lidar frame struct */
struct LidarFrame{
pcl::PointCloud<PointType>::Ptr laserCloud;
IMUIntegrator imuIntegrator;
Eigen::Vector3d P;
Eigen::Vector3d V;
Eigen::Quaterniond Q;
Eigen::Vector3d bg;
Eigen::Vector3d ba;
Eigen::VectorXf ground_plane_coeff;
double timeStamp;
LidarFrame(){
P.setZero();
V.setZero();
Q.setIdentity();
bg.setZero();
ba.setZero();
timeStamp = 0;
}
};
/** \brief point to line feature */
struct FeatureLine{
Eigen::Vector3d pointOri;
Eigen::Vector3d lineP1;
Eigen::Vector3d lineP2;
double error;
bool valid;
FeatureLine(Eigen::Vector3d po, Eigen::Vector3d p1, Eigen::Vector3d p2)
:pointOri(std::move(po)), lineP1(std::move(p1)), lineP2(std::move(p2)){
valid = false;
error = 0;
}
double ComputeError(const Eigen::Matrix4d& pose){
Eigen::Vector3d P_to_Map = pose.topLeftCorner(3,3) * pointOri + pose.topRightCorner(3,1);
double l12 = std::sqrt((lineP1(0) - lineP2(0))*(lineP1(0) - lineP2(0)) + (lineP1(1) - lineP2(1))*
(lineP1(1) - lineP2(1)) + (lineP1(2) - lineP2(2))*(lineP1(2) - lineP2(2)));
double a012 = std::sqrt(
((P_to_Map(0) - lineP1(0)) * (P_to_Map(1) - lineP2(1)) - (P_to_Map(0) - lineP2(0)) * (P_to_Map(1) - lineP1(1)))
* ((P_to_Map(0) - lineP1(0)) * (P_to_Map(1) - lineP2(1)) - (P_to_Map(0) - lineP2(0)) * (P_to_Map(1) - lineP1(1)))
+ ((P_to_Map(0) - lineP1(0)) * (P_to_Map(2) - lineP2(2)) - (P_to_Map(0) - lineP2(0)) * (P_to_Map(2) - lineP1(2)))
* ((P_to_Map(0) - lineP1(0)) * (P_to_Map(2) - lineP2(2)) - (P_to_Map(0) - lineP2(0)) * (P_to_Map(2) - lineP1(2)))
+ ((P_to_Map(1) - lineP1(1)) * (P_to_Map(2) - lineP2(2)) - (P_to_Map(1) - lineP2(1)) * (P_to_Map(2) - lineP1(2)))
* ((P_to_Map(1) - lineP1(1)) * (P_to_Map(2) - lineP2(2)) - (P_to_Map(1) - lineP2(1)) * (P_to_Map(2) - lineP1(2))));
error = a012 / l12;
}
};
/** \brief point to plan feature */
struct FeaturePlan{
Eigen::Vector3d pointOri;
double pa;
double pb;
double pc;
double pd;
double error;
bool valid;
FeaturePlan(const Eigen::Vector3d& po, const double& pa_, const double& pb_, const double& pc_, const double& pd_)
:pointOri(po), pa(pa_), pb(pb_), pc(pc_), pd(pd_){
valid = false;
error = 0;
}
double ComputeError(const Eigen::Matrix4d& pose){
Eigen::Vector3d P_to_Map = pose.topLeftCorner(3,3) * pointOri + pose.topRightCorner(3,1);
error = pa * P_to_Map(0) + pb * P_to_Map(1) + pc * P_to_Map(2) + pd;
}
};
/** \brief point to plan feature */
struct FeaturePlanVec{
Eigen::Vector3d pointOri;
Eigen::Vector3d pointProj;
Eigen::Matrix3d sqrt_info;
double error;
bool valid;
FeaturePlanVec(const Eigen::Vector3d& po, const Eigen::Vector3d& p_proj, Eigen::Matrix3d sqrt_info_)
:pointOri(po), pointProj(p_proj), sqrt_info(sqrt_info_) {
valid = false;
error = 0;
}
double ComputeError(const Eigen::Matrix4d& pose){
Eigen::Vector3d P_to_Map = pose.topLeftCorner(3,3) * pointOri + pose.topRightCorner(3,1);
error = (P_to_Map - pointProj).norm();
}
};
/** \brief non feature */
struct FeatureNon{
Eigen::Vector3d pointOri;
double pa;
double pb;
double pc;
double pd;
double error;
bool valid;
FeatureNon(const Eigen::Vector3d& po, const double& pa_, const double& pb_, const double& pc_, const double& pd_)
:pointOri(po), pa(pa_), pb(pb_), pc(pc_), pd(pd_){
valid = false;
error = 0;
}
double ComputeError(const Eigen::Matrix4d& pose){
Eigen::Vector3d P_to_Map = pose.topLeftCorner(3,3) * pointOri + pose.topRightCorner(3,1);
error = pa * P_to_Map(0) + pb * P_to_Map(1) + pc * P_to_Map(2) + pd;
}
};
public:
/** \brief constructor of Estimator
*/
Estimator(const float& filter_corner, const float& filter_surf);
~Estimator();
/** \brief Open a independent thread to increment MAP cloud
*/
[[noreturn]] void threadMapIncrement();
/** \brief construct sharp feature Ceres Costfunctions
* \param[in] edges: store costfunctions
* \param[in] m4d: lidar pose, represented by matrix 4X4
*/
void processPointToLine(std::vector<ceres::CostFunction *>& edges,
std::vector<FeatureLine>& vLineFeatures,
const pcl::PointCloud<PointType>::Ptr& laserCloudCorner,
const pcl::PointCloud<PointType>::Ptr& laserCloudCornerMap,
const pcl::KdTreeFLANN<PointType>::Ptr& kdtree,
const Eigen::Matrix4d& exTlb,
const Eigen::Matrix4d& m4d);
/** \brief construct Plan feature Ceres Costfunctions
* \param[in] edges: store costfunctions
* \param[in] m4d: lidar pose, represented by matrix 4X4
*/
void processPointToPlan(std::vector<ceres::CostFunction *>& edges,
std::vector<FeaturePlan>& vPlanFeatures,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurf,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurfMap,
const pcl::KdTreeFLANN<PointType>::Ptr& kdtree,
const Eigen::Matrix4d& exTlb,
const Eigen::Matrix4d& m4d);
void processPointToPlanVec(std::vector<ceres::CostFunction *>& edges,
std::vector<FeaturePlanVec>& vPlanFeatures,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurf,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurfMap,
const pcl::KdTreeFLANN<PointType>::Ptr& kdtree,
const Eigen::Matrix4d& exTlb,
const Eigen::Matrix4d& m4d);
void processNonFeatureICP(std::vector<ceres::CostFunction *>& edges,
std::vector<FeatureNon>& vNonFeatures,
const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeature,
const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureLocal,
const pcl::KdTreeFLANN<PointType>::Ptr& kdtreeLocal,
const Eigen::Matrix4d& exTlb,
const Eigen::Matrix4d& m4d);
/** \brief Transform Lidar Pose in slidewindow to double array
* \param[in] lidarFrameList: Lidar Poses in slidewindow
*/
void vector2double(const std::list<LidarFrame>& lidarFrameList);
/** \brief Transform double array to Lidar Pose in slidewindow
* \param[in] lidarFrameList: Lidar Poses in slidewindow
*/
void double2vector(std::list<LidarFrame>& lidarFrameList);
/** \brief estimate lidar pose by matching current lidar cloud with map cloud and tightly coupled IMU message
* \param[in] lidarFrameList: multi-frames of lidar cloud and lidar pose
* \param[in] exTlb: extrinsic matrix between lidar and IMU
* \param[in] gravity: gravity vector
*/
void EstimateLidarPose(std::list<LidarFrame>& lidarFrameList,
const Eigen::Matrix4d& exTlb,
const Eigen::Vector3d& gravity,
nav_msgs::Odometry& debugInfo);
void Estimate(std::list<LidarFrame>& lidarFrameList,
const Eigen::Matrix4d& exTlb,
const Eigen::Vector3d& gravity);
pcl::PointCloud<PointType>::Ptr get_corner_map(){
return map_manager->get_corner_map();
}
pcl::PointCloud<PointType>::Ptr get_surf_map(){
return map_manager->get_surf_map();
}
pcl::PointCloud<PointType>::Ptr get_nonfeature_map(){
return map_manager->get_nonfeature_map();
}
pcl::PointCloud<PointType>::Ptr get_init_ground_cloud(){
return initGroundCloud;
}
void MapIncrementLocal(const pcl::PointCloud<PointType>::Ptr& laserCloudCornerStack,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurfStack,
const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureStack,
const Eigen::Matrix4d& transformTobeMapped);
private:
/** \brief store map points */
MAP_MANAGER* map_manager;
int init_ground_count;
Eigen::VectorXf init_ground_plane_coeff;
double para_PR[SLIDEWINDOWSIZE][6];
double para_VBias[SLIDEWINDOWSIZE][9];
MarginalizationInfo *last_marginalization_info = nullptr;
std::vector<double *> last_marginalization_parameter_blocks;
std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudCornerLast;
std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudSurfLast;
std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudNonFeatureLast;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromLocal;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromLocal;
pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureFromLocal;
pcl::PointCloud<PointType>::Ptr laserCloudCornerForMap;
pcl::PointCloud<PointType>::Ptr laserCloudSurfForMap;
pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureForMap;
pcl::PointCloud<PointType>::Ptr initGroundCloud;
Eigen::Matrix4d transformForMap;
std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudCornerStack;
std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudSurfStack;
std::vector<pcl::PointCloud<PointType>::Ptr> laserCloudNonFeatureStack;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromLocal;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromLocal;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeNonFeatureFromLocal;
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterNonFeature;
std::mutex mtx_Map;
std::thread threadMap;
pcl::KdTreeFLANN<PointType> CornerKdMap[10000];
pcl::KdTreeFLANN<PointType> SurfKdMap[10000];
pcl::KdTreeFLANN<PointType> NonFeatureKdMap[10000];
pcl::PointCloud<PointType> GlobalSurfMap[10000];
pcl::PointCloud<PointType> GlobalCornerMap[10000];
pcl::PointCloud<PointType> GlobalNonFeatureMap[10000];
int laserCenWidth_last = 10;
int laserCenHeight_last = 5;
int laserCenDepth_last = 10;
static const int localMapWindowSize = 50;
int localMapID = 0;
pcl::PointCloud<PointType>::Ptr localCornerMap[localMapWindowSize];
pcl::PointCloud<PointType>::Ptr localSurfMap[localMapWindowSize];
pcl::PointCloud<PointType>::Ptr localNonFeatureMap[localMapWindowSize];
int map_update_ID = 0;
int map_skip_frame = 2; //every map_skip_frame frame update map
double plan_weight_tan = 0.0;
double thres_dist = 1.0;
};
#endif //LIO_LIVOX_ESTIMATOR_H
================================================
FILE: LIO-Livox/include/IMUIntegrator/IMUIntegrator.h
================================================
#ifndef LIO_LIVOX_IMUINTEGRATOR_H
#define LIO_LIVOX_IMUINTEGRATOR_H
#include <sensor_msgs/Imu.h>
#include <queue>
#include <mutex>
#include <Eigen/Core>
#include <utility>
#include <ros/ros.h>
#include "sophus/so3.hpp"
class IMUIntegrator{
public:
IMUIntegrator();
/** \brief constructor of IMUIntegrator
* \param[in] vIMU: IMU messages need to be integrated
*/
IMUIntegrator(std::vector<sensor_msgs::ImuConstPtr> vIMU);
void Reset();
/** \brief get delta quaternion after IMU integration
*/
const Eigen::Quaterniond & GetDeltaQ() const;
/** \brief get delta displacement after IMU integration
*/
const Eigen::Vector3d & GetDeltaP() const;
/** \brief get delta velocity after IMU integration
*/
const Eigen::Vector3d & GetDeltaV() const;
/** \brief get time span after IMU integration
*/
const double & GetDeltaTime() const;
/** \brief get linearized bias gyr
*/
const Eigen::Vector3d & GetBiasGyr() const;
/** \brief get linearized bias acc
*/
const Eigen::Vector3d& GetBiasAcc() const;
/** \brief get covariance matrix after IMU integration
*/
const Eigen::Matrix<double, 15, 15>& GetCovariance();
/** \brief get jacobian matrix after IMU integration
*/
const Eigen::Matrix<double, 15, 15> & GetJacobian() const;
/** \brief get average acceleration of IMU messages for initialization
*/
Eigen::Vector3d GetAverageAcc();
/** \brief push IMU message to the IMU buffer vimuMsg
* \param[in] imu: the IMU message need to be pushed
*/
void PushIMUMsg(const sensor_msgs::ImuConstPtr& imu);
void PushIMUMsg(const std::vector<sensor_msgs::ImuConstPtr>& vimu);
const std::vector<sensor_msgs::ImuConstPtr> & GetIMUMsg() const;
/** \brief only integrate gyro information of each IMU message stored in vimuMsg
* \param[in] lastTime: the left time boundary of vimuMsg
*/
void GyroIntegration(double lastTime);
/** \brief pre-integration of IMU messages stored in vimuMsg
*/
void PreIntegration(double lastTime, const Eigen::Vector3d& bg, const Eigen::Vector3d& ba);
/** \brief normal integration of IMU messages stored in vimuMsg
*/
void Integration(){}
public:
const double acc_n = 0.08;
const double gyr_n = 0.004;
const double acc_w = 2.0e-4;
const double gyr_w = 2.0e-5;
constexpr static const double lidar_m = 6e-3;
constexpr static const double gnorm = 9.805;
enum JacobianOrder
{
O_P = 0,
O_R = 3,
O_V = 6,
O_BG = 9,
O_BA = 12
};
private:
std::vector<sensor_msgs::ImuConstPtr> vimuMsg;
Eigen::Quaterniond dq;
Eigen::Vector3d dp;
Eigen::Vector3d dv;
Eigen::Vector3d linearized_bg;
Eigen::Vector3d linearized_ba;
Eigen::Matrix<double, 15, 15> covariance;
Eigen::Matrix<double, 15, 15> jacobian;
Eigen::Matrix<double, 12, 12> noise;
double dtime;
};
#endif //LIO_LIVOX_IMUINTEGRATOR_H
================================================
FILE: LIO-Livox/include/LidarFeatureExtractor/LidarFeatureExtractor.h
================================================
#ifndef LIO_LIVOX_LIDARFEATUREEXTRACTOR_H
#define LIO_LIVOX_LIDARFEATUREEXTRACTOR_H
#include <ros/ros.h>
#include <livox_ros_driver/CustomMsg.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <future>
#include "opencv2/core.hpp"
#include "segment/segment.hpp"
class LidarFeatureExtractor{
typedef pcl::PointXYZINormal PointType;
public:
/** \brief constructor of LidarFeatureExtractor
* \param[in] n_scans: lines used to extract lidar features
*/
LidarFeatureExtractor(int n_scans,int NumCurvSize,float DistanceFaraway,int NumFlat,int PartNum,float FlatThreshold,
float BreakCornerDis,float LidarNearestDis,float KdTreeCornerOutlierDis);
/** \brief transform float to int
*/
static uint32_t _float_as_int(float f){
union{uint32_t i; float f;} conv{};
conv.f = f;
return conv.i;
}
/** \brief transform int to float
*/
static float _int_as_float(uint32_t i){
union{float f; uint32_t i;} conv{};
conv.i = i;
return conv.f;
}
/** \brief Determine whether the point_list is flat
* \param[in] point_list: points need to be judged
* \param[in] plane_threshold
*/
bool plane_judge(const std::vector<PointType>& point_list,const int plane_threshold);
/** \brief Detect lidar feature points
* \param[in] cloud: original lidar cloud need to be detected
* \param[in] pointsLessSharp: less sharp index of cloud
* \param[in] pointsLessFlat: less flat index of cloud
*/
void detectFeaturePoint(pcl::PointCloud<PointType>::Ptr& cloud,
std::vector<int>& pointsLessSharp,
std::vector<int>& pointsLessFlat);
void detectFeaturePoint2(pcl::PointCloud<PointType>::Ptr& cloud,
pcl::PointCloud<PointType>::Ptr& pointsLessFlat,
pcl::PointCloud<PointType>::Ptr& pointsNonFeature);
void detectFeaturePoint3(pcl::PointCloud<PointType>::Ptr& cloud,
std::vector<int>& pointsLessSharp);
void FeatureExtract_with_segment(const livox_ros_driver::CustomMsgConstPtr &msg,
pcl::PointCloud<PointType>::Ptr& laserCloud,
pcl::PointCloud<PointType>::Ptr& laserConerFeature,
pcl::PointCloud<PointType>::Ptr& laserSurfFeature,
pcl::PointCloud<PointType>::Ptr& laserNonFeature,
sensor_msgs::PointCloud2 &msg2,
int Used_Line = 1);
/** \brief Detect lidar feature points of CustomMsg
* \param[in] msg: original CustomMsg need to be detected
* \param[in] laserCloud: transform CustomMsg to pcl point cloud format
* \param[in] laserConerFeature: less Coner features extracted from laserCloud
* \param[in] laserSurfFeature: less Surf features extracted from laserCloud
*/
void FeatureExtract(const livox_ros_driver::CustomMsgConstPtr &msg,
pcl::PointCloud<PointType>::Ptr& laserCloud,
pcl::PointCloud<PointType>::Ptr& laserConerFeature,
pcl::PointCloud<PointType>::Ptr& laserSurfFeature,
int Used_Line = 1);
private:
/** \brief lines used to extract lidar features */
const int N_SCANS;
/** \brief store original points of each line */
std::vector<pcl::PointCloud<PointType>::Ptr> vlines;
/** \brief store corner feature index of each line */
std::vector<std::vector<int>> vcorner;
/** \brief store surf feature index of each line */
std::vector<std::vector<int>> vsurf;
int thNumCurvSize;
float thDistanceFaraway;
int thNumFlat;
int thPartNum;
float thFlatThreshold;
float thBreakCornerDis;
float thLidarNearestDis;
};
#endif //LIO_LIVOX_LIDARFEATUREEXTRACTOR_H
================================================
FILE: LIO-Livox/include/MapManager/Map_Manager.h
================================================
#ifndef LIO_LIVOX_MAP_MANAGER_H
#define LIO_LIVOX_MAP_MANAGER_H
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <future>
class MAP_MANAGER{
typedef pcl::PointXYZINormal PointType;
public:
std::mutex mtx_MapManager;
/** \brief constructor of MAP_MANAGER */
MAP_MANAGER(const float& filter_corner, const float& filter_surf);
static size_t ToIndex(int i, int j, int k);
/** \brief transform float to int
*/
static uint32_t _float_as_int(float f){
union{uint32_t i; float f;} conv{};
conv.f = f;
return conv.i;
}
/** \brief transform int to float
*/
static float _int_as_float(uint32_t i){
union{float f; uint32_t i;} conv{};
conv.i = i;
return conv.f;
}
/** \brief transform point pi to the MAP coordinate
* \param[in] pi: point to be transformed
* \param[in] po: point after transfomation
* \param[in] _transformTobeMapped: transform matrix between pi and po
*/
static void pointAssociateToMap(PointType const * const pi,
PointType * const po,
const Eigen::Matrix4d& _transformTobeMapped);
void featureAssociateToMap(const pcl::PointCloud<PointType>::Ptr& laserCloudCorner,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurf,
const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeature,
const pcl::PointCloud<PointType>::Ptr& laserCloudCornerToMap,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurfToMap,
const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureToMap,
const Eigen::Matrix4d& transformTobeMapped);
/** \brief add new lidar points to the map
* \param[in] laserCloudCornerStack: coner feature points that need to be added to map
* \param[in] laserCloudSurfStack: surf feature points that need to be added to map
* \param[in] transformTobeMapped: transform matrix of the lidar pose
*/
void MapIncrement(const pcl::PointCloud<PointType>::Ptr& laserCloudCornerStack,
const pcl::PointCloud<PointType>::Ptr& laserCloudSurfStack,
const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureStack,
const Eigen::Matrix4d& transformTobeMapped);
/** \brief retrieve map points according to the lidar pose
* \param[in] laserCloudCornerFromMap: store coner feature points retrieved from map
* \param[in] laserCloudSurfFromMap: tore surf feature points retrieved from map
* \param[in] transformTobeMapped: transform matrix of the lidar pose
*/
void MapMove(const Eigen::Matrix4d& transformTobeMapped);
size_t FindUsedCornerMap(const PointType *p,int a,int b,int c);
size_t FindUsedSurfMap(const PointType *p,int a,int b,int c);
size_t FindUsedNonFeatureMap(const PointType *p,int a,int b,int c);
pcl::KdTreeFLANN<PointType> getCornerKdMap(int i){
return CornerKdMap_last[i];
}
pcl::KdTreeFLANN<PointType> getSurfKdMap(int i){
return SurfKdMap_last[i];
}
pcl::KdTreeFLANN<PointType> getNonFeatureKdMap(int i){
return NonFeatureKdMap_last[i];
}
pcl::PointCloud<PointType>::Ptr get_corner_map(){
return laserCloudCornerFromMap;
}
pcl::PointCloud<PointType>::Ptr get_surf_map(){
return laserCloudSurfFromMap;
}
pcl::PointCloud<PointType>::Ptr get_nonfeature_map(){
return laserCloudNonFeatureFromMap;
}
int get_map_current_pos(){
return currentUpdatePos;
}
int get_laserCloudCenWidth_last(){
return laserCloudCenWidth_last;
}
int get_laserCloudCenHeight_last(){
return laserCloudCenHeight_last;
}
int get_laserCloudCenDepth_last(){
return laserCloudCenDepth_last;
}
pcl::PointCloud<PointType> laserCloudSurf_for_match[4851];
pcl::PointCloud<PointType> laserCloudCorner_for_match[4851];
pcl::PointCloud<PointType> laserCloudNonFeature_for_match[4851];
private:
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
int laserCloudCenWidth_last = 10;
int laserCloudCenHeight_last = 5;
int laserCloudCenDepth_last = 10;
static const int laserCloudWidth = 21;
static const int laserCloudHeight = 11;
static const int laserCloudDepth = 21;
static const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudCornerArrayStack[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArrayStack[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureArrayStack[laserCloudNum];
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterNonFeature;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureFromMap;
pcl::KdTreeFLANN<PointType>::Ptr laserCloudCornerKdMap[laserCloudNum];
pcl::KdTreeFLANN<PointType>::Ptr laserCloudSurfKdMap[laserCloudNum];
pcl::KdTreeFLANN<PointType>::Ptr laserCloudNonFeatureKdMap[laserCloudNum];
pcl::KdTreeFLANN<PointType> CornerKdMap_copy[laserCloudNum];
pcl::KdTreeFLANN<PointType> SurfKdMap_copy[laserCloudNum];
pcl::KdTreeFLANN<PointType> NonFeatureKdMap_copy[laserCloudNum];
pcl::KdTreeFLANN<PointType> CornerKdMap_last[laserCloudNum];
pcl::KdTreeFLANN<PointType> SurfKdMap_last[laserCloudNum];
pcl::KdTreeFLANN<PointType> NonFeatureKdMap_last[laserCloudNum];
static const int localMapWindowSize = 60;
pcl::PointCloud<PointType>::Ptr localCornerMap[localMapWindowSize];
pcl::PointCloud<PointType>::Ptr localSurfMap[localMapWindowSize];
pcl::PointCloud<PointType>::Ptr localNonFeatureMap[localMapWindowSize];
int localMapID = 0;
int currentUpdatePos = 0;
int estimatorPos = 0;
};
#endif //LIO_LIVOX_MAP_MANAGER_H
================================================
FILE: LIO-Livox/include/segment/pointsCorrect.hpp
================================================
#ifndef _COMMON_HPP
#define _CONNON_HPP
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/common.h>
#include <Eigen/Dense>
#include <vector>
using namespace std;
typedef struct
{
Eigen::Matrix3f eigenVectorsPCA;
Eigen::Vector3f eigenValuesPCA;
std::vector<int> neibors;
} SNeiborPCA_cor;
int GetNeiborPCA_cor(SNeiborPCA_cor &npca, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> kdtree, pcl::PointXYZ searchPoint, float fSearchRadius);
int FilterGndForPos_cor(float* outPoints,float*inPoints,int inNum);
int CalGndPos_cor(float *gnd, float *fPoints,int pointNum,float fSearchRadius);
int GetRTMatrix_cor(float *RTM, float *v0, float *v1);
int CorrectPoints_cor(float *fPoints,int pointNum,float *gndPos);
int GetGndPos(float *pos, float *fPoints,int pointNum);
#endif
================================================
FILE: LIO-Livox/include/segment/segment.hpp
================================================
#ifndef _SEGMENT_HPP
#define _SEGMENT_HPP
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/common.h>
#include <Eigen/Dense>
#include <vector>
#include "pointsCorrect.hpp"
using namespace std;
#define SELF_CALI_FRAMES 20
#define GND_IMG_NX 150
#define GND_IMG_NY 400
#define GND_IMG_DX 0.2
#define GND_IMG_DY 0.2
#define GND_IMG_OFFX 40
#define GND_IMG_OFFY 40
#define GND_IMG_NX1 24
#define GND_IMG_NY1 20
#define GND_IMG_DX1 4
#define GND_IMG_DY1 4
#define GND_IMG_OFFX1 40
#define GND_IMG_OFFY1 40
#define DN_SAMPLE_IMG_NX 600 //(GND_IMG_NX)
#define DN_SAMPLE_IMG_NY 200 //(GND_IMG_NY)
#define DN_SAMPLE_IMG_NZ 100
#define DN_SAMPLE_IMG_DX 0.4 //(GND_IMG_DX)
#define DN_SAMPLE_IMG_DY 0.4 //(GND_IMG_DY)
#define DN_SAMPLE_IMG_DZ 0.2
#define DN_SAMPLE_IMG_OFFX 40 //(GND_IMG_OFFX)
#define DN_SAMPLE_IMG_OFFY 40 //(GND_IMG_OFFY)
#define DN_SAMPLE_IMG_OFFZ 2.5//2.5
#define FREE_ANG_NUM 360
#define FREE_PI (3.14159265)
#define FREE_DELTA_ANG (FREE_PI*2/FREE_ANG_NUM)
typedef struct
{
Eigen::Matrix3f eigenVectorsPCA;
Eigen::Vector3f eigenValuesPCA;
std::vector<int> neibors;
} SNeiborPCA;
typedef struct
{
// basic
int pnum;
float xmin;
float xmax;
float ymin;
float ymax;
float zmin;
float zmax;
float zmean;
// pca
float d0[3];
float d1[3];
float center[3];
float obb[8];
int cls;//类别
} SClusterFeature;
int FilterGndForPos(float* outPoints,float*inPoints,int inNum);
int CalNomarls(float *nvects, float *fPoints,int pointNum,float fSearchRadius);
int CalGndPos(float *gnd, float *fPoints,int pointNum,float fSearchRadius);
int GetNeiborPCA(SNeiborPCA &npca, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree, pcl::PointXYZ searchPoint, float fSearchRadius);
int CorrectPoints(float *fPoints,int pointNum,float *gndPos);
// 地面上物体分割
int AbvGndSeg(int *pLabel, float *fPoints, int pointNum);
int SegBG(int *pLabel, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> &kdtree, float fSearchRadius);
SClusterFeature FindACluster(int *pLabel, int seedId, int labelId, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> &kdtree, float fSearchRadius, float thrHeight);
int SegObjects(int *pLabel, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> &kdtree, float fSearchRadius);
int CalFreeRegion(float *pFreeDis, float *fPoints,int *pLabel,int pointNum);
int FreeSeg(float *fPoints,int *pLabel,int pointNum);
int CompleteObjects(int *pLabel, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> &kdtree, float fSearchRadius);
int ExpandObjects(int *pLabel, float* fPoints, int pointNum, float fSearchRadius);
int ExpandBG(int *pLabel, float* fPoints, int pointNum, float fSearchRadius);
// 地面分割
int GndSeg(int* pLabel,float *fPoints,int pointNum,float fSearchRadius);
class PCSeg
{
public:
PCSeg();
// functions
int DoSeg(int *pLabel, float* fPoints1, int pointNum);
int GetMainVectors(float*fPoints, int* pLabel, int pointNum);
int EncodeFeatures(float *pFeas);
int DoBoundaryDetection(float* fPoints1,int *pLabel1,int pointNum);
int DoTrafficLineDet(float *fPoints1,int *pLabel1,int pointNum);
int CorrectPoints(float *fPoints,int pointNum,float *gndPos);
float *PrePoints;
int numPrePoints = 0;
float gnd_pos[100*6];
int gnd_vet_len = 0;
int laneType=0;
float lanePosition[2] = {0};
// vars
unsigned char *pVImg;
float gndPos[6];
int posFlag;
// cluster features
float pVectors[256*3];
float pCenters[256*3];//重心
int pnum[256];
int objClass[256];
float zs[256];
float pOBBs[256*8];
float CBBox[256*7];//（a,x,y,z,l,w,h）
int clusterNum;
float *corPoints;
int corNum;
~PCSeg();
};
SClusterFeature CalBBox(float *fPoints,int pointNum);
SClusterFeature CalOBB(float *fPoints,int pointNum);
#endif
================================================
FILE: LIO-Livox/include/sophus/average.hpp
================================================
/// @file
/// Calculation of biinvariant means.
#ifndef SOPHUS_AVERAGE_HPP
#define SOPHUS_AVERAGE_HPP
#include "common.hpp"
#include "rxso2.hpp"
#include "rxso3.hpp"
#include "se2.hpp"
#include "se3.hpp"
#include "sim2.hpp"
#include "sim3.hpp"
#include "so2.hpp"
#include "so3.hpp"
namespace Sophus {
/// Calculates mean iteratively.
///
/// Returns ``nullopt`` if it does not converge.
///
template <class SequenceContainer>
optional<typename SequenceContainer::value_type> iterativeMean(
SequenceContainer const& foo_Ts_bar, int max_num_iterations) {
size_t N = foo_Ts_bar.size();
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
using Group = typename SequenceContainer::value_type;
using Scalar = typename Group::Scalar;
using Tangent = typename Group::Tangent;
// This implements the algorithm in the beginning of Sec. 4.2 in
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
Group foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
for (int i = 0; i < max_num_iterations; ++i) {
Tangent average;
setToZero<Tangent>(average);
for (Group const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
Group foo_T_newaverage = foo_T_average * Group::exp(average);
if (squaredNorm<Tangent>(
(foo_T_newaverage.inverse() * foo_T_average).log()) <
Constants<Scalar>::epsilon()) {
return foo_T_newaverage;
}
foo_T_average = foo_T_newaverage;
}
// LCOV_EXCL_START
return nullopt;
// LCOV_EXCL_STOP
}
#ifdef DOXYGEN_SHOULD_SKIP_THIS
/// Mean implementation for any Lie group.
template <class SequenceContainer, class Scalar>
optional<typename SequenceContainer::value_type> average(
SequenceContainer const& foo_Ts_bar);
#else
// Mean implementation for SO(2).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SO2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
// This implements rotational part of Proposition 12 from Sec. 6.2 of
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
SO2<Scalar> foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
Scalar average(0);
for (SO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
return foo_T_average * SO2<Scalar>::exp(average);
}
// Mean implementation for RxSO(2).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, RxSO2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
RxSO2<Scalar> foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
Vector2<Scalar> average(Scalar(0), Scalar(0));
for (RxSO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
return foo_T_average * RxSO2<Scalar>::exp(average);
}
namespace details {
template <class T>
void getQuaternion(T const&);
template <class Scalar>
Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
return R.unit_quaternion();
}
template <class Scalar>
Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
return sR.so3().unit_quaternion();
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
Eigen::Quaternion<Scalar> averageUnitQuaternion(
SequenceContainer const& foo_Ts_bar) {
// This: http://stackoverflow.com/a/27410865/1221742
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
Eigen::Matrix<Scalar, 4, Eigen::Dynamic> Q(4, N);
int i = 0;
Scalar w = Scalar(1. / N);
for (auto const& foo_T_bar : foo_Ts_bar) {
Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs();
++i;
}
Eigen::Matrix<Scalar, 4, 4> QQt = Q * Q.transpose();
// TODO: Figure out why we can't use SelfAdjointEigenSolver here.
Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4> > es(QQt);
std::complex<Scalar> max_eigenvalue = es.eigenvalues()[0];
Eigen::Matrix<std::complex<Scalar>, 4, 1> max_eigenvector =
es.eigenvectors().col(0);
for (int i = 1; i < 4; i++) {
if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) {
max_eigenvalue = es.eigenvalues()[i];
max_eigenvector = es.eigenvectors().col(i);
}
}
Eigen::Quaternion<Scalar> quat;
quat.coeffs() << //
max_eigenvector[0].real(), //
max_eigenvector[1].real(), //
max_eigenvector[2].real(), //
max_eigenvector[3].real();
return quat;
}
} // namespace details
// Mean implementation for SO(3).
//
// TODO: Detect degenerated cases and return nullopt.
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SO3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
return SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar));
}
// Mean implementation for R x SO(3).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, RxSO3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
Scalar scale_sum = Scalar(0);
using std::exp;
using std::log;
for (RxSO3<Scalar> const& foo_T_bar : foo_Ts_bar) {
scale_sum += log(foo_T_bar.scale());
}
return RxSO3<Scalar>(exp(scale_sum / Scalar(N)),
SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar)));
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SE2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
// TODO: Implement Proposition 12 from Sec. 6.2 of
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, Sim2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SE3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, Sim3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
#endif // DOXYGEN_SHOULD_SKIP_THIS
} // namespace Sophus
#endif // SOPHUS_AVERAGE_HPP
================================================
FILE: LIO-Livox/include/sophus/common.hpp
================================================
/// @file
/// Common functionality.
#ifndef SOPHUS_COMMON_HPP
#define SOPHUS_COMMON_HPP
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <random>
#include <type_traits>
#include <Eigen/Core>
#if !defined(SOPHUS_DISABLE_ENSURES)
#include "formatstring.hpp"
#endif //!defined(SOPHUS_DISABLE_ENSURES)
// following boost's assert.hpp
#undef SOPHUS_ENSURE
// ENSURES are similar to ASSERTS, but they are always checked for (including in
// release builds). At the moment there are no ASSERTS in Sophus which should
// only be used for checks which are performance critical.
#ifdef __GNUC__
#define SOPHUS_FUNCTION __PRETTY_FUNCTION__
#elif (_MSC_VER >= 1310)
#define SOPHUS_FUNCTION __FUNCTION__
#else
#define SOPHUS_FUNCTION "unknown"
#endif
// Make sure this compiles with older versions of Eigen which do not have
// EIGEN_DEVICE_FUNC defined.
#ifndef EIGEN_DEVICE_FUNC
#define EIGEN_DEVICE_FUNC
#endif
// NVCC on windows has issues with defaulting the Map specialization constructors, so
// special case that specific platform case.
#if defined(_MSC_VER) && defined(__CUDACC__)
#define SOPHUS_WINDOW_NVCC_FALLBACK
#endif
// Make sure this compiles with older versions of Eigen which do not have
// EIGEN_DEFAULT_COPY_CONSTRUCTOR defined
#ifndef EIGEN_DEFAULT_COPY_CONSTRUCTOR
#if EIGEN_HAS_CXX11 && !defined(SOPHUS_WINDOW_NVCC_FALLBACK)
#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default;
#else
#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS)
#endif
#ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS
#error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS"
#endif
#define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \
EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived)
#endif
#ifndef SOPHUS_INHERIT_ASSIGNMENT_OPERATORS
#ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS
#error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS"
#endif
#define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived)
#endif
#define SOPHUS_FUNC EIGEN_DEVICE_FUNC
#if defined(SOPHUS_DISABLE_ENSURES)
#define SOPHUS_ENSURE(expr, ...) ((void)0)
#elif defined(SOPHUS_ENABLE_ENSURE_HANDLER)
namespace Sophus {
void ensureFailed(char const* function, char const* file, int line,
char const* description);
}
#define SOPHUS_ENSURE(expr, ...) \
((expr) ? ((void)0) \
: ::Sophus::ensureFailed( \
SOPHUS_FUNCTION, __FILE__, __LINE__, \
Sophus::details::FormatString(__VA_ARGS__).c_str()))
#else
// LCOV_EXCL_START
namespace Sophus {
template <class... Args>
SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line,
char const* description, Args&&... args) {
std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
function, file, line);
#ifdef __CUDACC__
std::printf("%s", description);
#else
std::cout << details::FormatString(description, std::forward<Args>(args)...)
<< std::endl;
std::abort();
#endif
}
} // namespace Sophus
// LCOV_EXCL_STOP
#define SOPHUS_ENSURE(expr, ...) \
((expr) ? ((void)0) \
: Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \
##__VA_ARGS__))
#endif
namespace Sophus {
template <class Scalar>
struct Constants {
SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
SOPHUS_FUNC static Scalar epsilonSqrt() {
using std::sqrt;
return sqrt(epsilon());
}
SOPHUS_FUNC static Scalar pi() {
return Scalar(3.141592653589793238462643383279502884);
}
};
template <>
struct Constants<float> {
SOPHUS_FUNC static float constexpr epsilon() {
return static_cast<float>(1e-5);
}
SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
SOPHUS_FUNC static float constexpr pi() {
return 3.141592653589793238462643383279502884f;
}
};
/// Nullopt type of lightweight optional class.
struct nullopt_t {
explicit constexpr nullopt_t() {}
};
constexpr nullopt_t nullopt{};
/// Lightweight optional implementation which requires ``T`` to have a
/// default constructor.
///
/// TODO: Replace with std::optional once Sophus moves to c++17.
///
template <class T>
class optional {
public:
optional() : is_valid_(false) {}
optional(nullopt_t) : is_valid_(false) {}
optional(T const& type) : type_(type), is_valid_(true) {}
explicit operator bool() const { return is_valid_; }
T const* operator->() const {
SOPHUS_ENSURE(is_valid_, "must be valid");
return &type_;
}
T* operator->() {
SOPHUS_ENSURE(is_valid_, "must be valid");
return &type_;
}
T const& operator*() const {
SOPHUS_ENSURE(is_valid_, "must be valid");
return type_;
}
T& operator*() {
SOPHUS_ENSURE(is_valid_, "must be valid");
return type_;
}
private:
T type_;
bool is_valid_;
};
template <bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
template <class G>
struct IsUniformRandomBitGenerator {
static const bool value = std::is_unsigned<typename G::result_type>::value &&
std::is_unsigned<decltype(G::min())>::value &&
std::is_unsigned<decltype(G::max())>::value;
};
} // namespace Sophus
#endif // SOPHUS_COMMON_HPP
================================================
FILE: LIO-Livox/include/sophus/example_ensure_handler.cpp
================================================
#include "common.hpp"
#include <cstdio>
#include <cstdlib>
namespace Sophus {
void ensureFailed(char const* function, char const* file, int line,
char const* description) {
std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
file, function, line);
std::printf("Description: %s\n", description);
std::abort();
}
} // namespace Sophus
================================================
FILE: LIO-Livox/include/sophus/formatstring.hpp
================================================
/// @file
/// FormatString functionality
#ifndef SOPHUS_FORMATSTRING_HPP
#define SOPHUS_FORMATSTRING_HPP
#include <iostream>
namespace Sophus {
namespace details {
// Following: http://stackoverflow.com/a/22759544
template <class T>
class IsStreamable {
private:
template <class TT>
static auto test(int)
-> decltype(std::declval<std::stringstream&>() << std::declval<TT>(),
std::true_type());
template <class>
static auto test(...) -> std::false_type;
public:
static bool const value = decltype(test<T>(0))::value;
};
template <class T>
class ArgToStream {
public:
static void impl(std::stringstream& stream, T&& arg) {
stream << std::forward<T>(arg);
}
};
inline void FormatStream(std::stringstream& stream, char const* text) {
stream << text;
return;
}
// Following: http://en.cppreference.com/w/cpp/language/parameter_pack
template <class T, typename... Args>
void FormatStream(std::stringstream& stream, char const* text, T&& arg,
Args&&... args) {
static_assert(IsStreamable<T>::value,
"One of the args has no ostream overload!");
for (; *text != '\0'; ++text) {
if (*text == '%') {
ArgToStream<T&&>::impl(stream, std::forward<T>(arg));
FormatStream(stream, text + 1, std::forward<Args>(args)...);
return;
}
stream << *text;
}
stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1
<< " args unused.";
return;
}
template <class... Args>
std::string FormatString(char const* text, Args&&... args) {
std::stringstream stream;
FormatStream(stream, text, std::forward<Args>(args)...);
return stream.str();
}
inline std::string FormatString() { return std::string(); }
} // namespace details
} // namespace Sophus
#endif //SOPHUS_FORMATSTRING_HPP
================================================
FILE: LIO-Livox/include/sophus/geometry.hpp
================================================
/// @file
/// Transformations between poses and hyperplanes.
#ifndef GEOMETRY_HPP
#define GEOMETRY_HPP
#include "se2.hpp"
#include "se3.hpp"
#include "so2.hpp"
#include "so3.hpp"
#include "types.hpp"
namespace Sophus {
/// Takes in a rotation ``R_foo_plane`` and returns the corresponding line
/// normal along the y-axis (in reference frame ``foo``).
///
template <class T>
Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
return R_foo_line.matrix().col(1);
}
/// Takes in line normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_line``.
///
/// Precondition: ``normal_foo`` must not be close to zero.
///
template <class T>
SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "%",
normal_foo.transpose());
normal_foo.normalize();
return SO2<T>(normal_foo.y(), -normal_foo.x());
}
/// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane
/// normal along the z-axis
/// (in reference frame ``foo``).
///
template <class T>
Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
return R_foo_plane.matrix().col(2);
}
/// Takes in plane normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_plane``.
///
/// Note: The ``plane`` frame is defined as such that the normal points along
/// the positive z-axis. One can specify hints for the x-axis and y-axis
/// of the ``plane`` frame.
///
/// Preconditions:
/// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to
/// zero.
/// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular.
///
template <class T>
Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0),
T(0)),
Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1),
T(0))) {
SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::epsilon(),
"xDirHint (%) and yDirHint (%) must be perpendicular.",
xDirHint_foo.transpose(), yDirHint_foo.transpose());
using std::abs;
using std::sqrt;
T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm();
T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm();
T const normal_foo_sqr_length = normal_foo.squaredNorm();
SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
xDirHint_foo.transpose());
SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
yDirHint_foo.transpose());
SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "%",
normal_foo.transpose());
Matrix3<T> basis_foo;
basis_foo.col(2) = normal_foo;
if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
xDirHint_foo.normalize();
}
if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
yDirHint_foo.normalize();
}
if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
basis_foo.col(2).normalize();
}
T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo));
T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo));
if (abs_x_dot_z < abs_y_dot_z) {
// basis_foo.z and xDirHint are far from parallel.
basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized();
basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2));
} else {
// basis_foo.z and yDirHint are far from parallel.
basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized();
basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0));
}
T det = basis_foo.determinant();
// sanity check
SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::epsilon(),
"Determinant of basis is not 1, but %. Basis is \n%\n", det,
basis_foo);
return basis_foo;
}
/// Takes in plane normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_plane``.
///
/// See ``rotationFromNormal`` for details.
///
template <class T>
SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
return SO3<T>(rotationFromNormal(normal_foo));
}
/// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in
/// reference frame ``foo``.
///
/// Note: The plane is defined by X-axis of the ``line`` frame.
///
template <class T>
Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation());
}
/// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``.
///
/// Note: The line is defined by X-axis of the frame ``line``.
///
template <class T>
SE2<T> SE2FromLine(Line2<T> const& line_foo) {
T const d = line_foo.offset();
Vector2<T> const n = line_foo.normal();
SO2<T> const R_foo_plane = SO2FromNormal(n);
return SE2<T>(R_foo_plane, -d * n);
}
/// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in
/// reference frame ``foo``.
///
/// Note: The plane is defined by XY-plane of the frame ``plane``.
///
template <class T>
Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation());
}
/// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``.
///
/// Note: The plane is defined by XY-plane of the frame ``plane``.
///
template <class T>
SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
T const d = plane_foo.offset();
Vector3<T> const n = plane_foo.normal();
SO3<T> const R_foo_plane = SO3FromNormal(n);
return SE3<T>(R_foo_plane, -d * n);
}
/// Takes in a hyperplane and returns unique representation by ensuring that the
/// ``offset`` is not negative.
///
template <class T, int N>
Eigen::Hyperplane<T, N> makeHyperplaneUnique(
Eigen::Hyperplane<T, N> const& plane) {
if (plane.offset() >= 0) {
return plane;
}
return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
}
} // namespace Sophus
#endif // GEOMETRY_HPP
================================================
FILE: LIO-Livox/include/sophus/interpolate.hpp
================================================
/// @file
/// Interpolation for Lie groups.
#ifndef SOPHUS_INTERPOLATE_HPP
#define SOPHUS_INTERPOLATE_HPP
#include <Eigen/Eigenvalues>
#include "interpolate_details.hpp"
namespace Sophus {
/// This function interpolates between two Lie group elements ``foo_T_bar``
/// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
///
/// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
/// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
/// ``foo_T_bar``.
///
/// (Since interpolation on Lie groups is inverse-invariant, we can equivalently
/// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
/// return value being ``quiz_T_foo``.)
///
/// Precondition: ``p`` must be in [0, 1].
///
template <class G, class Scalar2 = typename G::Scalar>
enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {
using Scalar = typename G::Scalar;
Scalar inter_p(p);
SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
"p (%) must in [0, 1].");
return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}
} // namespace Sophus
#endif // SOPHUS_INTERPOLATE_HPP
================================================
FILE: LIO-Livox/include/sophus/interpolate_details.hpp
================================================
#ifndef SOPHUS_INTERPOLATE_DETAILS_HPP
#define SOPHUS_INTERPOLATE_DETAILS_HPP
#include "rxso2.hpp"
#include "rxso3.hpp"
#include "se2.hpp"
#include "se3.hpp"
#include "sim2.hpp"
#include "sim3.hpp"
#include "so2.hpp"
#include "so3.hpp"
namespace Sophus {
namespace interp_details {
template <class Group>
struct Traits;
template <class Scalar>
struct Traits<SO2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
using std::abs;
Scalar angle = foo_T_bar.log();
return abs(abs(angle) - Constants<Scalar>::pi()) <
Constants<Scalar>::epsilon();
}
};
template <class Scalar>
struct Traits<RxSO2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
}
};
template <class Scalar>
struct Traits<SO3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
using std::abs;
Scalar angle = foo_T_bar.logAndTheta().theta;
return abs(abs(angle) - Constants<Scalar>::pi()) <
Constants<Scalar>::epsilon();
}
};
template <class Scalar>
struct Traits<RxSO3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
}
};
template <class Scalar>
struct Traits<SE2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
}
};
template <class Scalar>
struct Traits<SE3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
}
};
template <class Scalar>
struct Traits<Sim2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(
foo_T_bar.rxso2().so2());
;
}
};
template <class Scalar>
struct Traits<Sim3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(
foo_T_bar.rxso3().so3());
;
}
};
} // namespace interp_details
} // namespace Sophus
#endif // SOPHUS_INTERPOLATE_DETAILS_HPP
================================================
FILE: LIO-Livox/include/sophus/num_diff.hpp
================================================
/// @file
/// Numerical differentiation using finite differences
#ifndef SOPHUS_NUM_DIFF_HPP
#define SOPHUS_NUM_DIFF_HPP
#include <functional>
#include <type_traits>
#include <utility>
#include "types.hpp"
namespace Sophus {
namespace details {
template <class Scalar>
class Curve {
public:
template <class Fn>
static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) {
using ReturnType = decltype(curve(t));
static_assert(std::is_floating_point<Scalar>::value,
"Scalar must be a floating point type.");
static_assert(IsFloatingPoint<ReturnType>::value,
"ReturnType must be either a floating point scalar, "
"vector or matrix.");
return (curve(t + h) - curve(t - h)) / (Scalar(2) * h);
}
};
template <class Scalar, int N, int M>
class VectorField {
public:
static Eigen::Matrix<Scalar, N, M> num_diff(
std::function<Sophus::Vector<Scalar, N>(Sophus::Vector<Scalar, M>)>
vector_field,
Sophus::Vector<Scalar, M> const& a, Scalar eps) {
static_assert(std::is_floating_point<Scalar>::value,
"Scalar must be a floating point type.");
Eigen::Matrix<Scalar, N, M> J;
Sophus::Vector<Scalar, M> h;
h.setZero();
for (int i = 0; i < M; ++i) {
h[i] = eps;
J.col(i) =
(vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps);
h[i] = Scalar(0);
}
return J;
}
};
template <class Scalar, int N>
class VectorField<Scalar, N, 1> {
public:
static Eigen::Matrix<Scalar, N, 1> num_diff(
std::function<Sophus::Vector<Scalar, N>(Scalar)> vector_field,
Scalar const& a, Scalar eps) {
return details::Curve<Scalar>::num_diff(std::move(vector_field), a, eps);
}
};
} // namespace details
/// Calculates the derivative of a curve at a point ``t``.
///
/// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it
/// returns either a Scalar, a vector or a matrix.
///
template <class Scalar, class Fn>
auto curveNumDiff(Fn curve, Scalar t,
Scalar h = Constants<Scalar>::epsilonSqrt())
-> decltype(details::Curve<Scalar>::num_diff(std::move(curve), t, h)) {
return details::Curve<Scalar>::num_diff(std::move(curve), t, h);
}
/// Calculates the derivative of a vector field at a point ``a``.
///
/// Here, a vector field is a function from a vector space to another vector
/// space.
///
template <class Scalar, int N, int M, class ScalarOrVector, class Fn>
Eigen::Matrix<Scalar, N, M> vectorFieldNumDiff(
Fn vector_field, ScalarOrVector const& a,
Scalar eps = Constants<Scalar>::epsilonSqrt()) {
return details::VectorField<Scalar, N, M>::num_diff(std::move(vector_field),
a, eps);
}
} // namespace Sophus
#endif // SOPHUS_NUM_DIFF_HPP
================================================
FILE: LIO-Livox/include/sophus/rotation_matrix.hpp
================================================
/// @file
/// Rotation matrix helper functions.
#ifndef SOPHUS_ROTATION_MATRIX_HPP
#define SOPHUS_ROTATION_MATRIX_HPP
#include <Eigen/Dense>
#include <Eigen/SVD>
#include "types.hpp"
namespace Sophus {
/// Takes in arbitrary square matrix and returns true if it is
/// orthogonal.
template <class D>
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
using Scalar = typename D::Scalar;
static int const N = D::RowsAtCompileTime;
static int const M = D::ColsAtCompileTime;
static_assert(N == M, "must be a square matrix");
static_assert(N >= 2, "must have compile time dimension >= 2");
return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
Constants<Scalar>::epsilon();
}
/// Takes in arbitrary square matrix and returns true if it is
/// "scaled-orthogonal" with positive determinant.
///
template <class D>
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
using Scalar = typename D::Scalar;
static int const N = D::RowsAtCompileTime;
static int const M = D::ColsAtCompileTime;
using std::pow;
using std::sqrt;
Scalar det = sR.determinant();
if (det <= Scalar(0)) {
return false;
}
Scalar scale_sqr = pow(det, Scalar(2. / N));
static_assert(N == M, "must be a square matrix");
static_assert(N >= 2, "must have compile time dimension >= 2");
return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
.template lpNorm<Eigen::Infinity>() <
sqrt(Constants<Scalar>::epsilon());
}
/// Takes in arbitrary square matrix (2x2 or larger) and returns closest
/// orthogonal matrix with positive determinant.
template <class D>
SOPHUS_FUNC enable_if_t<
std::is_floating_point<typename D::Scalar>::value,
Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
using Scalar = typename D::Scalar;
static int const N = D::RowsAtCompileTime;
static int const M = D::ColsAtCompileTime;
static_assert(N == M, "must be a square matrix");
static_assert(N >= 2, "must have compile time dimension >= 2");
Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(
R, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Determine determinant of orthogonal matrix U*V'.
Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
// Starting from the identity matrix D, set the last entry to d (+1 or
// -1), so that det(U*D*V') = 1.
Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();
Diag(N - 1, N - 1) = d;
return svd.matrixU() * Diag * svd.matrixV().transpose();
}
} // namespace Sophus
#endif // SOPHUS_ROTATION_MATRIX_HPP
================================================
FILE: LIO-Livox/include/sophus/rxso2.hpp
================================================
/// @file
/// Direct product R X SO(2) - rotation and scaling in 2d.
#ifndef SOPHUS_RXSO2_HPP
#define SOPHUS_RXSO2_HPP
#include "so2.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class RxSO2;
using RxSO2d = RxSO2<double>;
using RxSO2f = RxSO2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::RxSO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Sophus::Vector2<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
: traits<Sophus::RxSO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
: traits<Sophus::RxSO2<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// RxSO2 base type - implements RxSO2 class but is storage agnostic
///
/// This class implements the group ``R+ x SO(2)``, the direct product of the
/// group of positive scalar 2x2 matrices (= isomorph to the positive
/// real numbers) and the two-dimensional special orthogonal group SO(2).
/// Geometrically, it is the group of rotation and scaling in two dimensions.
/// As a matrix groups, R+ x SO(2) consists of matrices of the form ``s * R``
/// where ``R`` is an orthogonal matrix with ``det(R) = 1`` and ``s > 0``
/// being a positive real number. In particular, it has the following form:
///
/// | s * cos(theta) s * -sin(theta) |
/// | s * sin(theta) s * cos(theta) |
///
/// where ``theta`` being the rotation angle. Internally, it is represented by
/// the first column of the rotation matrix, or in other words by a non-zero
/// complex number.
///
/// R+ x SO(2) is not compact, but a commutative group. First it is not compact
/// since the scale factor is not bound. Second it is commutative since
/// ``sR(alpha, s1) * sR(beta, s2) = sR(beta, s2) * sR(alpha, s1)``, simply
/// because ``alpha + beta = beta + alpha`` and ``s1 * s2 = s2 * s1`` with
/// ``alpha`` and ``beta`` being rotation angles and ``s1``, ``s2`` being scale
/// factors.
///
/// This class has the explicit class invariant that the scale ``s`` is not
/// too close to zero. Strictly speaking, it must hold that:
///
/// ``complex().norm() >= Constants::epsilon()``.
///
/// In order to obey this condition, group multiplication is implemented with
/// saturation such that a product always has a scale which is equal or greater
/// this threshold.
template <class Derived>
class RxSO2Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using ComplexType = typename Eigen::internal::traits<Derived>::ComplexType;
using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (one for rotation and one for scaling).
static int constexpr DoF = 2;
/// Number of internal parameters used (complex number is a tuple).
static int constexpr num_parameters = 2;
/// Group transformations are 2x2 matrices.
static int constexpr N = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with RxSO2 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using RxSO2Product = RxSO2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
/// For RxSO(2), it simply returns the identity matrix.
///
SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
/// Returns rotation angle.
///
SOPHUS_FUNC Scalar angle() const { return SO2<Scalar>(complex()).log(); }
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC RxSO2<NewScalarType> cast() const {
return RxSO2<NewScalarType>(complex().template cast<NewScalarType>());
}
/// This provides unsafe read/write access to internal data. RxSO(2) is
/// represented by a complex number (two parameters). When using direct
/// write access, the user needs to take care of that the complex number is
/// not set close to zero.
///
/// Note: The first parameter represents the real part, while the
/// second parameter represent the imaginary part.
///
SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); }
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const { return complex().data(); }
/// Returns group inverse.
///
SOPHUS_FUNC RxSO2<Scalar> inverse() const {
Scalar squared_scale = complex().squaredNorm();
return RxSO2<Scalar>(complex().x() / squared_scale,
-complex().y() / squared_scale);
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (scaled rotation matrices) to elements of the tangent
/// space (rotation-vector plus logarithm of scale factor).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of RxSO2.
///
SOPHUS_FUNC Tangent log() const {
using std::log;
Tangent theta_sigma;
theta_sigma[1] = log(scale());
theta_sigma[0] = SO2<Scalar>(complex()).log();
return theta_sigma;
}
/// Returns 2x2 matrix representation of the instance.
///
/// For RxSO2, the matrix representation is an scaled orthogonal matrix ``sR``
/// with ``det(R)=s^2``, thus a scaled rotation matrix ``R`` with scale
/// ``s``.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation sR;
// clang-format off
sR << complex()[0], -complex()[1],
complex()[1], complex()[0];
// clang-format on
return sR;
}
/// Assignment operator.
///
SOPHUS_FUNC RxSO2Base& operator=(RxSO2Base const& other) = default;
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC RxSO2Base<Derived>& operator=(
RxSO2Base<OtherDerived> const& other) {
complex_nonconst() = other.complex();
return *this;
}
/// Group multiplication, which is rotation concatenation and scale
/// multiplication.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived>
SOPHUS_FUNC RxSO2Product<OtherDerived> operator*(
RxSO2Base<OtherDerived> const& other) const {
using ResultT = ReturnScalar<OtherDerived>;
Scalar lhs_real = complex().x();
Scalar lhs_imag = complex().y();
typename OtherDerived::Scalar const& rhs_real = other.complex().x();
typename OtherDerived::Scalar const& rhs_imag = other.complex().y();
/// complex multiplication
typename RxSO2Product<OtherDerived>::ComplexType result_complex(
lhs_real * rhs_real - lhs_imag * rhs_imag,
lhs_real * rhs_imag + lhs_imag * rhs_real);
const ResultT squared_scale = result_complex.squaredNorm();
if (squared_scale <
Constants<ResultT>::epsilon() * Constants<ResultT>::epsilon()) {
/// Saturation to ensure class invariant.
result_complex.normalize();
result_complex *= Constants<ResultT>::epsilon();
}
return RxSO2Product<OtherDerived>(result_complex);
}
/// Group action on 2-points.
///
/// This function rotates a 2 dimensional point ``p`` by the SO2 element
/// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor ``s``:
///
/// ``p_bar = s * (bar_R_foo * p_foo)``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return matrix() * p;
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rsp = *this * p.template head<2>();
return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), p(2));
}
/// Group action on lines.
///
/// This function rotates a parameterized line ``l(t) = o + t * d`` by the SO2
/// element and scales it by the scale factor
///
/// Origin ``o`` is rotated and scaled
/// Direction ``d`` is rotated (preserving it's norm)
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(), (*this) * l.direction() / scale());
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SO2's Scalar type.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC RxSO2Base<Derived>& operator*=(
RxSO2Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns internal parameters of RxSO(2).
///
/// It returns (c[0], c[1]), with c being the complex number.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
return complex();
}
/// Sets non-zero complex
///
/// Precondition: ``z`` must not be close to zero.
SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
SOPHUS_ENSURE(z.squaredNorm() > Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
static_cast<Derived*>(this)->complex_nonconst() = z;
}
/// Accessor of complex.
///
SOPHUS_FUNC ComplexType const& complex() const {
return static_cast<Derived const*>(this)->complex();
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Transformation rotationMatrix() const {
ComplexTemporaryType norm_quad = complex();
norm_quad.normalize();
return SO2<Scalar>(norm_quad).matrix();
}
/// Returns scale.
///
SOPHUS_FUNC
Scalar scale() const { return complex().norm(); }
/// Setter of rotation angle, leaves scale as is.
///
SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(theta)); }
/// Setter of complex using rotation matrix ``R``, leaves scale as is.
///
/// Precondition: ``R`` must be orthogonal with determinant of one.
///
SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
setSO2(SO2<Scalar>(R));
}
/// Sets scale and leaves rotation as is.
///
SOPHUS_FUNC void setScale(Scalar const& scale) {
using std::sqrt;
complex_nonconst().normalize();
complex_nonconst() *= scale;
}
/// Setter of complex number using scaled rotation matrix ``sR``.
///
/// Precondition: The 2x2 matrix must be "scaled orthogonal"
/// and have a positive determinant.
///
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR),
"sR must be scaled orthogonal:\n %", sR);
complex_nonconst() = sR.col(0);
}
/// Setter of SO(2) rotations, leaves scale as is.
///
SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
using std::sqrt;
Scalar saved_scale = scale();
complex_nonconst() = so2.unit_complex();
complex_nonconst() *= saved_scale;
}
SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
protected:
/// Mutator of complex is private to ensure class invariant.
///
SOPHUS_FUNC ComplexType& complex_nonconst() {
return static_cast<Derived*>(this)->complex_nonconst();
}
};
/// RxSO2 using storage; derived from RxSO2Base.
template <class Scalar_, int Options>
class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
public:
using Base = RxSO2Base<RxSO2<Scalar_, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using ComplexMember = Eigen::Matrix<Scalar, 2, 1, Options>;
/// ``Base`` is friend so complex_nonconst can be accessed from ``Base``.
friend class RxSO2Base<RxSO2<Scalar_, Options>>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes complex number to identity rotation and
/// scale to 1.
///
SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {}
/// Copy constructor
///
SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
/// Copy-like constructor from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
: complex_(other.complex()) {}
/// Constructor from scaled rotation matrix
///
/// Precondition: rotation matrix need to be scaled orthogonal with
/// determinant of ``s^2``.
///
SOPHUS_FUNC explicit RxSO2(Transformation const& sR) {
this->setScaledRotationMatrix(sR);
}
/// Constructor from scale factor and rotation matrix ``R``.
///
/// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant
/// of 1 and ``scale`` must to be close to zero.
///
SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
: RxSO2((scale * SO2<Scalar>(R).unit_complex()).eval()) {}
/// Constructor from scale factor and SO2
///
/// Precondition: ``scale`` must be close to zero.
///
SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
: RxSO2((scale * so2.unit_complex()).eval()) {}
/// Constructor from complex number.
///
/// Precondition: complex number must not be close to zero.
///
SOPHUS_FUNC explicit RxSO2(Vector2<Scalar> const& z) : complex_(z) {
SOPHUS_ENSURE(complex_.squaredNorm() >= Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon: % vs %",
complex_.squaredNorm(),
Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon());
}
/// Constructor from complex number.
///
/// Precondition: complex number must not be close to zero.
///
SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag)
: RxSO2(Vector2<Scalar>(real, imag)) {}
/// Accessor of complex.
///
SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Group exponential
///
/// This functions takes in an element of tangent space (= rotation angle
/// plus logarithm of scale) and returns the corresponding element of the
/// group RxSO2.
///
/// To be more specific, this function computes ``expmat(hat(theta))``
/// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
/// hat()-operator of RSO2.
///
SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) {
using std::exp;
Scalar const theta = a[0];
Scalar const sigma = a[1];
Scalar s = exp(sigma);
Vector2<Scalar> z = SO2<Scalar>::exp(theta).unit_complex();
z *= s;
return RxSO2<Scalar>(z);
}
/// Returns the ith infinitesimal generators of ``R+ x SO(2)``.
///
/// The infinitesimal generators of RxSO2 are:
///
/// ```
/// | 0 -1 |
/// G_0 = | 1 0 |
///
/// | 1 0 |
/// G_1 = | 0 1 |
/// ```
///
/// Precondition: ``i`` must be 0, or 1.
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 && i <= 1, "i should be 0 or 1.");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// hat-operator
///
/// It takes in the 2-vector representation ``a`` (= rotation angle plus
/// logarithm of scale) and returns the corresponding matrix representation
/// of Lie algebra element.
///
/// Formally, the hat()-operator of RxSO2 is defined as
///
/// ``hat(.): R^2 -> R^{2x2}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2)
///
/// with ``G_i`` being the ith infinitesimal generator of RxSO2.
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation A;
// clang-format off
A << a(1), -a(0),
a(0), a(1);
// clang-format on
return A;
}
/// Lie bracket
///
/// It computes the Lie bracket of RxSO(2). To be more specific, it computes
///
/// ``[omega_1, omega_2]_rxso2 := vee([hat(omega_1), hat(omega_2)])``
///
/// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
/// hat()-operator and ``vee(.)`` the vee()-operator of RxSO2.
///
SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
Vector2<Scalar> res;
res.setZero();
return res;
}
/// Draw uniform sample from RxSO(2) manifold.
///
/// The scale factor is drawn uniformly in log2-space from [-1, 1],
/// hence the scale is in [0.5, 2)].
///
template <class UniformRandomBitGenerator>
static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
using std::exp2;
return RxSO2(exp2(uniform(generator)),
SO2<Scalar>::sampleUniform(generator));
}
/// vee-operator
///
/// It takes the 2x2-matrix representation ``Omega`` and maps it to the
/// corresponding vector representation of Lie algebra.
///
/// This is the inverse of the hat()-operator, see above.
///
/// Precondition: ``Omega`` must have the following structure:
///
/// | d -x |
/// | x d |
///
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
using std::abs;
return Tangent(Omega(1, 0), Omega(0, 0));
}
protected:
SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
ComplexMember complex_;
};
} // namespace Sophus
namespace Eigen {
/// Specialization of Eigen::Map for ``RxSO2``; derived from RxSO2Base.
///
/// Allows us to wrap RxSO2 objects around POD array (e.g. external z style
/// complex).
template <class Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_>, Options>
: public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>> {
using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
/// ``Base`` is friend so complex_nonconst can be accessed from ``Base``.
friend class Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>>;
// LCOV_EXCL_START
SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map);
// LCOV_EXCL_STOP
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar* coeffs) : complex_(coeffs) {}
/// Accessor of complex.
///
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar>, Options> const& complex() const {
return complex_;
}
protected:
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& complex_nonconst() {
return complex_;
}
Map<Sophus::Vector2<Scalar>, Options> complex_;
};
/// Specialization of Eigen::Map for ``RxSO2 const``; derived from RxSO2Base.
///
/// Allows us to wrap RxSO2 objects around POD array (e.g. external z style
/// complex).
template <class Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_> const, Options>
: public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>> {
public:
using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
Map(Scalar const* coeffs) : complex_(coeffs) {}
/// Accessor of complex.
///
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar> const, Options> const& complex() const {
return complex_;
}
protected:
Map<Sophus::Vector2<Scalar> const, Options> const complex_;
};
} // namespace Eigen
#endif /// SOPHUS_RXSO2_HPP
================================================
FILE: LIO-Livox/include/sophus/rxso3.hpp
================================================
/// @file
/// Direct product R X SO(3) - rotation and scaling in 3d.
#ifndef SOPHUS_RXSO3_HPP
#define SOPHUS_RXSO3_HPP
#include "so3.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class RxSO3;
using RxSO3d = RxSO3<double>;
using RxSO3f = RxSO3<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::RxSO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Eigen::Quaternion<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
: traits<Sophus::RxSO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
: traits<Sophus::RxSO3<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// RxSO3 base type - implements RxSO3 class but is storage agnostic
///
/// This class implements the group ``R+ x SO(3)``, the direct product of the
/// group of positive scalar 3x3 matrices (= isomorph to the positive
/// real numbers) and the three-dimensional special orthogonal group SO(3).
/// Geometrically, it is the group of rotation and scaling in three dimensions.
/// As a matrix groups, RxSO3 consists of matrices of the form ``s * R``
/// where ``R`` is an orthogonal matrix with ``det(R)=1`` and ``s > 0``
/// being a positive real number.
///
/// Internally, RxSO3 is represented by the group of non-zero quaternions.
/// In particular, the scale equals the squared(!) norm of the quaternion ``q``,
/// ``s = |q|^2``. This is a most compact representation since the degrees of
/// freedom (DoF) of RxSO3 (=4) equals the number of internal parameters (=4).
///
/// This class has the explicit class invariant that the scale ``s`` is not
/// too close to zero. Strictly speaking, it must hold that:
///
/// ``quaternion().squaredNorm() >= Constants::epsilon()``.
///
/// In order to obey this condition, group multiplication is implemented with
/// saturation such that a product always has a scale which is equal or greater
/// this threshold.
template <class Derived>
class RxSO3Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using QuaternionType =
typename Eigen::internal::traits<Derived>::QuaternionType;
using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (three for rotation and one for scaling).
static int constexpr DoF = 4;
/// Number of internal parameters used (quaternion is a 4-tuple).
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
struct TangentAndTheta {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Tangent tangent;
Scalar theta;
};
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with RxSO3 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using RxSO3Product = RxSO3<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector3<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
/// For RxSO(3), it simply returns the rotation matrix corresponding to ``A``.
///
SOPHUS_FUNC Adjoint Adj() const {
Adjoint res;
res.setIdentity();
res.template topLeftCorner<3, 3>() = rotationMatrix();
return res;
}
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC RxSO3<NewScalarType> cast() const {
return RxSO3<NewScalarType>(quaternion().template cast<NewScalarType>());
}
/// This provides unsafe read/write access to internal data. RxSO(3) is
/// represented by an Eigen::Quaternion (four parameters). When using direct
/// write access, the user needs to take care of that the quaternion is not
/// set close to zero.
///
/// Note: The first three Scalars represent the imaginary parts, while the
/// forth Scalar represent the real part.
///
SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); }
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const {
return quaternion().coeffs().data();
}
/// Returns group inverse.
///
SOPHUS_FUNC RxSO3<Scalar> inverse() const {
return RxSO3<Scalar>(quaternion().inverse());
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (scaled rotation matrices) to elements of the tangent
/// space (rotation-vector plus logarithm of scale factor).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of RxSO3.
///
SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
/// As above, but also returns ``theta = |omega|``.
///
SOPHUS_FUNC TangentAndTheta logAndTheta() const {
using std::log;
Scalar scale = quaternion().squaredNorm();
TangentAndTheta result;
result.tangent[3] = log(scale);
auto omega_and_theta = SO3<Scalar>(quaternion()).logAndTheta();
result.tangent.template head<3>() = omega_and_theta.tangent;
result.theta = omega_and_theta.theta;
return result;
}
/// Returns 3x3 matrix representation of the instance.
///
/// For RxSO3, the matrix representation is an scaled orthogonal matrix ``sR``
/// with ``det(R)=s^3``, thus a scaled rotation matrix ``R`` with scale
/// ``s``.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation sR;
Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x();
Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y();
Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z();
Scalar const w_sq = quaternion().w() * quaternion().w();
Scalar const two_vx = Scalar(2) * quaternion().vec().x();
Scalar const two_vy = Scalar(2) * quaternion().vec().y();
Scalar const two_vz = Scalar(2) * quaternion().vec().z();
Scalar const two_vx_vy = two_vx * quaternion().vec().y();
Scalar const two_vx_vz = two_vx * quaternion().vec().z();
Scalar const two_vx_w = two_vx * quaternion().w();
Scalar const two_vy_vz = two_vy * quaternion().vec().z();
Scalar const two_vy_w = two_vy * quaternion().w();
Scalar const two_vz_w = two_vz * quaternion().w();
sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq;
sR(1, 0) = two_vx_vy + two_vz_w;
sR(2, 0) = two_vx_vz - two_vy_w;
sR(0, 1) = two_vx_vy - two_vz_w;
sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq;
sR(2, 1) = two_vx_w + two_vy_vz;
sR(0, 2) = two_vx_vz + two_vy_w;
sR(1, 2) = -two_vx_w + two_vy_vz;
sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq;
return sR;
}
/// Assignment operator.
///
SOPHUS_FUNC RxSO3Base& operator=(RxSO3Base const& other) = default;
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC RxSO3Base<Derived>& operator=(
RxSO3Base<OtherDerived> const& other) {
quaternion_nonconst() = other.quaternion();
return *this;
}
/// Group multiplication, which is rotation concatenation and scale
/// multiplication.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived>
SOPHUS_FUNC RxSO3Product<OtherDerived> operator*(
RxSO3Base<OtherDerived> const& other) const {
using ResultT = ReturnScalar<OtherDerived>;
typename RxSO3Product<OtherDerived>::QuaternionType result_quaternion(
quaternion() * other.quaternion());
ResultT scale = result_quaternion.squaredNorm();
if (scale < Constants<ResultT>::epsilon()) {
SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero.");
/// Saturation to ensure class invariant.
result_quaternion.normalize();
result_quaternion.coeffs() *= sqrt(Constants<Scalar>::epsilon());
}
return RxSO3Product<OtherDerived>(result_quaternion);
}
/// Group action on 3-points.
///
/// This function rotates a 3 dimensional point ``p`` by the SO3 element
/// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor
/// ``s``:
///
/// ``p_bar = s * (bar_R_foo * p_foo)``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 3>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
// Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459
Scalar scale = quaternion().squaredNorm();
PointProduct<PointDerived> two_vec_cross_p = quaternion().vec().cross(p);
two_vec_cross_p += two_vec_cross_p;
return scale * p + (quaternion().w() * two_vec_cross_p +
quaternion().vec().cross(two_vec_cross_p));
}
/// Group action on homogeneous 3-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 4>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rsp = *this * p.template head<3>();
return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), rsp(2), p(3));
}
/// Group action on lines.
///
/// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3
/// element ans scales it by the scale factor:
///
/// Origin ``o`` is rotated and scaled
/// Direction ``d`` is rotated (preserving it's norm)
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(),
(*this) * l.direction() / quaternion().squaredNorm());
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SO3's Scalar type.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC RxSO3Base<Derived>& operator*=(
RxSO3Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns internal parameters of RxSO(3).
///
/// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the
/// quaternion.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
return quaternion().coeffs();
}
/// Sets non-zero quaternion
///
/// Precondition: ``quat`` must not be close to zero.
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
static_cast<Derived*>(this)->quaternion_nonconst() = quat;
}
/// Accessor of quaternion.
///
SOPHUS_FUNC QuaternionType const& quaternion() const {
return static_cast<Derived const*>(this)->quaternion();
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Transformation rotationMatrix() const {
QuaternionTemporaryType norm_quad = quaternion();
norm_quad.normalize();
return norm_quad.toRotationMatrix();
}
/// Returns scale.
///
SOPHUS_FUNC
Scalar scale() const { return quaternion().squaredNorm(); }
/// Setter of quaternion using rotation matrix ``R``, leaves scale as is.
///
SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
using std::sqrt;
Scalar saved_scale = scale();
quaternion_nonconst() = R;
quaternion_nonconst().coeffs() *= sqrt(saved_scale);
}
/// Sets scale and leaves rotation as is.
///
/// Note: This function as a significant computational cost, since it has to
/// call the square root twice.
///
SOPHUS_FUNC
void setScale(Scalar const& scale) {
using std::sqrt;
quaternion_nonconst().normalize();
quaternion_nonconst().coeffs() *= sqrt(scale);
}
/// Setter of quaternion using scaled rotation matrix ``sR``.
///
/// Precondition: The 3x3 matrix must be "scaled orthogonal"
/// and have a positive determinant.
///
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
Transformation squared_sR = sR * sR.transpose();
Scalar squared_scale =
Scalar(1. / 3.) *
(squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2));
SOPHUS_ENSURE(squared_scale >= Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
Scalar scale = sqrt(squared_scale);
quaternion_nonconst() = sR / scale;
quaternion_nonconst().coeffs() *= sqrt(scale);
}
/// Setter of SO(3) rotations, leaves scale as is.
///
SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
using std::sqrt;
Scalar saved_scale = scale();
quaternion_nonconst() = so3.unit_quaternion();
quaternion_nonconst().coeffs() *= sqrt(saved_scale);
}
SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
protected:
/// Mutator of quaternion is private to ensure class invariant.
///
SOPHUS_FUNC QuaternionType& quaternion_nonconst() {
return static_cast<Derived*>(this)->quaternion_nonconst();
}
};
/// RxSO3 using storage; derived from RxSO3Base.
template <class Scalar_, int Options>
class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
public:
using Base = RxSO3Base<RxSO3<Scalar_, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using QuaternionMember = Eigen::Quaternion<Scalar, Options>;
/// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
friend class RxSO3Base<RxSO3<Scalar_, Options>>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes quaternion to identity rotation and scale
/// to 1.
///
SOPHUS_FUNC RxSO3()
: quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
/// Copy constructor
///
SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
/// Copy-like constructor from OtherDerived
///
template <class OtherDerived>
SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
: quaternion_(other.quaternion()) {}
/// Constructor from scaled rotation matrix
///
/// Precondition: rotation matrix need to be scaled orthogonal with
/// determinant of ``s^3``.
///
SOPHUS_FUNC explicit RxSO3(Transformation const& sR) {
this->setScaledRotationMatrix(sR);
}
/// Constructor from scale factor and rotation matrix ``R``.
///
/// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant
/// of 1 and ``scale`` must not be close to zero.
///
SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
: quaternion_(R) {
SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
using std::sqrt;
quaternion_.coeffs() *= sqrt(scale);
}
/// Constructor from scale factor and SO3
///
/// Precondition: ``scale`` must not to be close to zero.
///
SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
: quaternion_(so3.unit_quaternion()) {
SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
using std::sqrt;
quaternion_.coeffs() *= sqrt(scale);
}
/// Constructor from quaternion
///
/// Precondition: quaternion must not be close to zero.
///
template <class D>
SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
: quaternion_(quat) {
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type.");
SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
}
/// Accessor of quaternion.
///
SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; }
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Group exponential
///
/// This functions takes in an element of tangent space (= rotation 3-vector
/// plus logarithm of scale) and returns the corresponding element of the
/// group RxSO3.
///
/// To be more specific, thixs function computes ``expmat(hat(omega))``
/// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
/// hat()-operator of RSO3.
///
SOPHUS_FUNC static RxSO3<Scalar> exp(Tangent const& a) {
Scalar theta;
return expAndTheta(a, &theta);
}
/// As above, but also returns ``theta = |omega|`` as out-parameter.
///
/// Precondition: ``theta`` must not be ``nullptr``.
///
SOPHUS_FUNC static RxSO3<Scalar> expAndTheta(Tangent const& a,
Scalar* theta) {
SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
using std::exp;
using std::sqrt;
Vector3<Scalar> const omega = a.template head<3>();
Scalar sigma = a[3];
Scalar sqrt_scale = sqrt(exp(sigma));
Eigen::Quaternion<Scalar> quat =
SO3<Scalar>::expAndTheta(omega, theta).unit_quaternion();
quat.coeffs() *= sqrt_scale;
return RxSO3<Scalar>(quat);
}
/// Returns the ith infinitesimal generators of ``R+ x SO(3)``.
///
/// The infinitesimal generators of RxSO3 are:
///
/// ```
/// | 0 0 0 |
/// G_0 = | 0 0 -1 |
/// | 0 1 0 |
///
/// | 0 0 1 |
/// G_1 = | 0 0 0 |
/// | -1 0 0 |
///
/// | 0 -1 0 |
/// G_2 = | 1 0 0 |
/// | 0 0 0 |
///
/// | 1 0 0 |
/// G_2 = | 0 1 0 |
/// | 0 0 1 |
/// ```
///
/// Precondition: ``i`` must be 0, 1, 2 or 3.
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 && i <= 3, "i should be in range [0,3].");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// hat-operator
///
/// It takes in the 4-vector representation ``a`` (= rotation vector plus
/// logarithm of scale) and returns the corresponding matrix representation
/// of Lie algebra element.
///
/// Formally, the hat()-operator of RxSO3 is defined as
///
/// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2,3)
///
/// with ``G_i`` being the ith infinitesimal generator of RxSO3.
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation A;
// clang-format off
A << a(3), -a(2), a(1),
a(2), a(3), -a(0),
-a(1), a(0), a(3);
// clang-format on
return A;
}
/// Lie bracket
///
/// It computes the Lie bracket of RxSO(3). To be more specific, it computes
///
/// ``[omega_1, omega_2]_rxso3 := vee([hat(omega_1), hat(omega_2)])``
///
/// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
/// hat()-operator and ``vee(.)`` the vee()-operator of RxSO3.
///
SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
Vector3<Scalar> const omega1 = a.template head<3>();
Vector3<Scalar> const omega2 = b.template head<3>();
Vector4<Scalar> res;
res.template head<3>() = omega1.cross(omega2);
res[3] = Scalar(0);
return res;
}
/// Draw uniform sample from RxSO(3) manifold.
///
/// The scale factor is drawn uniformly in log2-space from [-1, 1],
/// hence the scale is in [0.5, 2].
///
template <class UniformRandomBitGenerator>
static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
using std::exp2;
return RxSO3(exp2(uniform(generator)),
SO3<Scalar>::sampleUniform(generator));
}
/// vee-operator
///
/// It takes the 3x3-matrix representation ``Omega`` and maps it to the
/// corresponding vector representation of Lie algebra.
///
/// This is the inverse of the hat()-operator, see above.
///
/// Precondition: ``Omega`` must have the following structure:
///
/// | d -c b |
/// | c d -a |
/// | -b a d |
///
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
using std::abs;
return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0));
}
protected:
SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quaternion_; }
QuaternionMember quaternion_;
};
} // namespace Sophus
namespace Eigen {
/// Specialization of Eigen::Map for ``RxSO3``; derived from RxSO3Base
///
/// Allows us to wrap RxSO3 objects around POD array (e.g. external c style
/// quaternion).
template <class Scalar_, int Options>
class Map<Sophus::RxSO3<Scalar_>, Options>
: public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>> {
public:
using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
/// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
friend class Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
// LCOV_EXCL_START
SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map);
// LCOV_EXCL_STOP
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {}
/// Accessor of quaternion.
///
SOPHUS_FUNC
Map<Eigen::Quaternion<Scalar>, Options> const& quaternion() const {
return quaternion_;
}
protected:
SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>& quaternion_nonconst() {
return quaternion_;
}
Map<Eigen::Quaternion<Scalar>, Options> quaternion_;
};
/// Specialization of Eigen::Map for ``RxSO3 const``; derived from RxSO3Base.
///
/// Allows us to wrap RxSO3 objects around POD array (e.g. external c style
/// quaternion).
template <class Scalar_, int Options>
class Map<Sophus::RxSO3<Scalar_> const, Options>
: public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>> {
public:
using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
Map(Scalar const* coeffs) : quaternion_(coeffs) {}
/// Accessor of quaternion.
///
SOPHUS_FUNC
Map<Eigen::Quaternion<Scalar> const, Options> const& quaternion() const {
return quaternion_;
}
protected:
Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_;
};
} // namespace Eigen
#endif /// SOPHUS_RXSO3_HPP
================================================
FILE: LIO-Livox/include/sophus/se2.hpp
================================================
/// @file
/// Special Euclidean group SE(2) - rotation and translation in 2d.
#ifndef SOPHUS_SE2_HPP
#define SOPHUS_SE2_HPP
#include "so2.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class SE2;
using SE2d = SE2<double>;
using SE2f = SE2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector2<Scalar, Options>;
using SO2Type = Sophus::SO2<Scalar, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_>, Options>>
: traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
: traits<Sophus::SE2<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
using SO2Type = Map<Sophus::SO2<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// SE2 base type - implements SE2 class but is storage agnostic.
///
/// SE(2) is the group of rotations and translation in 2d. It is the
/// semi-direct product of SO(2) and the 2d Euclidean vector space. The class
/// is represented using a composition of SO2Group for rotation and a 2-vector
/// for translation.
///
/// SE(2) is neither compact, nor a commutative group.
///
/// See SO2Group for more details of the rotation representation in 2d.
///
template <class Derived>
class SE2Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using SO2Type = typename Eigen::internal::traits<Derived>::SO2Type;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (two for translation, three for rotation).
static int constexpr DoF = 3;
/// Number of internal parameters used (tuple for complex, two for
/// translation).
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with SE2 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using SE2Product = SE2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
SOPHUS_FUNC Adjoint Adj() const {
Matrix<Scalar, 2, 2> const& R = so2().matrix();
Transformation res;
res.setIdentity();
res.template topLeftCorner<2, 2>() = R;
res(0, 2) = translation()[1];
res(1, 2) = -translation()[0];
return res;
}
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC SE2<NewScalarType> cast() const {
return SE2<NewScalarType>(so2().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
/// Returns derivative of this * exp(x) wrt x at x=0.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector2<Scalar> const c = unit_complex();
Scalar o(0);
J(0, 0) = o;
J(0, 1) = o;
J(0, 2) = -c[1];
J(1, 0) = o;
J(1, 1) = o;
J(1, 2) = c[0];
J(2, 0) = c[0];
J(2, 1) = -c[1];
J(2, 2) = o;
J(3, 0) = c[1];
J(3, 1) = c[0];
J(3, 2) = o;
return J;
}
/// Returns group inverse.
///
SOPHUS_FUNC SE2<Scalar> inverse() const {
SO2<Scalar> const invR = so2().inverse();
return SE2<Scalar>(invR, invR * (translation() * Scalar(-1)));
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (rigid body transformations) to elements of the
/// tangent space (twist).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of SE(2).
///
SOPHUS_FUNC Tangent log() const {
using std::abs;
Tangent upsilon_theta;
Scalar theta = so2().log();
upsilon_theta[2] = theta;
Scalar halftheta = Scalar(0.5) * theta;
Scalar halftheta_by_tan_of_halftheta;
Vector2<Scalar> z = so2().unit_complex();
Scalar real_minus_one = z.x() - Scalar(1.);
if (abs(real_minus_one) < Constants<Scalar>::epsilon()) {
halftheta_by_tan_of_halftheta =
Scalar(1.) - Scalar(1. / 12) * theta * theta;
} else {
halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);
}
Matrix<Scalar, 2, 2> V_inv;
V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta,
halftheta_by_tan_of_halftheta;
upsilon_theta.template head<2>() = V_inv * translation();
return upsilon_theta;
}
/// Normalize SO2 element
///
/// It re-normalizes the SO2 element.
///
SOPHUS_FUNC void normalize() { so2().normalize(); }
/// Returns 3x3 matrix representation of the instance.
///
/// It has the following form:
///
/// | R t |
/// | o 1 |
///
/// where ``R`` is a 2x2 rotation matrix, ``t`` a translation 2-vector and
/// ``o`` a 2-column vector of zeros.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation homogenious_matrix;
homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3();
homogenious_matrix.row(2) =
Matrix<Scalar, 1, 3>(Scalar(0), Scalar(0), Scalar(1));
return homogenious_matrix;
}
/// Returns the significant first two rows of the matrix above.
///
SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
Matrix<Scalar, 2, 3> matrix;
matrix.template topLeftCorner<2, 2>() = rotationMatrix();
matrix.col(2) = translation();
return matrix;
}
/// Assignment operator.
///
SOPHUS_FUNC SE2Base& operator=(SE2Base const& other) = default;
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const& other) {
so2() = other.so2();
translation() = other.translation();
return *this;
}
/// Group multiplication, which is rotation concatenation.
///
template <typename OtherDerived>
SOPHUS_FUNC SE2Product<OtherDerived> operator*(
SE2Base<OtherDerived> const& other) const {
return SE2Product<OtherDerived>(
so2() * other.so2(), translation() + so2() * other.translation());
}
/// Group action on 2-points.
///
/// This function rotates and translates a two dimensional point ``p`` by the
/// SE(2) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body
/// transformation):
///
/// ``p_bar = bar_R_foo * p_foo + t_bar``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return so2() * p + translation();
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const PointProduct<HPointDerived> tp =
so2() * p.template head<2>() + p(2) * translation();
return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2));
}
/// Group action on lines.
///
/// This function rotates and translates a parametrized line
/// ``l(t) = o + t * d`` by the SE(2) element:
///
/// Origin ``o`` is rotated and translated using SE(2) action
/// Direction ``d`` is rotated using SO(2) action
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(), so2() * l.direction());
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SO2's Scalar type.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns internal parameters of SE(2).
///
/// It returns (c[0], c[1], t[0], t[1]),
/// with c being the unit complex number, t the translation 3-vector.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
Sophus::Vector<Scalar, num_parameters> p;
p << so2().params(), translation();
return p;
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Matrix<Scalar, 2, 2> rotationMatrix() const {
return so2().matrix();
}
/// Takes in complex number, and normalizes it.
///
/// Precondition: The complex number must not be close to zero.
///
SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
return so2().setComplex(complex);
}
/// Sets ``so3`` using ``rotation_matrix``.
///
/// Precondition: ``R`` must be orthogonal and ``det(R)=1``.
///
SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
R.determinant());
typename SO2Type::ComplexTemporaryType const complex(
Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1)));
so2().setComplex(complex);
}
/// Mutator of SO3 group.
///
SOPHUS_FUNC
SO2Type& so2() { return static_cast<Derived*>(this)->so2(); }
/// Accessor of SO3 group.
///
SOPHUS_FUNC
SO2Type const& so2() const {
return static_cast<Derived const*>(this)->so2();
}
/// Mutator of translation vector.
///
SOPHUS_FUNC
TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
/// Accessor of translation vector
///
SOPHUS_FUNC
TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
/// Accessor of unit complex number.
///
SOPHUS_FUNC
typename Eigen::internal::traits<Derived>::SO2Type::ComplexT const&
unit_complex() const {
return so2().unit_complex();
}
};
/// SE2 using default storage; derived from SE2Base.
template <class Scalar_, int Options>
class SE2 : public SE2Base<SE2<Scalar_, Options>> {
public:
using Base = SE2Base<SE2<Scalar_, Options>>;
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using SO2Member = SO2<Scalar, Options>;
using TranslationMember = Vector2<Scalar, Options>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes rigid body motion to the identity.
///
SOPHUS_FUNC SE2();
/// Copy constructor
///
SOPHUS_FUNC SE2(SE2 const& other) = default;
/// Copy-like constructor from OtherDerived
///
template <class OtherDerived>
SOPHUS_FUNC SE2(SE2Base<OtherDerived> const& other)
: so2_(other.so2()), translation_(other.translation()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from SO3 and translation vector
///
template <class OtherDerived, class D>
SOPHUS_FUNC SE2(SO2Base<OtherDerived> const& so2,
Eigen::MatrixBase<D> const& translation)
: so2_(so2), translation_(translation) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from rotation matrix and translation vector
///
/// Precondition: Rotation matrix needs to be orthogonal with determinant
/// of 1.
///
SOPHUS_FUNC
SE2(typename SO2<Scalar>::Transformation const& rotation_matrix,
Point const& translation)
: so2_(rotation_matrix), translation_(translation) {}
/// Constructor from rotation angle and translation vector.
///
SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation)
: so2_(theta), translation_(translation) {}
/// Constructor from complex number and translation vector
///
/// Precondition: ``complex`` must not be close to zero.
SOPHUS_FUNC SE2(Vector2<Scalar> const& complex, Point const& translation)
: so2_(complex), translation_(translation) {}
/// Constructor from 3x3 matrix
///
/// Precondition: Rotation matrix needs to be orthogonal with determinant
/// of 1. The last row must be ``(0, 0, 1)``.
///
SOPHUS_FUNC explicit SE2(Transformation const& T)
: so2_(T.template topLeftCorner<2, 2>().eval()),
translation_(T.template block<2, 1>(0, 2)) {}
/// This provides unsafe read/write access to internal data. SO(2) is
/// represented by a complex number (two parameters). When using direct write
/// access, the user needs to take care of that the complex number stays
/// normalized.
///
SOPHUS_FUNC Scalar* data() {
// so2_ and translation_ are layed out sequentially with no padding
return so2_.data();
}
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const {
/// so2_ and translation_ are layed out sequentially with no padding
return so2_.data();
}
/// Accessor of SO3
///
SOPHUS_FUNC SO2Member& so2() { return so2_; }
/// Mutator of SO3
///
SOPHUS_FUNC SO2Member const& so2() const { return so2_; }
/// Mutator of translation vector
///
SOPHUS_FUNC TranslationMember& translation() { return translation_; }
/// Accessor of translation vector
///
SOPHUS_FUNC TranslationMember const& translation() const {
return translation_;
}
/// Returns derivative of exp(x) wrt. x.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const& upsilon_theta) {
using std::abs;
using std::cos;
using std::pow;
using std::sin;
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector<Scalar, 2> upsilon = upsilon_theta.template head<2>();
Scalar theta = upsilon_theta[2];
if (abs(theta) < Constants<Scalar>::epsilon()) {
Scalar const o(0);
Scalar const i(1);
// clang-format off
J << o, o, o, o, o, i, i, o, -Scalar(0.5) * upsilon[1], o, i,
Scalar(0.5) * upsilon[0];
// clang-format on
return J;
}
Scalar const c0 = sin(theta);
Scalar const c1 = cos(theta);
Scalar const c2 = 1.0 / theta;
Scalar const c3 = c0 * c2;
Scalar const c4 = -c1 + Scalar(1);
Scalar const c5 = c2 * c4;
Scalar const c6 = c1 * c2;
Scalar const c7 = pow(theta, -2);
Scalar const c8 = c0 * c7;
Scalar const c9 = c4 * c7;
Scalar const o = Scalar(0);
J(0, 0) = o;
J(0, 1) = o;
J(0, 2) = -c0;
J(1, 0) = o;
J(1, 1) = o;
J(1, 2) = c1;
J(2, 0) = c3;
J(2, 1) = -c5;
J(2, 2) =
-c3 * upsilon[1] + c6 * upsilon[0] - c8 * upsilon[0] + c9 * upsilon[1];
J(3, 0) = c5;
J(3, 1) = c3;
J(3, 2) =
c3 * upsilon[0] + c6 * upsilon[1] - c8 * upsilon[1] - c9 * upsilon[0];
return J;
}
/// Returns derivative of exp(x) wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Scalar const o(0);
Scalar const i(1);
// clang-format off
J << o, o, o, o, o, i, i, o, o, o, i, o;
// clang-format on
return J;
}
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Group exponential
///
/// This functions takes in an element of tangent space (= twist ``a``) and
/// returns the corresponding element of the group SE(2).
///
/// The first two components of ``a`` represent the translational part
/// ``upsilon`` in the tangent space of SE(2), while the last three components
/// of ``a`` represents the rotation vector ``omega``.
/// To be more specific, this function computes ``expmat(hat(a))`` with
/// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
/// of SE(2), see below.
///
SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
Scalar theta = a[2];
SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
Scalar sin_theta_by_theta;
Scalar one_minus_cos_theta_by_theta;
using std::abs;
if (abs(theta) < Constants<Scalar>::epsilon()) {
Scalar theta_sq = theta * theta;
sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq;
one_minus_cos_theta_by_theta =
Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
} else {
sin_theta_by_theta = so2.unit_complex().y() / theta;
one_minus_cos_theta_by_theta =
(Scalar(1.) - so2.unit_complex().x()) / theta;
}
Vector2<Scalar> trans(
sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1],
one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]);
return SE2<Scalar>(so2, trans);
}
/// Returns closest SE3 given arbitrary 4x4 matrix.
///
template <class S = Scalar>
static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
fitToSE2(Matrix3<Scalar> const& T) {
return SE2(SO2<Scalar>::fitToSO2(T.template block<2, 2>(0, 0)),
T.template block<2, 1>(0, 2));
}
/// Returns the ith infinitesimal generators of SE(2).
///
/// The infinitesimal generators of SE(2) are:
///
/// ```
/// | 0 0 1 |
/// G_0 = | 0 0 0 |
/// | 0 0 0 |
///
/// | 0 0 0 |
/// G_1 = | 0 0 1 |
/// | 0 0 0 |
///
/// | 0 -1 0 |
/// G_2 = | 1 0 0 |
/// | 0 0 0 |
/// ```
///
/// Precondition: ``i`` must be in 0, 1 or 2.
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2].");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// hat-operator
///
/// It takes in the 3-vector representation (= twist) and returns the
/// corresponding matrix representation of Lie algebra element.
///
/// Formally, the hat()-operator of SE(3) is defined as
///
/// ``hat(.): R^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2)
///
/// with ``G_i`` being the ith infinitesimal generator of SE(2).
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation Omega;
Omega.setZero();
Omega.template topLeftCorner<2, 2>() = SO2<Scalar>::hat(a[2]);
Omega.col(2).template head<2>() = a.template head<2>();
return Omega;
}
/// Lie bracket
///
/// It computes the Lie bracket of SE(2). To be more specific, it computes
///
/// ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])``
///
/// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
/// hat()-operator and ``vee(.)`` the vee()-operator of SE(2).
///
SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
Vector2<Scalar> upsilon1 = a.template head<2>();
Vector2<Scalar> upsilon2 = b.template head<2>();
Scalar theta1 = a[2];
Scalar theta2 = b[2];
return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],
theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0));
}
/// Construct pure rotation.
///
static SOPHUS_FUNC SE2 rot(Scalar const& x) {
return SE2(SO2<Scalar>(x), Sophus::Vector2<Scalar>::Zero());
}
/// Draw uniform sample from SE(2) manifold.
///
/// Translations are drawn component-wise from the range [-1, 1].
///
template <class UniformRandomBitGenerator>
static SE2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
return SE2(SO2<Scalar>::sampleUniform(generator),
Vector2<Scalar>(uniform(generator), uniform(generator)));
}
/// Construct a translation only SE(2) instance.
///
template <class T0, class T1>
static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
return SE2(SO2<Scalar>(), Vector2<Scalar>(x, y));
}
static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {
return SE2(SO2<Scalar>(), xy);
}
/// Construct x-axis translation.
///
static SOPHUS_FUNC SE2 transX(Scalar const& x) {
return SE2::trans(x, Scalar(0));
}
/// Construct y-axis translation.
///
static SOPHUS_FUNC SE2 transY(Scalar const& y) {
return SE2::trans(Scalar(0), y);
}
/// vee-operator
///
/// It takes the 3x3-matrix representation ``Omega`` and maps it to the
/// corresponding 3-vector representation of Lie algebra.
///
/// This is the inverse of the hat()-operator, see above.
///
/// Precondition: ``Omega`` must have the following structure:
///
/// | 0 -d a |
/// | d 0 b |
/// | 0 0 0 |
///
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
SOPHUS_ENSURE(
Omega.row(2).template lpNorm<1>() < Constants<Scalar>::epsilon(),
"Omega: \n%", Omega);
Tangent upsilon_omega;
upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
return upsilon_omega;
}
protected:
SO2Member so2_;
TranslationMember translation_;
};
template <class Scalar, int Options>
SE2<Scalar, Options>::SE2() : translation_(TranslationMember::Zero()) {
static_assert(std::is_standard_layout<SE2>::value,
"Assume standard layout for the use of offsetof check below.");
static_assert(
offsetof(SE2, so2_) + sizeof(Scalar) * SO2<Scalar>::num_parameters ==
offsetof(SE2, translation_),
"This class assumes packed storage and hence will only work "
"correctly depending on the compiler (options) - in "
"particular when using [this->data(), this-data() + "
"num_parameters] to access the raw data in a contiguous fashion.");
}
} // namespace Sophus
namespace Eigen {
/// Specialization of Eigen::Map for ``SE2``; derived from SE2Base.
///
/// Allows us to wrap SE2 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::SE2<Scalar_>, Options>
: public Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>> {
public:
using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
// LCOV_EXCL_START
SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map);
// LCOV_EXCL_STOP
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
Map(Scalar* coeffs)
: so2_(coeffs),
translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
/// Mutator of SO3
///
SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
/// Accessor of SO3
///
SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options> const& so2() const {
return so2_;
}
/// Mutator of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
return translation_;
}
/// Accessor of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
return translation_;
}
protected:
Map<Sophus::SO2<Scalar>, Options> so2_;
Map<Sophus::Vector2<Scalar>, Options> translation_;
};
/// Specialization of Eigen::Map for ``SE2 const``; derived from SE2Base.
///
/// Allows us to wrap SE2 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::SE2<Scalar_> const, Options>
: public Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>> {
public:
using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC Map(Scalar const* coeffs)
: so2_(coeffs),
translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
/// Accessor of SO3
///
SOPHUS_FUNC Map<Sophus::SO2<Scalar> const, Options> const& so2() const {
return so2_;
}
/// Accessor of translation vector
///
SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
const {
return translation_;
}
protected:
Map<Sophus::SO2<Scalar> const, Options> const so2_;
Map<Sophus::Vector2<Scalar> const, Options> const translation_;
};
} // namespace Eigen
#endif
================================================
FILE: LIO-Livox/include/sophus/se3.hpp
================================================
/// @file
/// Special Euclidean group SE(3) - rotation and translation in 3d.
#ifndef SOPHUS_SE3_HPP
#define SOPHUS_SE3_HPP
#include "so3.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class SE3;
using SE3d = SE3<double>;
using SE3f = SE3<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::SE3<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector3<Scalar, Options>;
using SO3Type = Sophus::SO3<Scalar, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE3<Scalar_>, Options>>
: traits<Sophus::SE3<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
using SO3Type = Map<Sophus::SO3<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
: traits<Sophus::SE3<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
using SO3Type = Map<Sophus::SO3<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// SE3 base type - implements SE3 class but is storage agnostic.
///
/// SE(3) is the group of rotations and translation in 3d. It is the
/// semi-direct product of SO(3) and the 3d Euclidean vector space. The class
/// is represented using a composition of SO3 for rotation and a one 3-vector
/// for translation.
///
/// SE(3) is neither compact, nor a commutative group.
///
/// See SO3 for more details of the rotation representation in 3d.
///
template <class Derived>
class SE3Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using SO3Type = typename Eigen::internal::traits<Derived>::SO3Type;
using QuaternionType = typename SO3Type::QuaternionType;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (three for translation, three for rotation).
static int constexpr DoF = 6;
/// Number of internal parameters used (4-tuple for quaternion, three for
/// translation).
static int constexpr num_parameters = 7;
/// Group transformations are 4x4 matrices.
static int constexpr N = 4;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with SE3 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using SE3Product = SE3<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector3<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
SOPHUS_FUNC Adjoint Adj() const {
Sophus::Matrix3<Scalar> const R = so3().matrix();
Adjoint res;
res.block(0, 0, 3, 3) = R;
res.block(3, 3, 3, 3) = R;
res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R;
res.block(3, 0, 3, 3) = Matrix3<Scalar>::Zero(3, 3);
return res;
}
/// Extract rotation angle about canonical X-axis
///
Scalar angleX() const { return so3().angleX(); }
/// Extract rotation angle about canonical Y-axis
///
Scalar angleY() const { return so3().angleY(); }
/// Extract rotation angle about canonical Z-axis
///
Scalar angleZ() const { return so3().angleZ(); }
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC SE3<NewScalarType> cast() const {
return SE3<NewScalarType>(so3().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
/// Returns derivative of this * exp(x) wrt x at x=0.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
Eigen::Quaternion<Scalar> const q = unit_quaternion();
Scalar const c0 = Scalar(0.5) * q.w();
Scalar const c1 = Scalar(0.5) * q.z();
Scalar const c2 = -c1;
Scalar const c3 = Scalar(0.5) * q.y();
Scalar const c4 = Scalar(0.5) * q.x();
Scalar const c5 = -c4;
Scalar const c6 = -c3;
Scalar const c7 = q.w() * q.w();
Scalar const c8 = q.x() * q.x();
Scalar const c9 = q.y() * q.y();
Scalar const c10 = -c9;
Scalar const c11 = q.z() * q.z();
Scalar const c12 = -c11;
Scalar const c13 = Scalar(2) * q.w();
Scalar const c14 = c13 * q.z();
Scalar const c15 = Scalar(2) * q.x();
Scalar const c16 = c15 * q.y();
Scalar const c17 = c13 * q.y();
Scalar const c18 = c15 * q.z();
Scalar const c19 = c7 - c8;
Scalar const c20 = c13 * q.x();
Scalar const c21 = Scalar(2) * q.y() * q.z();
J(0, 0) = 0;
J(0, 1) = 0;
J(0, 2) = 0;
J(0, 3) = c0;
J(0, 4) = c2;
J(0, 5) = c3;
J(1, 0) = 0;
J(1, 1) = 0;
J(1, 2) = 0;
J(1, 3) = c1;
J(1, 4) = c0;
J(1, 5) = c5;
J(2, 0) = 0;
J(2, 1) = 0;
J(2, 2) = 0;
J(2, 3) = c6;
J(2, 4) = c4;
J(2, 5) = c0;
J(3, 0) = 0;
J(3, 1) = 0;
J(3, 2) = 0;
J(3, 3) = c5;
J(3, 4) = c6;
J(3, 5) = c2;
J(4, 0) = c10 + c12 + c7 + c8;
J(4, 1) = -c14 + c16;
J(4, 2) = c17 + c18;
J(4, 3) = 0;
J(4, 4) = 0;
J(4, 5) = 0;
J(5, 0) = c14 + c16;
J(5, 1) = c12 + c19 + c9;
J(5, 2) = -c20 + c21;
J(5, 3) = 0;
J(5, 4) = 0;
J(5, 5) = 0;
J(6, 0) = -c17 + c18;
J(6, 1) = c20 + c21;
J(6, 2) = c10 + c11 + c19;
J(6, 3) = 0;
J(6, 4) = 0;
J(6, 5) = 0;
return J;
}
/// Returns group inverse.
///
SOPHUS_FUNC SE3<Scalar> inverse() const {
SO3<Scalar> invR = so3().inverse();
return SE3<Scalar>(invR, invR * (translation() * Scalar(-1)));
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (rigid body transformations) to elements of the
/// tangent space (twist).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of SE(3).
///
SOPHUS_FUNC Tangent log() const {
// For the derivation of the logarithm of SE(3), see
// J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
// and logarithms of orthogonal matrices", IJRA 2002.
// https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
// (Sec. 6., pp. 8)
using std::abs;
using std::cos;
using std::sin;
Tangent upsilon_omega;
auto omega_and_theta = so3().logAndTheta();
Scalar theta = omega_and_theta.theta;
upsilon_omega.template tail<3>() = omega_and_theta.tangent;
Matrix3<Scalar> const Omega =
SO3<Scalar>::hat(upsilon_omega.template tail<3>());
if (abs(theta) < Constants<Scalar>::epsilon()) {
Matrix3<Scalar> const V_inv = Matrix3<Scalar>::Identity() -
Scalar(0.5) * Omega +
Scalar(1. / 12.) * (Omega * Omega);
upsilon_omega.template head<3>() = V_inv * translation();
} else {
Scalar const half_theta = Scalar(0.5) * theta;
Matrix3<Scalar> const V_inv =
(Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +
(Scalar(1) -
theta * cos(half_theta) / (Scalar(2) * sin(half_theta))) /
(theta * theta) * (Omega * Omega));
upsilon_omega.template head<3>() = V_inv * translation();
}
return upsilon_omega;
}
/// It re-normalizes the SO3 element.
///
/// Note: Because of the class invariant of SO3, there is typically no need to
/// call this function directly.
///
SOPHUS_FUNC void normalize() { so3().normalize(); }
/// Returns 4x4 matrix representation of the instance.
///
/// It has the following form:
///
/// | R t |
/// | o 1 |
///
/// where ``R`` is a 3x3 rotation matrix, ``t`` a translation 3-vector and
/// ``o`` a 3-column vector of zeros.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation homogenious_matrix;
homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
homogenious_matrix.row(3) =
Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
return homogenious_matrix;
}
/// Returns the significant first three rows of the matrix above.
///
SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const {
Matrix<Scalar, 3, 4> matrix;
matrix.template topLeftCorner<3, 3>() = rotationMatrix();
matrix.col(3) = translation();
return matrix;
}
/// Assignment operator.
///
SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) = default;
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC SE3Base<Derived>& operator=(SE3Base<OtherDerived> const& other) {
so3() = other.so3();
translation() = other.translation();
return *this;
}
/// Group multiplication, which is rotation concatenation.
///
template <typename OtherDerived>
SOPHUS_FUNC SE3Product<OtherDerived> operator*(
SE3Base<OtherDerived> const& other) const {
return SE3Product<OtherDerived>(
so3() * other.so3(), translation() + so3() * other.translation());
}
/// Group action on 3-points.
///
/// This function rotates and translates a three dimensional point ``p`` by
/// the SE(3) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body
/// transformation):
///
/// ``p_bar = bar_R_foo * p_foo + t_bar``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 3>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return so3() * p + translation();
}
/// Group action on homogeneous 3-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 4>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const PointProduct<HPointDerived> tp =
so3() * p.template head<3>() + p(3) * translation();
return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3));
}
/// Group action on lines.
///
/// This function rotates and translates a parametrized line
/// ``l(t) = o + t * d`` by the SE(3) element:
///
/// Origin is transformed using SE(3) action
/// Direction is transformed using rotation part
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(), so3() * l.direction());
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SE3's Scalar type.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC SE3Base<Derived>& operator*=(SE3Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const { return so3().matrix(); }
/// Mutator of SO3 group.
///
SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3(); }
/// Accessor of SO3 group.
///
SOPHUS_FUNC SO3Type const& so3() const {
return static_cast<const Derived*>(this)->so3();
}
/// Takes in quaternion, and normalizes it.
///
/// Precondition: The quaternion must not be close to zero.
///
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
so3().setQuaternion(quat);
}
/// Sets ``so3`` using ``rotation_matrix``.
///
/// Precondition: ``R`` must be orthogonal and ``det(R)=1``.
///
SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& R) {
SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
R.determinant());
so3().setQuaternion(Eigen::Quaternion<Scalar>(R));
}
/// Returns internal parameters of SE(3).
///
/// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]),
/// with q being the unit quaternion, t the translation 3-vector.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
Sophus::Vector<Scalar, num_parameters> p;
p << so3().params(), translation();
return p;
}
/// Mutator of translation vector.
///
SOPHUS_FUNC TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
/// Accessor of translation vector
///
SOPHUS_FUNC TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
/// Accessor of unit quaternion.
///
SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
return this->so3().unit_quaternion();
}
};
/// SE3 using default storage; derived from SE3Base.
template <class Scalar_, int Options>
class SE3 : public SE3Base<SE3<Scalar_, Options>> {
using Base = SE3Base<SE3<Scalar_, Options>>;
public:
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using SO3Member = SO3<Scalar, Options>;
using TranslationMember = Vector3<Scalar, Options>;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes rigid body motion to the identity.
///
SOPHUS_FUNC SE3();
/// Copy constructor
///
SOPHUS_FUNC SE3(SE3 const& other) = default;
/// Copy-like constructor from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC SE3(SE3Base<OtherDerived> const& other)
: so3_(other.so3()), translation_(other.translation()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from SO3 and translation vector
///
template <class OtherDerived, class D>
SOPHUS_FUNC SE3(SO3Base<OtherDerived> const& so3,
Eigen::MatrixBase<D> const& translation)
: so3_(so3), translation_(translation) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from rotation matrix and translation vector
///
/// Precondition: Rotation matrix needs to be orthogonal with determinant
/// of 1.
///
SOPHUS_FUNC
SE3(Matrix3<Scalar> const& rotation_matrix, Point const& translation)
: so3_(rotation_matrix), translation_(translation) {}
/// Constructor from quaternion and translation vector.
///
/// Precondition: ``quaternion`` must not be close to zero.
///
SOPHUS_FUNC SE3(Eigen::Quaternion<Scalar> const& quaternion,
Point const& translation)
: so3_(quaternion), translation_(translation) {}
/// Constructor from 4x4 matrix
///
/// Precondition: Rotation matrix needs to be orthogonal with determinant
/// of 1. The last row must be ``(0, 0, 0, 1)``.
///
SOPHUS_FUNC explicit SE3(Matrix4<Scalar> const& T)
: so3_(T.template topLeftCorner<3, 3>()),
translation_(T.template block<3, 1>(0, 3)) {
SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0),
Scalar(0), Scalar(1)))
.squaredNorm() < Constants<Scalar>::epsilon(),
"Last row is not (0,0,0,1), but (%).", T.row(3));
}
/// This provides unsafe read/write access to internal data. SO(3) is
/// represented by an Eigen::Quaternion (four parameters). When using direct
/// write access, the user needs to take care of that the quaternion stays
/// normalized.
///
SOPHUS_FUNC Scalar* data() {
// so3_ and translation_ are laid out sequentially with no padding
return so3_.data();
}
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const {
// so3_ and translation_ are laid out sequentially with no padding
return so3_.data();
}
/// Mutator of SO3
///
SOPHUS_FUNC SO3Member& so3() { return so3_; }
/// Accessor of SO3
///
SOPHUS_FUNC SO3Member const& so3() const { return so3_; }
/// Mutator of translation vector
///
SOPHUS_FUNC TranslationMember& translation() { return translation_; }
/// Accessor of translation vector
///
SOPHUS_FUNC TranslationMember const& translation() const {
return translation_;
}
/// Returns derivative of exp(x) wrt. x.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const& upsilon_omega) {
using std::cos;
using std::pow;
using std::sin;
using std::sqrt;
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector<Scalar, 3> upsilon = upsilon_omega.template head<3>();
Sophus::Vector<Scalar, 3> omega = upsilon_omega.template tail<3>();
Scalar const c0 = omega[0] * omega[0];
Scalar const c1 = omega[1] * omega[1];
Scalar const c2 = omega[2] * omega[2];
Scalar const c3 = c0 + c1 + c2;
Scalar const o(0);
Scalar const h(0.5);
Scalar const i(1);
if (c3 < Constants<Scalar>::epsilon()) {
Scalar const ux = Scalar(0.5) * upsilon[0];
Scalar const uy = Scalar(0.5) * upsilon[1];
Scalar const uz = Scalar(0.5) * upsilon[2];
/// clang-format off
J << o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o,
o, i, o, o, o, uz, -uy, o, i, o, -uz, o, ux, o, o, i, uy, -ux, o;
/// clang-format on
return J;
}
Scalar const c4 = sqrt(c3);
Scalar const c5 = Scalar(1.0) / c4;
Scalar const c6 = Scalar(0.5) * c4;
Scalar const c7 = sin(c6);
Scalar const c8 = c5 * c7;
Scalar const c9 = pow(c3, -3.0L / 2.0L);
Scalar const c10 = c7 * c9;
Scalar const c11 = Scalar(1.0) / c3;
Scalar const c12 = cos(c6);
Scalar const c13 = Scalar(0.5) * c11 * c12;
Scalar const c14 = c7 * c9 * omega[0];
Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
Scalar const c18 = omega[1] * omega[2];
Scalar const c19 = -c10 * c18 + c13 * c18;
Scalar const c20 = c5 * omega[0];
Scalar const c21 = Scalar(0.5) * c7;
Scalar const c22 = c5 * omega[1];
Scalar const c23 = c5 * omega[2];
Scalar const c24 = -c1;
Scalar const c25 = -c2;
Scalar const c26 = c24 + c25;
Scalar const c27 = sin(c4);
Scalar const c28 = -c27 + c4;
Scalar const c29 = c28 * c9;
Scalar const c30 = cos(c4);
Scalar const c31 = -c30 + Scalar(1);
Scalar const c32 = c11 * c31;
Scalar const c33 = c32 * omega[2];
Scalar const c34 = c29 * omega[0];
Scalar const c35 = c34 * omega[1];
Scalar const c36 = c32 * omega[1];
Scalar const c37 = c34 * omega[2];
Scalar const c38 = pow(c3, -5.0L / 2.0L);
Scalar const c39 = Scalar(3) * c28 * c38 * omega[0];
Scalar const c40 = c26 * c9;
Scalar const c41 = -c20 * c30 + c20;
Scalar const c42 = c27 * c9 * omega[0];
Scalar const c43 = c42 * omega[1];
Scalar const c44 = pow(c3, -2);
Scalar const c45 = Scalar(2) * c31 * c44 * omega[0];
Scalar const c46 = c45 * omega[1];
Scalar const c47 = c29 * omega[2];
Scalar const c48 = c43 - c46 + c47;
Scalar const c49 = Scalar(3) * c0 * c28 * c38;
Scalar const c50 = c9 * omega[0] * omega[2];
Scalar const c51 = c41 * c50 - c49 * omega[2];
Scalar const c52 = c9 * omega[0] * omega[1];
Scalar const c53 = c41 * c52 - c49 * omega[1];
Scalar const c54 = c42 * omega[2];
Scalar const c55 = c45 * omega[2];
Scalar const c56 = c29 * omega[1];
Scalar const c57 = -c54 + c55 + c56;
Scalar const c58 = Scalar(-2) * c56;
Scalar const c59 = Scalar(3) * c28 * c38 * omega[1];
Scalar const c60 = -c22 * c30 + c22;
Scalar const c61 = -c18 * c39;
Scalar const c62 = c32 + c61;
Scalar const c63 = c27 * c9;
Scalar const c64 = c1 * c63;
Scalar const c65 = Scalar(2) * c31 * c44;
Scalar const c66 = c1 * c65;
Scalar const c67 = c50 * c60;
S
gitextract_r29jj8ts/
├── .gitignore
├── LICENSE
├── LIO-Livox/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── README.md
│ ├── cmake/
│ │ ├── FindEigen3.cmake
│ │ └── FindSuiteSparse.cmake
│ ├── config/
│ │ └── horizon_config.yaml
│ ├── include/
│ │ ├── Estimator/
│ │ │ └── Estimator.h
│ │ ├── IMUIntegrator/
│ │ │ └── IMUIntegrator.h
│ │ ├── LidarFeatureExtractor/
│ │ │ └── LidarFeatureExtractor.h
│ │ ├── MapManager/
│ │ │ └── Map_Manager.h
│ │ ├── segment/
│ │ │ ├── pointsCorrect.hpp
│ │ │ └── segment.hpp
│ │ ├── sophus/
│ │ │ ├── average.hpp
│ │ │ ├── common.hpp
│ │ │ ├── example_ensure_handler.cpp
│ │ │ ├── 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
│ │ │ ├── test_macros.hpp
│ │ │ ├── types.hpp
│ │ │ └── velocities.hpp
│ │ └── utils/
│ │ ├── ceresfunc.h
│ │ ├── math_utils.hpp
│ │ └── pcl_utils.hpp
│ ├── launch/
│ │ └── livox_odometry.launch
│ ├── package.xml
│ ├── rviz_cfg/
│ │ └── lio.rviz
│ └── src/
│ ├── lio/
│ │ ├── Estimator.cpp
│ │ ├── IMUIntegrator.cpp
│ │ ├── LidarFeatureExtractor.cpp
│ │ ├── Map_Manager.cpp
│ │ ├── PoseEstimation.cpp
│ │ ├── ScanRegistration.cpp
│ │ └── ceresfunc.cpp
│ └── segment/
│ ├── pointsCorrect.cpp
│ └── segment.cpp
├── README.md
├── SC-PGO/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── include/
│ │ ├── livox_mapping/
│ │ │ ├── common.h
│ │ │ └── tic_toc.h
│ │ └── scancontext/
│ │ ├── KDTreeVectorOfVectorsAdaptor.h
│ │ ├── Scancontext.cpp
│ │ ├── Scancontext.h
│ │ └── nanoflann.hpp
│ ├── launch/
│ │ └── livox_mapping.launch
│ ├── package.xml
│ ├── rviz_cfg/
│ │ └── livox_mapping.rviz
│ ├── src/
│ │ └── laserPosegraphOptimization.cpp
│ └── utils/
│ ├── moving_object_removal/
│ │ ├── README.md
│ │ └── move_object_removal_livox.py
│ └── python/
│ ├── bone_table.npy
│ ├── jet_table.npy
│ ├── makeMergedMap.py
│ └── pypcdMyUtils.py
└── ad_localization_msgs/
├── CMakeLists.txt
├── msg/
│ └── NavStateInfo.msg
└── package.xml
SYMBOL INDEX (786 symbols across 42 files)
FILE: LIO-Livox/include/Estimator/Estimator.h
function class (line 25) | class Estimator{
function ComputeError (line 80) | struct FeaturePlan{
function ComputeError (line 100) | struct FeaturePlanVec{
function ComputeError (line 118) | struct FeatureNon{
FILE: LIO-Livox/include/IMUIntegrator/IMUIntegrator.h
function class (line 11) | class IMUIntegrator{
FILE: LIO-Livox/include/LidarFeatureExtractor/LidarFeatureExtractor.h
function class (line 13) | class LidarFeatureExtractor{
FILE: LIO-Livox/include/MapManager/Map_Manager.h
function class (line 8) | class MAP_MANAGER{
FILE: LIO-Livox/include/segment/segment.hpp
class PCSeg (line 112) | class PCSeg
FILE: LIO-Livox/include/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: LIO-Livox/include/sophus/common.hpp
type Sophus (line 77) | namespace Sophus {
function SOPHUS_FUNC (line 92) | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file,...
type Constants (line 116) | struct Constants {
method SOPHUS_FUNC (line 117) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 119) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 124) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 130) | struct Constants<float> {
method epsilon (line 131) | constexpr epsilon() {
method SOPHUS_FUNC (line 135) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 137) | constexpr pi() {
type nullopt_t (line 143) | struct nullopt_t {
method nullopt_t (line 144) | explicit constexpr nullopt_t() {}
class optional (line 155) | class optional {
method optional (line 157) | optional() : is_valid_(false) {}
method optional (line 159) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 161) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 165) | T const* operator->() const {
method T (line 170) | T* operator->() {
method T (line 175) | T const& operator*() const {
method T (line 180) | T& operator*() {
type IsUniformRandomBitGenerator (line 194) | struct IsUniformRandomBitGenerator {
type Sophus (line 90) | namespace Sophus {
function SOPHUS_FUNC (line 92) | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file,...
type Constants (line 116) | struct Constants {
method SOPHUS_FUNC (line 117) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 119) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 124) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 130) | struct Constants<float> {
method epsilon (line 131) | constexpr epsilon() {
method SOPHUS_FUNC (line 135) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 137) | constexpr pi() {
type nullopt_t (line 143) | struct nullopt_t {
method nullopt_t (line 144) | explicit constexpr nullopt_t() {}
class optional (line 155) | class optional {
method optional (line 157) | optional() : is_valid_(false) {}
method optional (line 159) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 161) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 165) | T const* operator->() const {
method T (line 170) | T* operator->() {
method T (line 175) | T const& operator*() const {
method T (line 180) | T& operator*() {
type IsUniformRandomBitGenerator (line 194) | struct IsUniformRandomBitGenerator {
type Sophus (line 113) | namespace Sophus {
function SOPHUS_FUNC (line 92) | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file,...
type Constants (line 116) | struct Constants {
method SOPHUS_FUNC (line 117) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 119) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 124) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 130) | struct Constants<float> {
method epsilon (line 131) | constexpr epsilon() {
method SOPHUS_FUNC (line 135) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 137) | constexpr pi() {
type nullopt_t (line 143) | struct nullopt_t {
method nullopt_t (line 144) | explicit constexpr nullopt_t() {}
class optional (line 155) | class optional {
method optional (line 157) | optional() : is_valid_(false) {}
method optional (line 159) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 161) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 165) | T const* operator->() const {
method T (line 170) | T* operator->() {
method T (line 175) | T const& operator*() const {
method T (line 180) | T& operator*() {
type IsUniformRandomBitGenerator (line 194) | struct IsUniformRandomBitGenerator {
FILE: LIO-Livox/include/sophus/example_ensure_handler.cpp
type Sophus (line 6) | namespace Sophus {
function ensureFailed (line 7) | void ensureFailed(char const* function, char const* file, int line,
FILE: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/sophus/interpolate.hpp
type Sophus (line 11) | namespace Sophus {
function interpolate (line 27) | enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
FILE: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/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: LIO-Livox/include/sophus/so3.hpp
type Sophus (line 17) | namespace Sophus {
class SO3 (line 19) | class SO3
class SO3Base (line 77) | class SO3Base {
type TangentAndTheta (line 98) | struct TangentAndTheta {
method SOPHUS_FUNC (line 130) | SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
method SOPHUS_FUNC (line 135) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX(...
method SOPHUS_FUNC (line 144) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY(...
method SOPHUS_FUNC (line 158) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ(...
method SOPHUS_FUNC (line 167) | SOPHUS_FUNC SO3<NewScalarType> cast() const {
method SOPHUS_FUNC (line 179) | SOPHUS_FUNC Scalar* data() {
method SOPHUS_FUNC (line 185) | SOPHUS_FUNC Scalar const* data() const {
method Dx_this_mul_exp_x_at_0 (line 191) | Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 243) | SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
function SOPHUS_FUNC (line 247) | SOPHUS_FUNC TangentAndTheta logAndTheta() const {
function SOPHUS_FUNC (line 297) | SOPHUS_FUNC void normalize() {
function SOPHUS_FUNC (line 310) | SOPHUS_FUNC Transformation matrix() const {
function SOPHUS_FUNC (line 321) | SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& o...
function SOPHUS_FUNC (line 329) | SOPHUS_FUNC SO3Product<OtherDerived> operator*(
function SOPHUS_FUNC (line 362) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
function SOPHUS_FUNC (line 377) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
function SOPHUS_FUNC (line 390) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 400) | SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& ...
function SOPHUS_FUNC (line 409) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quater...
function SOPHUS_FUNC (line 416) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
function Dxi_exp_x_matrix_at_0 (line 431) | class SO3 : public SO3Base<SO3<Scalar_, Options>> {
function SOPHUS_FUNC (line 576) | SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
function SOPHUS_FUNC (line 585) | SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
function SOPHUS_FUNC (line 626) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3>
function SOPHUS_FUNC (line 651) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 673) | SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
function SOPHUS_FUNC (line 697) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
function SOPHUS_FUNC (line 704) | static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
function SOPHUS_FUNC (line 710) | static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
function SOPHUS_FUNC (line 716) | static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
function SO3 (line 724) | static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 756) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
function SOPHUS_FUNC (line 760) | SOPHUS_FUNC static Transformation JacobianRInv(Tangent const& w) {
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_>>
type traits<Map<Sophus::SO3<Scalar_> const, Options_>> (line 43) | struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>
class Map<Sophus::SO3<Scalar_>, Options> (line 800) | class Map<Sophus::SO3<Scalar_>, Options>
method SOPHUS_FUNC (line 822) | SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
type Sophus (line 52) | namespace Sophus {
class SO3 (line 19) | class SO3
class SO3Base (line 77) | class SO3Base {
type TangentAndTheta (line 98) | struct TangentAndTheta {
method SOPHUS_FUNC (line 130) | SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
method SOPHUS_FUNC (line 135) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX(...
method SOPHUS_FUNC (line 144) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY(...
method SOPHUS_FUNC (line 158) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ(...
method SOPHUS_FUNC (line 167) | SOPHUS_FUNC SO3<NewScalarType> cast() const {
method SOPHUS_FUNC (line 179) | SOPHUS_FUNC Scalar* data() {
method SOPHUS_FUNC (line 185) | SOPHUS_FUNC Scalar const* data() const {
method Dx_this_mul_exp_x_at_0 (line 191) | Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 243) | SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
function SOPHUS_FUNC (line 247) | SOPHUS_FUNC TangentAndTheta logAndTheta() const {
function SOPHUS_FUNC (line 297) | SOPHUS_FUNC void normalize() {
function SOPHUS_FUNC (line 310) | SOPHUS_FUNC Transformation matrix() const {
function SOPHUS_FUNC (line 321) | SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& o...
function SOPHUS_FUNC (line 329) | SOPHUS_FUNC SO3Product<OtherDerived> operator*(
function SOPHUS_FUNC (line 362) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
function SOPHUS_FUNC (line 377) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
function SOPHUS_FUNC (line 390) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 400) | SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& ...
function SOPHUS_FUNC (line 409) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quater...
function SOPHUS_FUNC (line 416) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
function Dxi_exp_x_matrix_at_0 (line 431) | class SO3 : public SO3Base<SO3<Scalar_, Options>> {
function SOPHUS_FUNC (line 576) | SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
function SOPHUS_FUNC (line 585) | SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
function SOPHUS_FUNC (line 626) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3>
function SOPHUS_FUNC (line 651) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 673) | SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
function SOPHUS_FUNC (line 697) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
function SOPHUS_FUNC (line 704) | static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
function SOPHUS_FUNC (line 710) | static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
function SOPHUS_FUNC (line 716) | static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
function SO3 (line 724) | static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 756) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
function SOPHUS_FUNC (line 760) | SOPHUS_FUNC static Transformation JacobianRInv(Tangent const& w) {
type Eigen (line 794) | 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_>>
type traits<Map<Sophus::SO3<Scalar_> const, Options_>> (line 43) | struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>
class Map<Sophus::SO3<Scalar_>, Options> (line 800) | class Map<Sophus::SO3<Scalar_>, Options>
method SOPHUS_FUNC (line 822) | SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
class Map<Sophus::SO3<Scalar_> const, Options> (line 847) | class Map<Sophus::SO3<Scalar_> const, Options>
method SOPHUS_FUNC (line 861) | SOPHUS_FUNC Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {}
FILE: LIO-Livox/include/sophus/test_macros.hpp
type Sophus (line 7) | namespace Sophus {
type details (line 8) | namespace details {
class Pretty (line 11) | class Pretty {
method impl (line 13) | static std::string impl(Scalar s) { return FormatString("%", s); }
class Pretty<Eigen::Matrix<Scalar, M, N>> (line 17) | class Pretty<Eigen::Matrix<Scalar, M, N>> {
method impl (line 19) | static std::string impl(Matrix<Scalar, M, N> const& v) {
function pretty (line 25) | std::string pretty(T const& v) {
function testFailed (line 30) | void testFailed(bool& passed, char const* func, char const* file, in...
function processTestResult (line 39) | void processTestResult(bool passed) {
FILE: LIO-Livox/include/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: LIO-Livox/include/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(
FILE: LIO-Livox/include/utils/ceresfunc.h
type ResidualBlockInfo (line 15) | struct ResidualBlockInfo
type ThreadsStruct (line 78) | struct ThreadsStruct
function class (line 93) | class MarginalizationInfo
function const (line 322) | struct Cost_NavState_PR_Ground
function ceres (line 353) | static ceres::CostFunction *Create(Eigen::VectorXf ground_plane_coeff) {
type Cost_NavState_IMU_Line (line 439) | struct Cost_NavState_IMU_Line
function ceres (line 536) | static ceres::CostFunction *Create(const Eigen::Vector3d& curr_point_,
function const (line 557) | struct Cost_NavState_IMU_Plan_Vec
type Cost_Initialization_IMU (line 693) | struct Cost_Initialization_IMU
function ceres (line 761) | static ceres::CostFunction *Create(IMUIntegrator& measure_,
type Cost_Initialization_Prior_bv (line 783) | struct Cost_Initialization_Prior_bv
type Cost_Initialization_Prior_R (line 820) | struct Cost_Initialization_Prior_R
FILE: LIO-Livox/include/utils/math_utils.hpp
type livox_slam_ware (line 17) | namespace livox_slam_ware {
function skewSymmetric (line 26) | inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) {
function quaternionNormalize (line 43) | inline void quaternionNormalize(Eigen::Vector4d& q) {
function quaternionMultiplication (line 54) | inline Eigen::Vector4d quaternionMultiplication(
function smallAngleQuaternion (line 79) | inline Eigen::Vector4d smallAngleQuaternion(
function getSmallAngleQuaternion (line 107) | inline Eigen::Quaterniond getSmallAngleQuaternion(
function quaternionToRotation (line 139) | inline Eigen::Matrix3d quaternionToRotation(
function rotationToQuaternion (line 163) | inline Eigen::Vector4d rotationToQuaternion(
function R2ypr (line 208) | void R2ypr(const Eigen::Matrix<T, 3, 3>& R, Eigen::Matrix<T, 3, 1> &yp...
function ominus (line 224) | Eigen::Matrix<T, 3, 1> ominus(const Eigen::Matrix<T, 4, 1>& ground_pla...
FILE: LIO-Livox/include/utils/pcl_utils.hpp
type livox_slam_ware (line 6) | namespace livox_slam_ware {
function get_plane_coeffs (line 11) | Eigen::VectorXf get_plane_coeffs (typename pcl::PointCloud<PointType>:...
FILE: LIO-Livox/src/lio/PoseEstimation.cpp
function pubOdometry (line 51) | void pubOdometry(const Eigen::Matrix4d& newPose, double& timefullCloud){
function fullCallBack (line 107) | void fullCallBack(const sensor_msgs::PointCloud2ConstPtr &msg){
function imu_callback (line 113) | void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg){
function fetchImuMsgs (line 124) | bool fetchImuMsgs(double startTime, double endTime, std::vector<sensor_m...
function RemoveLidarDistortion (line 181) | void RemoveLidarDistortion(pcl::PointCloud<PointType>::Ptr& cloud,
function ConvertPointCloudFromOdometerToGround (line 203) | void ConvertPointCloudFromOdometerToGround(pcl::PointCloud<PointType>::P...
function ConvertPointFromLidarToIMU (line 220) | void ConvertPointFromLidarToIMU(PointType const * const pi,
function TryMAPInitialization (line 236) | bool TryMAPInitialization() {
function process (line 426) | void process(){
function main (line 664) | int main(int argc, char** argv)
FILE: LIO-Livox/src/lio/ScanRegistration.cpp
function lidarCallBackHorizon (line 20) | void lidarCallBackHorizon(const livox_ros_driver::CustomMsgConstPtr &msg) {
function main (line 39) | int main(int argc, char** argv)
FILE: LIO-Livox/src/segment/pointsCorrect.cpp
function GetNeiborPCA_cor (line 7) | int GetNeiborPCA_cor(SNeiborPCA_cor &npca, pcl::PointCloud<pcl::PointXYZ...
function FilterGndForPos_cor (line 45) | int FilterGndForPos_cor(float* outPoints,float*inPoints,int inNum)
function CalGndPos_cor (line 122) | int CalGndPos_cor(float *gnd, float *fPoints,int pointNum,float fSearchR...
function GetRTMatrix_cor (line 218) | int GetRTMatrix_cor(float *RTM, float *v0, float *v1)
function CorrectPoints_cor (line 262) | int CorrectPoints_cor(float *fPoints,int pointNum,float *gndPos)
function GetGndPos (line 287) | int GetGndPos(float *pos, float *fPoints,int pointNum){
FILE: LIO-Livox/src/segment/segment.cpp
function FilterGndForPos (line 280) | int FilterGndForPos(float* outPoints,float*inPoints,int inNum)
function CalNomarls (line 348) | int CalNomarls(float *nvects, float *fPoints,int pointNum,float fSearchR...
function CalGndPos (line 425) | int CalGndPos(float *gnd, float *fPoints,int pointNum,float fSearchRadius)
function GetNeiborPCA (line 532) | int GetNeiborPCA(SNeiborPCA &npca, pcl::PointCloud<pcl::PointXYZ>::Ptr c...
function GetRTMatrix (line 572) | int GetRTMatrix(float *RTM, float *v0, float *v1) // v0 gndpos v1:001垂直向上
function AbvGndSeg (line 646) | int AbvGndSeg(int *pLabel, float *fPoints, int pointNum)
function SegBG (line 675) | int SegBG(int *pLabel, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::K...
function SClusterFeature (line 732) | SClusterFeature FindACluster(int *pLabel, int seedId, int labelId, pcl::...
function SegObjects (line 801) | int SegObjects(int *pLabel, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, p...
function CompleteObjects (line 870) | int CompleteObjects(int *pLabel, pcl::PointCloud<pcl::PointXYZ>::Ptr clo...
function ExpandObjects (line 902) | int ExpandObjects(int *pLabel, float* fPoints, int pointNum, float fSear...
function ExpandBG (line 954) | int ExpandBG(int *pLabel, float* fPoints, int pointNum, float fSearchRad...
function CalFreeRegion (line 1007) | int CalFreeRegion(float *pFreeDis, float *fPoints,int *pLabel,int pointNum)
function FreeSeg (line 1036) | int FreeSeg(float *fPoints,int *pLabel,int pointNum)
function GndSeg (line 1063) | int GndSeg(int* pLabel,float *fPoints,int pointNum,float fSearchRadius)
function SClusterFeature (line 1179) | SClusterFeature CalBBox(float *fPoints,int pointNum)
function SClusterFeature (line 1231) | SClusterFeature CalOBB(float *fPoints,int pointNum)
FILE: SC-PGO/include/livox_mapping/common.h
type pcl (line 43) | typedef pcl::PointXYZI PointType;
function rad2deg (line 45) | inline double rad2deg(double radians)
function deg2rad (line 50) | inline double deg2rad(double degrees)
type Pose6D (line 55) | struct Pose6D {
FILE: SC-PGO/include/livox_mapping/tic_toc.h
function class (line 10) | class TicToc
function class (line 34) | class TicTocV2
function tic (line 48) | void tic()
function toc (line 53) | void toc( std::string _about_task )
FILE: SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h
type typename (line 53) | typedef typename Distance::template traits<num_t,self_t>::distance_t met...
type nanoflann (line 54) | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexTy...
function m_data (line 59) | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const Ve...
function num_t (line 103) | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
FILE: SC-PGO/include/scancontext/Scancontext.cpp
function coreImportTest (line 6) | void coreImportTest (void)
function rad2deg (line 12) | float rad2deg(float radians)
function deg2rad (line 17) | float deg2rad(float degrees)
function xy2theta (line 23) | float xy2theta( const float & _x, const float & _y )
function MatrixXd (line 39) | MatrixXd circshift( MatrixXd &_mat, int _num_shift )
function eig2stdvec (line 62) | std::vector<float> eig2stdvec( MatrixXd _eigmat )
function MatrixXd (line 151) | MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _sca...
function MatrixXd (line 198) | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
function MatrixXd (line 214) | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
FILE: SC-PGO/include/scancontext/Scancontext.h
function class (line 57) | class SCManager
FILE: SC-PGO/include/scancontext/nanoflann.hpp
function T (line 79) | T pi_const() {
type has_resize (line 87) | struct has_resize : std::false_type {}
type has_resize<T, decltype((void)std::declval<T>().resize(1), 0)> (line 90) | struct has_resize<T, decltype((void)std::declval<T>().resize(1), 0)>
type has_assign (line 93) | struct has_assign : std::false_type {}
type has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)> (line 96) | struct has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)>
function resize (line 103) | inline typename std::enable_if<has_resize<Container>::value, void>::type
function resize (line 113) | inline typename std::enable_if<!has_resize<Container>::value, void>::type
function assign (line 123) | inline typename std::enable_if<has_assign<Container>::value, void>::type
function assign (line 132) | inline typename std::enable_if<!has_assign<Container>::value, void>::type
class KNNResultSet (line 142) | class KNNResultSet {
method KNNResultSet (line 155) | inline KNNResultSet(CountType capacity_)
method init (line 158) | inline void init(IndexType *indices_, DistanceType *dists_) {
method CountType (line 166) | inline CountType size() const { return count; }
method full (line 168) | inline bool full() const { return count == capacity; }
method addPoint (line 175) | inline bool addPoint(DistanceType dist, IndexType index) {
type IndexDist_Sorter (line 208) | struct IndexDist_Sorter {
class RadiusResultSet (line 220) | class RadiusResultSet {
method RadiusResultSet (line 230) | inline RadiusResultSet(
method init (line 237) | inline void init() { clear(); }
method clear (line 238) | inline void clear() { m_indices_dists.clear(); }
method size (line 240) | inline size_t size() const { return m_indices_dists.size(); }
method full (line 242) | inline bool full() const { return true; }
method addPoint (line 249) | inline bool addPoint(DistanceType dist, IndexType index) {
method DistanceType (line 255) | inline DistanceType worstDist() const { return radius; }
method worst_item (line 261) | std::pair<IndexType, DistanceType> worst_item() const {
method save_value (line 279) | void save_value(FILE *stream, const T &value, size_t count = 1) {
method save_value (line 284) | void save_value(FILE *stream, const std::vector<T> &value) {
method load_value (line 291) | void load_value(FILE *stream, T &value, size_t count = 1) {
method load_value (line 298) | void load_value(FILE *stream, std::vector<T> &value) {
type Metric (line 315) | struct Metric {}
type L1_Adaptor (line 324) | struct L1_Adaptor {
method L1_Adaptor (line 330) | L1_Adaptor(const DataSource &_data_source) : data_source(_data_sourc...
method DistanceType (line 332) | inline DistanceType evalMetric(const T *a, const size_t b_idx, size_...
method DistanceType (line 363) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type L2_Adaptor (line 375) | struct L2_Adaptor {
method L2_Adaptor (line 381) | L2_Adaptor(const DataSource &_data_source) : data_source(_data_sourc...
method DistanceType (line 383) | inline DistanceType evalMetric(const T *a, const size_t b_idx, size_...
method DistanceType (line 411) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type L2_Simple_Adaptor (line 423) | struct L2_Simple_Adaptor {
method L2_Simple_Adaptor (line 429) | L2_Simple_Adaptor(const DataSource &_data_source)
method DistanceType (line 432) | inline DistanceType evalMetric(const T *a, const size_t b_idx,
method DistanceType (line 443) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type SO2_Adaptor (line 455) | struct SO2_Adaptor {
method SO2_Adaptor (line 461) | SO2_Adaptor(const DataSource &_data_source) : data_source(_data_sour...
method DistanceType (line 463) | inline DistanceType evalMetric(const T *a, const size_t b_idx,
method DistanceType (line 471) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type SO3_Adaptor (line 490) | struct SO3_Adaptor {
method SO3_Adaptor (line 496) | SO3_Adaptor(const DataSource &_data_source)
method DistanceType (line 499) | inline DistanceType evalMetric(const T *a, const size_t b_idx,
method DistanceType (line 505) | inline DistanceType accum_dist(const U a, const V b, const size_t id...
type metric_L1 (line 511) | struct metric_L1 : public Metric {
type traits (line 512) | struct traits {
type metric_L2 (line 517) | struct metric_L2 : public Metric {
type traits (line 518) | struct traits {
type metric_L2_Simple (line 523) | struct metric_L2_Simple : public Metric {
type traits (line 524) | struct traits {
type metric_SO2 (line 529) | struct metric_SO2 : public Metric {
type traits (line 530) | struct traits {
type metric_SO3 (line 535) | struct metric_SO3 : public Metric {
type traits (line 536) | struct traits {
type KDTreeSingleIndexAdaptorParams (line 547) | struct KDTreeSingleIndexAdaptorParams {
method KDTreeSingleIndexAdaptorParams (line 548) | KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
type SearchParams (line 555) | struct SearchParams {
method SearchParams (line 558) | SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ ...
method T (line 579) | inline T *allocate(size_t count = 1) {
class PooledAllocator (line 602) | class PooledAllocator {
method internal_init (line 613) | void internal_init() {
method PooledAllocator (line 627) | PooledAllocator() { internal_init(); }
method free_all (line 635) | void free_all() {
method T (line 703) | T *allocate(const size_t count = 1) {
type array_or_vector_selector (line 716) | struct array_or_vector_selector {
type array_or_vector_selector<-1, T> (line 720) | struct array_or_vector_selector<-1, T> {
class KDTreeBaseClass (line 740) | class KDTreeBaseClass {
method freeIndex (line 745) | void freeIndex(Derived &obj) {
type Node (line 755) | struct Node {
type leaf (line 759) | struct leaf {
type nonleaf (line 762) | struct nonleaf {
type Interval (line 772) | struct Interval {
method size (line 814) | size_t size(const Derived &obj) const { return obj.m_size; }
method veclen (line 817) | size_t veclen(const Derived &obj) {
method ElementType (line 822) | inline ElementType dataset_get(const Derived &obj, size_t idx,
method usedMemory (line 831) | size_t usedMemory(Derived &obj) {
method computeMinMax (line 837) | void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,
method NodePtr (line 858) | NodePtr divideTree(Derived &obj, const IndexType left, const IndexTy...
method middleSplit_ (line 910) | void middleSplit_(Derived &obj, IndexType *ind, IndexType count,
method planeSplit (line 968) | void planeSplit(Derived &obj, IndexType *ind, const IndexType count,
method DistanceType (line 1006) | DistanceType computeInitialDistances(const Derived &obj,
method save_tree (line 1025) | void save_tree(Derived &obj, FILE *stream, NodePtr tree) {
method load_tree (line 1035) | void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {
method saveIndex_ (line 1051) | void saveIndex_(Derived &obj, FILE *stream) {
method loadIndex_ (line 1065) | void loadIndex_(Derived &obj, FILE *stream) {
class KDTreeSingleIndexAdaptor (line 1117) | class KDTreeSingleIndexAdaptor
method KDTreeSingleIndexAdaptor (line 1123) | KDTreeSingleIndexAdaptor(
method KDTreeSingleIndexAdaptor (line 1171) | KDTreeSingleIndexAdaptor(const int dimensionality,
method buildIndex (line 1191) | void buildIndex() {
method findNeighbors (line 1222) | bool findNeighbors(RESULTSET &result, const ElementType *vec,
method knnSearch (line 1253) | size_t knnSearch(const ElementType *query_point, const size_t num_cl...
method radiusSearch (line 1278) | size_t
method radiusSearchCustomCallback (line 1296) | size_t radiusSearchCustomCallback(
method init_vind (line 1308) | void init_vind() {
method computeBoundingBox (line 1317) | void computeBoundingBox(BoundingBox &bbox) {
method searchLevel (line 1347) | bool searchLevel(RESULTSET &result_set, const ElementType *vec,
method saveIndex (line 1418) | void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
method loadIndex (line 1425) | void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
class KDTreeSingleIndexDynamicAdaptor_ (line 1467) | class KDTreeSingleIndexDynamicAdaptor_
method KDTreeSingleIndexDynamicAdaptor_ (line 1518) | KDTreeSingleIndexDynamicAdaptor_(
method KDTreeSingleIndexDynamicAdaptor_ (line 1535) | KDTreeSingleIndexDynamicAdaptor_
method buildIndex (line 1554) | void buildIndex() {
method findNeighbors (line 1583) | bool findNeighbors(RESULTSET &result, const ElementType *vec,
method knnSearch (line 1613) | size_t knnSearch(const ElementType *query_point, const size_t num_cl...
method radiusSearch (line 1638) | size_t
method radiusSearchCustomCallback (line 1656) | size_t radiusSearchCustomCallback(
method computeBoundingBox (line 1666) | void computeBoundingBox(BoundingBox &bbox) {
method searchLevel (line 1696) | void searchLevel(RESULTSET &result_set, const ElementType *vec,
method saveIndex (line 1762) | void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
method loadIndex (line 1769) | void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
class KDTreeSingleIndexDynamicAdaptor (line 1788) | class KDTreeSingleIndexDynamicAdaptor {
method First0Bit (line 1822) | int First0Bit(IndexType num) {
method init (line 1832) | void init() {
method KDTreeSingleIndexDynamicAdaptor (line 1857) | KDTreeSingleIndexDynamicAdaptor(const int dimensionality,
method KDTreeSingleIndexDynamicAdaptor (line 1878) | KDTreeSingleIndexDynamicAdaptor(
method addPoints (line 1883) | void addPoints(IndexType start, IndexType end) {
method removePoint (line 1906) | void removePoint(size_t idx) {
method findNeighbors (line 1926) | bool findNeighbors(RESULTSET &result, const ElementType *vec,
type KDTreeEigenMatrixAdaptor (line 1954) | struct KDTreeEigenMatrixAdaptor {
method KDTreeEigenMatrixAdaptor (line 1968) | KDTreeEigenMatrixAdaptor(const size_t dimensionality,
method KDTreeEigenMatrixAdaptor (line 1987) | KDTreeEigenMatrixAdaptor(const self_t &) = delete;
method query (line 1999) | inline void query(const num_t *query_point, const size_t num_closest,
method self_t (line 2010) | const self_t &derived() const { return *this; }
method self_t (line 2011) | self_t &derived() { return *this; }
method kdtree_get_point_count (line 2014) | inline size_t kdtree_get_point_count() const {
method num_t (line 2019) | inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {
method kdtree_get_bbox (line 2028) | bool kdtree_get_bbox(BBOX & /*bb*/) const {
FILE: SC-PGO/src/laserPosegraphOptimization.cpp
function padZeros (line 149) | std::string padZeros(int val, int num_digits = 6) {
function Pose6DtoGTSAMPose3 (line 155) | gtsam::Pose3 Pose6DtoGTSAMPose3(const Pose6D& p)
function Pose6DtoTransform (line 160) | tf::Transform Pose6DtoTransform(const Pose6D& p)
function Pose6D (line 168) | Pose6D TransformtoPose6D(const tf::Transform& trans)
function Pose6D (line 175) | Pose6D PoseInterpolation(const Pose6D& p_before, const Pose6D& p_after, ...
function laserOdometryHandler (line 186) | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &_laserOdom...
function laserCloudFullResHandler (line 203) | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &_l...
function rtkHandler (line 210) | void rtkHandler(const ad_localization_msgs::NavStateInfo::ConstPtr &_rtk)
function initNoises (line 229) | void initNoises( void )
function Pose6D (line 261) | Pose6D getOdom(nav_msgs::Odometry::ConstPtr _odom)
function Pose6D (line 274) | Pose6D diffTransformation(const Pose6D& _p1, const Pose6D& _p2)
function Pose6D (line 287) | Pose6D convertOdomToENU(const Pose6D &pose_curr)
function local2global (line 295) | pcl::PointCloud<PointType>::Ptr local2global(const pcl::PointCloud<Point...
function pubMap (line 317) | void pubMap(void)
function pubPath (line 342) | void pubPath( void )
function updatePoses (line 389) | void updatePoses(void)
function runISAM2opt (line 414) | void runISAM2opt(void)
function transformPointCloud (line 427) | pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<Poin...
function loopFindNearKeyframesCloud (line 453) | void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKe...
function doICPVirtualRelative (line 486) | std::optional<gtsam::Pose3> doICPVirtualRelative( int _loop_kf_idx, int ...
function process_pg (line 581) | void process_pg()
function performSCLoopClosure (line 782) | void performSCLoopClosure(void)
function process_lcd (line 801) | void process_lcd(void)
function process_icp (line 813) | void process_icp(void)
function process_viz_path (line 846) | void process_viz_path(void)
function process_isam (line 858) | void process_isam(void)
function process_viz_map (line 873) | void process_viz_map(void)
function process_interactive_slam_result (line 885) | void process_interactive_slam_result() {
function save_odo (line 911) | void save_odo() {
function save_map (line 938) | void save_map() {
function MySigintHandler (line 950) | void MySigintHandler(int sig)
function main (line 961) | int main(int argc, char **argv)
FILE: SC-PGO/utils/moving_object_removal/move_object_removal_livox.py
function set_log_level (line 42) | def set_log_level(level: str = "TRACE", save_log: bool = False, log_file...
function set_calib_parameters (line 54) | def set_calib_parameters():
function init_model (line 64) | def init_model(config=None, check_point=None):
function align_file_lists (line 77) | def align_file_lists(img_list, pc_list):
function files_saving_path (line 104) | def files_saving_path(base_path=None):
function fetch_dataset (line 127) | def fetch_dataset(data_base, livox_pc_path=None, img_path=None):
function img_undistort (line 159) | def img_undistort(img_ori, intrinsic_param, distort_param):
function convert_rot_trans (line 171) | def convert_rot_trans(extrinsic):
function project_pts_to_img (line 178) | def project_pts_to_img(pts, image, extrinsic, intrinsic, cam_dist, polyg...
function img_process (line 258) | def img_process(img, model, intrinsic_param, distort_param, visual_flag=...
function projection_visualization (line 265) | def projection_visualization(img, pts, save_pth=None):
function enlarge_bbox (line 301) | def enlarge_bbox(bbox_init, img_size, enlarge_step=10):
function extract_inference_result (line 309) | def extract_inference_result(model, img, result, score_thr=0.6):
function inference_and_remove (line 350) | def inference_and_remove(pc_list, img_list, inference_model, save_path=N...
function euler_to_rotMat (line 381) | def euler_to_rotMat(yaw, pitch, roll):
function main (line 398) | def main(inference=False):
FILE: SC-PGO/utils/python/pypcdMyUtils.py
function make_xyzi_point_cloud (line 5) | def make_xyzi_point_cloud(xyzl, label_type='f'):
Condensed preview — 73 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (839K chars).
[
{
"path": ".gitignore",
"chars": 21,
"preview": ".vscode/\n__pycache__/"
},
{
"path": "LICENSE",
"chars": 1517,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2021, PJLab-ADG\nAll rights reserved.\n\nRedistribution and use in source and binary fo"
},
{
"path": "LIO-Livox/CMakeLists.txt",
"chars": 1840,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(livox_odometry)\n\nSET(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_FLAGS \"-std"
},
{
"path": "LIO-Livox/LICENSE",
"chars": 1505,
"preview": "BSD 3-Clause License\n\nCopyright (c) 2021, LIVOX\nAll rights reserved.\n\nRedistribution and use in source and binary forms,"
},
{
"path": "LIO-Livox/README.md",
"chars": 8610,
"preview": "# LIO-Livox (A Robust LiDAR-Inertial Odometry for Livox LiDAR)\nThis respository implements a robust LiDAR-inertial odome"
},
{
"path": "LIO-Livox/cmake/FindEigen3.cmake",
"chars": 3249,
"preview": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3"
},
{
"path": "LIO-Livox/cmake/FindSuiteSparse.cmake",
"chars": 21946,
"preview": "# Ceres Solver - A fast non-linear least squares minimizer\n# Copyright 2015 Google Inc. All rights reserved.\n# http://ce"
},
{
"path": "LIO-Livox/config/horizon_config.yaml",
"chars": 611,
"preview": "%YAML:1.0\n\n\n# switches\nLidar_Type: 0 # 0-horizon\nUsed_Line: 6 # lines used for lio, set to 1~6\nFeature_Mode: 0 "
},
{
"path": "LIO-Livox/include/Estimator/Estimator.h",
"chars": 10658,
"preview": "#ifndef LIO_LIVOX_ESTIMATOR_H\n#define LIO_LIVOX_ESTIMATOR_H\n\n#include <ros/ros.h>\n#include <pcl_conversions/pcl_conversi"
},
{
"path": "LIO-Livox/include/IMUIntegrator/IMUIntegrator.h",
"chars": 3010,
"preview": "#ifndef LIO_LIVOX_IMUINTEGRATOR_H\n#define LIO_LIVOX_IMUINTEGRATOR_H\n#include <sensor_msgs/Imu.h>\n#include <queue>\n#inclu"
},
{
"path": "LIO-Livox/include/LidarFeatureExtractor/LidarFeatureExtractor.h",
"chars": 4146,
"preview": "#ifndef LIO_LIVOX_LIDARFEATUREEXTRACTOR_H\n#define LIO_LIVOX_LIDARFEATUREEXTRACTOR_H\n#include <ros/ros.h>\n#include <livox"
},
{
"path": "LIO-Livox/include/MapManager/Map_Manager.h",
"chars": 6471,
"preview": "#ifndef LIO_LIVOX_MAP_MANAGER_H\n#define LIO_LIVOX_MAP_MANAGER_H\n#include <pcl/kdtree/kdtree_flann.h>\n#include <pcl/point"
},
{
"path": "LIO-Livox/include/segment/pointsCorrect.hpp",
"chars": 1097,
"preview": "#ifndef _COMMON_HPP\n#define _CONNON_HPP\n\n#include <pcl/kdtree/kdtree_flann.h>\n#include <pcl/point_cloud.h>\n#include <pcl"
},
{
"path": "LIO-Livox/include/segment/segment.hpp",
"chars": 4263,
"preview": "#ifndef _SEGMENT_HPP\n#define _SEGMENT_HPP\n\n#include <pcl/kdtree/kdtree_flann.h>\n#include <pcl/point_cloud.h>\n#include <p"
},
{
"path": "LIO-Livox/include/sophus/average.hpp",
"chars": 8227,
"preview": "/// @file\n/// Calculation of biinvariant means.\n\n#ifndef SOPHUS_AVERAGE_HPP\n#define SOPHUS_AVERAGE_HPP\n\n#include \"common"
},
{
"path": "LIO-Livox/include/sophus/common.hpp",
"chars": 5521,
"preview": "/// @file\n/// Common functionality.\n\n#ifndef SOPHUS_COMMON_HPP\n#define SOPHUS_COMMON_HPP\n\n#include <cmath>\n#include <cst"
},
{
"path": "LIO-Livox/include/sophus/example_ensure_handler.cpp",
"chars": 398,
"preview": "#include \"common.hpp\"\n\n#include <cstdio>\n#include <cstdlib>\n\nnamespace Sophus {\nvoid ensureFailed(char const* function, "
},
{
"path": "LIO-Livox/include/sophus/formatstring.hpp",
"chars": 1810,
"preview": "/// @file\n/// FormatString functionality\n\n#ifndef SOPHUS_FORMATSTRING_HPP\n#define SOPHUS_FORMATSTRING_HPP\n\n#include <ios"
},
{
"path": "LIO-Livox/include/sophus/geometry.hpp",
"chars": 6141,
"preview": "/// @file\n/// Transformations between poses and hyperplanes.\n\n#ifndef GEOMETRY_HPP\n#define GEOMETRY_HPP\n\n#include \"se2.h"
},
{
"path": "LIO-Livox/include/sophus/interpolate.hpp",
"chars": 1282,
"preview": "/// @file\n/// Interpolation for Lie groups.\n\n#ifndef SOPHUS_INTERPOLATE_HPP\n#define SOPHUS_INTERPOLATE_HPP\n\n#include <Ei"
},
{
"path": "LIO-Livox/include/sophus/interpolate_details.hpp",
"chars": 2647,
"preview": "#ifndef SOPHUS_INTERPOLATE_DETAILS_HPP\n#define SOPHUS_INTERPOLATE_DETAILS_HPP\n\n#include \"rxso2.hpp\"\n#include \"rxso3.hpp\""
},
{
"path": "LIO-Livox/include/sophus/num_diff.hpp",
"chars": 2850,
"preview": "/// @file\n/// Numerical differentiation using finite differences\n\n#ifndef SOPHUS_NUM_DIFF_HPP\n#define SOPHUS_NUM_DIFF_HP"
},
{
"path": "LIO-Livox/include/sophus/rotation_matrix.hpp",
"chars": 2685,
"preview": "/// @file\n/// Rotation matrix helper functions.\n\n#ifndef SOPHUS_ROTATION_MATRIX_HPP\n#define SOPHUS_ROTATION_MATRIX_HPP\n\n"
},
{
"path": "LIO-Livox/include/sophus/rxso2.hpp",
"chars": 22182,
"preview": "/// @file\n/// Direct product R X SO(2) - rotation and scaling in 2d.\n\n#ifndef SOPHUS_RXSO2_HPP\n#define SOPHUS_RXSO2_HPP\n"
},
{
"path": "LIO-Livox/include/sophus/rxso3.hpp",
"chars": 25099,
"preview": "/// @file\n/// Direct product R X SO(3) - rotation and scaling in 3d.\n\n#ifndef SOPHUS_RXSO3_HPP\n#define SOPHUS_RXSO3_HPP\n"
},
{
"path": "LIO-Livox/include/sophus/se2.hpp",
"chars": 26757,
"preview": "/// @file\n/// Special Euclidean group SE(2) - rotation and translation in 2d.\n\n#ifndef SOPHUS_SE2_HPP\n#define SOPHUS_SE2"
},
{
"path": "LIO-Livox/include/sophus/se3.hpp",
"chars": 35838,
"preview": "/// @file\n/// Special Euclidean group SE(3) - rotation and translation in 3d.\n\n#ifndef SOPHUS_SE3_HPP\n#define SOPHUS_SE3"
},
{
"path": "LIO-Livox/include/sophus/sim2.hpp",
"chars": 24297,
"preview": "/// @file\n/// Similarity group Sim(2) - scaling, rotation and translation in 2d.\n\n#ifndef SOPHUS_SIM2_HPP\n#define SOPHUS"
},
{
"path": "LIO-Livox/include/sophus/sim3.hpp",
"chars": 25120,
"preview": "/// @file\n/// Similarity group Sim(3) - scaling, rotation and translation in 3d.\n\n#ifndef SOPHUS_SIM3_HPP\n#define SOPHUS"
},
{
"path": "LIO-Livox/include/sophus/sim_details.hpp",
"chars": 3536,
"preview": "#ifndef SOPHUS_SIM_DETAILS_HPP\n#define SOPHUS_SIM_DETAILS_HPP\n\n#include \"types.hpp\"\n\nnamespace Sophus {\nnamespace detail"
},
{
"path": "LIO-Livox/include/sophus/so2.hpp",
"chars": 21328,
"preview": "/// @file\n/// Special orthogonal group SO(2) - rotation in 2d.\n\n#ifndef SOPHUS_SO2_HPP\n#define SOPHUS_SO2_HPP\n\n#include "
},
{
"path": "LIO-Livox/include/sophus/so3.hpp",
"chars": 29726,
"preview": "/// @file\n/// Special orthogonal group SO(3) - rotation in 3d.\n\n#ifndef SOPHUS_SO3_HPP\n#define SOPHUS_SO3_HPP\n\n#include "
},
{
"path": "LIO-Livox/include/sophus/test_macros.hpp",
"chars": 6620,
"preview": "#ifndef SOPUHS_TESTS_MACROS_HPP\n#define SOPUHS_TESTS_MACROS_HPP\n\n#include <sophus/types.hpp>\n#include <sophus/formatstri"
},
{
"path": "LIO-Livox/include/sophus/types.hpp",
"chars": 6323,
"preview": "/// @file\n/// Common type aliases.\n\n#ifndef SOPHUS_TYPES_HPP\n#define SOPHUS_TYPES_HPP\n\n#include <type_traits>\n#include \""
},
{
"path": "LIO-Livox/include/sophus/velocities.hpp",
"chars": 2401,
"preview": "#ifndef SOPHUS_VELOCITIES_HPP\n#define SOPHUS_VELOCITIES_HPP\n\n#include <functional>\n\n#include \"num_diff.hpp\"\n#include \"se"
},
{
"path": "LIO-Livox/include/utils/ceresfunc.h",
"chars": 32696,
"preview": "#ifndef LIO_LIVOX_CERESFUNC_H\n#define LIO_LIVOX_CERESFUNC_H\n#include <ceres/ceres.h>\n#include <glog/logging.h>\n#include "
},
{
"path": "LIO-Livox/include/utils/math_utils.hpp",
"chars": 7247,
"preview": "/*\n * COPYRIGHT AND PERMISSION NOTICE\n * Penn Software MSCKF_VIO\n * Copyright (C) 2017 The Trustees of the University of"
},
{
"path": "LIO-Livox/include/utils/pcl_utils.hpp",
"chars": 1147,
"preview": "#ifndef PCL_UTILS_HPP\n#define PCL_UTILS_HPP\n\n#include <pcl/sample_consensus/ransac.h>\n#include <pcl/sample_consensus/sac"
},
{
"path": "LIO-Livox/launch/livox_odometry.launch",
"chars": 1247,
"preview": "<launch>\n\n <node pkg=\"livox_odometry\" type=\"ScanRegistration\" name=\"ScanRegistration\" output=\"screen\">\n\n <para"
},
{
"path": "LIO-Livox/package.xml",
"chars": 1891,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>livox_odometry</name>\n <version>0.0.0</version>\n\n <description>\n This is th"
},
{
"path": "LIO-Livox/rviz_cfg/lio.rviz",
"chars": 7576,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 85\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "LIO-Livox/src/lio/Estimator.cpp",
"chars": 57167,
"preview": "#include \"Estimator/Estimator.h\"\n\nEstimator::Estimator(const float& filter_corner, const float& filter_surf){\n laserClo"
},
{
"path": "LIO-Livox/src/lio/IMUIntegrator.cpp",
"chars": 5312,
"preview": "#include \"IMUIntegrator/IMUIntegrator.h\"\n\nIMUIntegrator::IMUIntegrator(){\n Reset();\n noise.setZero();\n noise.block<3,"
},
{
"path": "LIO-Livox/src/lio/LidarFeatureExtractor.cpp",
"chars": 43931,
"preview": "#include \"LidarFeatureExtractor/LidarFeatureExtractor.h\"\n\nLidarFeatureExtractor::LidarFeatureExtractor(int n_scans,int N"
},
{
"path": "LIO-Livox/src/lio/Map_Manager.cpp",
"chars": 26741,
"preview": "#include \"MapManager/Map_Manager.h\"\n#include <fstream>\n\nMAP_MANAGER::MAP_MANAGER(const float& filter_corner, const float"
},
{
"path": "LIO-Livox/src/lio/PoseEstimation.cpp",
"chars": 28751,
"preview": "#include \"Estimator/Estimator.h\"\n#include <fstream>\n#include <memory>\n#include <string>\n#include <iomanip>\ntypedef pcl::"
},
{
"path": "LIO-Livox/src/lio/ScanRegistration.cpp",
"chars": 3499,
"preview": "#include \"LidarFeatureExtractor/LidarFeatureExtractor.h\"\n\ntypedef pcl::PointXYZINormal PointType;\n\nros::Publisher pubFul"
},
{
"path": "LIO-Livox/src/lio/ceresfunc.cpp",
"chars": 1663,
"preview": "#include \"utils/ceresfunc.h\"\n\nvoid* ThreadsConstructA(void* threadsstruct)\n{\n ThreadsStruct* p = ((ThreadsStruct*)threa"
},
{
"path": "LIO-Livox/src/segment/pointsCorrect.cpp",
"chars": 10020,
"preview": "#include \"segment/pointsCorrect.hpp\"\n\nfloat gnd_pos[6];\nint frame_count = 0;\nint frame_lenth_threshold = 5;//5 frames up"
},
{
"path": "LIO-Livox/src/segment/segment.cpp",
"chars": 39626,
"preview": "#include \"segment/segment.hpp\"\n\n#define N_FRAME 5\n\nfloat tmp_gnd_pose[100*6];\nint tem_gnd_num = 0;\n\nPCSeg::PCSeg()\n{\n "
},
{
"path": "README.md",
"chars": 8362,
"preview": "# Livox-Mapping\n\nThis repository implements an all-in-one and ready-to-use LiDAR-inertial odometry system for Livox LiDA"
},
{
"path": "SC-PGO/.gitignore",
"chars": 4,
"preview": "PCD/"
},
{
"path": "SC-PGO/CMakeLists.txt",
"chars": 1235,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(livox_mapping)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# set(CMAKE_CXX_FLAGS \"-st"
},
{
"path": "SC-PGO/README.md",
"chars": 5858,
"preview": "# SC-A-LOAM\n\n## What is SC-A-LOAM? \n- A real-time LiDAR SLAM package that integrates A-LOAM and ScanContext. \n - **A-"
},
{
"path": "SC-PGO/include/livox_mapping/common.h",
"chars": 2374,
"preview": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOA"
},
{
"path": "SC-PGO/include/livox_mapping/tic_toc.h",
"chars": 1343,
"preview": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#pragma"
},
{
"path": "SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h",
"chars": 5448,
"preview": "/***********************************************************************\n * Software License Agreement (BSD License)\n *\n"
},
{
"path": "SC-PGO/include/scancontext/Scancontext.cpp",
"chars": 14621,
"preview": "#include \"scancontext/Scancontext.h\"\n\n// namespace SC2\n// {\n\nvoid coreImportTest (void)\n{\n cout << \"scancontext lib i"
},
{
"path": "SC-PGO/include/scancontext/Scancontext.h",
"chars": 5209,
"preview": "#pragma once\n\n#include <ctime>\n#include <cassert>\n#include <cmath>\n#include <utility>\n#include <vector>\n#include <algori"
},
{
"path": "SC-PGO/include/scancontext/nanoflann.hpp",
"chars": 73146,
"preview": "/***********************************************************************\n * Software License Agreement (BSD License)\n *\n"
},
{
"path": "SC-PGO/launch/livox_mapping.launch",
"chars": 1870,
"preview": "<launch>\n\n <param name=\"mapviz_filter_size\" type=\"double\" value=\"0.05\"/>\n\n <!-- SC-A-LOAM -->\n <param name=\"key"
},
{
"path": "SC-PGO/package.xml",
"chars": 1516,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>livox_mapping</name>\n <version>0.0.0</version>\n\n <description>\n This is an "
},
{
"path": "SC-PGO/rviz_cfg/livox_mapping.rviz",
"chars": 16835,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 85\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "SC-PGO/src/laserPosegraphOptimization.cpp",
"chars": 44232,
"preview": "#include <fstream>\n#include <math.h>\n#include <vector>\n#include <mutex>\n#include <queue>\n#include <thread>\n#include <ios"
},
{
"path": "SC-PGO/utils/moving_object_removal/README.md",
"chars": 490,
"preview": "# Moving Object Removal\n\nPlease refer to [mmdection](https://github.com/open-mmlab/mmdetection) for the installation pro"
},
{
"path": "SC-PGO/utils/moving_object_removal/move_object_removal_livox.py",
"chars": 14171,
"preview": "import pcl\nfrom loguru import logger\n\nimport os\nimport sys\nimport time\nimport numpy as np\n\nimport cv2\nimport matplotlib."
},
{
"path": "SC-PGO/utils/python/makeMergedMap.py",
"chars": 5350,
"preview": "import os \nimport sys\nimport time \nimport copy \nfrom io import StringIO\n\nimport pypcd # for the install, use this comman"
},
{
"path": "SC-PGO/utils/python/pypcdMyUtils.py",
"chars": 1244,
"preview": "import numpy as np\nimport pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --us"
},
{
"path": "ad_localization_msgs/CMakeLists.txt",
"chars": 574,
"preview": "cmake_minimum_required(VERSION 3.0.2)\nproject(ad_localization_msgs)\n\n## Compile as C++11, supported in ROS Kinetic and n"
},
{
"path": "ad_localization_msgs/msg/NavStateInfo.msg",
"chars": 2481,
"preview": "# Header include seq, timestamp(last node pub time), and frame_id(sensor model)\nstd_msgs/Header header\n\n# The time of po"
},
{
"path": "ad_localization_msgs/package.xml",
"chars": 856,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>ad_localization_msgs</name>\n <version>0.0.0</version>\n <description>The ad_loc"
}
]
// ... and 2 more files (download for full content)
About this extraction
This page contains the full source code of the PJLab-ADG/Livox-Mapping GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 73 files (786.6 KB), approximately 229.5k tokens, and a symbol index with 786 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.