第一次提交代码，启动崩溃待解决

CMakeLists.txt

@@ -0,0 +1,177 @@
+# Copyright(c) 2019 livoxtech limited.
+cmake_minimum_required(VERSION 2.8.3)
+
+
+#---------------------------------------------------------------------------------------
+# Start livox_ros_driver project
+#---------------------------------------------------------------------------------------
+
+project(lio_livox_cpp)
+
+#SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+
+#---------------------------------------------------------------------------------------
+# find package and the dependecy
+#---------------------------------------------------------------------------------------
+find_package(Boost 1.54 REQUIRED COMPONENTS
+	system
+	thread
+	chrono
+	)
+
+find_package (Eigen3 REQUIRED CONFIG QUIET)
+find_package(Pangolin 0.8 REQUIRED)
+find_package(PCL REQUIRED)
+find_package(Ceres REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(SuiteSparse REQUIRED)
+
+find_package(PkgConfig)
+pkg_check_modules(APR apr-1)
+if (APR_FOUND)
+	message(${APR_INCLUDE_DIRS})
+	message(${APR_LIBRARIES})
+endif (APR_FOUND)
+
+
+#---------------------------------------------------------------------------------------
+# Set default build to release
+#---------------------------------------------------------------------------------------
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose Release or Debug" FORCE)
+endif()
+
+#---------------------------------------------------------------------------------------
+# Compiler config
+#---------------------------------------------------------------------------------------
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+## make sure the livox_sdk_static library is installed
+find_library(LIVOX_SDK_LIBRARY liblivox_sdk_static.a /usr/local/lib)
+
+if((NOT LIVOX_SDK_LIBRARY) OR (NOT EXISTS ${LIVOX_SDK_LIBRARY}))
+	# couldn't find the livox sdk library
+	message("Coudn't find livox sdk library!")
+	message("Download Livox-SDK from github and build&install it please!")
+	message("Download Livox-SDK from github and build&install it please!")
+	message("Download Livox-SDK from github and build&install it please!")
+
+	message("git clone Livox-SDK from github temporarily, only for ROS distro jenkins build!")
+
+	# clone livox sdk source code from github
+	execute_process(COMMAND rm -rf ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK OUTPUT_VARIABLE cmd_res)
+	message("Try to pull the livox sdk source code from github")
+	FOREACH(res ${cmd_res})
+		MESSAGE(${res})
+	ENDFOREACH()
+
+	execute_process(COMMAND git clone https://github.com/Livox-SDK/Livox-SDK.git ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK OUTPUT_VARIABLE cmd_res)
+	FOREACH(res ${cmd_res})
+		MESSAGE(${res})
+	ENDFOREACH()
+
+	execute_process(COMMAND cmake .. WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build OUTPUT_VARIABLE cmd_res)
+	FOREACH(res ${cmd_res})
+		MESSAGE(${res})
+	ENDFOREACH()
+
+	execute_process(COMMAND make WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build OUTPUT_VARIABLE cmd_res)
+	FOREACH(res ${cmd_res})
+		MESSAGE(${res})
+	ENDFOREACH()
+
+	include_directories(
+		./
+		${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/sdk_core/include
+	)
+
+	link_directories(
+		./
+		${CMAKE_CURRENT_SOURCE_DIR}/Livox-SDK/build/sdk_core
+	)
+
+else()
+	message("find livox sdk library success")
+endif()
+
+include_directories(
+		define
+		lio/include
+		lio/src
+		${Pangolin_INCLUDE_DIRS}
+		${catkin_INCLUDE_DIRS}
+		${EIGEN3_INCLUDE_DIR}
+		${PCL_INCLUDE_DIRS}
+		${CERES_INCLUDE_DIRS}
+		${OpenCV_INCLUDE_DIRS}
+		${SUITESPARSE_INCLUDE_DIRS}
+		${APR_INCLUDE_DIRS}
+		livox/common
+		livox/common/rapidjson
+		livox/comon/rapdidxml
+		livox/common/comm
+		livox/timesync
+		livox/timesync/user_uart
+		livox/livox_driver)
+## PCL library
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+#---------------------------------------------------------------------------------------
+# generate excutable and add libraries
+#---------------------------------------------------------------------------------------
+add_executable(${PROJECT_NAME}_node
+
+		lio/src/PoseEstimation.cpp
+		lio/src/segment/LidarFeatureExtractor.cpp
+		lio/src/segment/segment.cpp
+		lio/src/segment/pointsCorrect.cpp
+		lio/src/lio/LocalMapIvox.cpp
+		lio/src/lio/mapper.cpp
+		lio/src/lio/Estimator.cpp
+		lio/src/lio/IMUIntegrator.cpp
+		lio/src/lio/ceresfunc.cpp
+
+		livox/livox_driver/lvx_file.cpp
+		livox/livox_driver/ldq.cpp
+		livox/livox_driver/lds.cpp
+		livox/livox_driver/lds_lvx.cpp
+		livox/livox_driver/lds_lidar.cpp
+		livox/livox_driver/lds_hub.cpp
+		livox/livox_driver/lddc.cpp
+		livox/timesync/timesync.cpp
+		livox/timesync/user_uart/user_uart.cpp
+		livox/common/comm/comm_protocol.cpp
+		livox/common/comm/sdk_protocol.cpp
+		livox/common/comm/gps_protocol.cpp
+
+
+    )
+
+#---------------------------------------------------------------------------------------
+# precompile macro and compile option
+#---------------------------------------------------------------------------------------
+target_compile_options(${PROJECT_NAME}_node
+    PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wall>
+    )
+
+#---------------------------------------------------------------------------------------
+# link libraries
+#---------------------------------------------------------------------------------------
+target_link_libraries(${PROJECT_NAME}_node
+		pango_display
+	livox_sdk_static.a
+		${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES}
+	${Boost_LIBRARY}
+	${APR_LIBRARIES}
+		-lpthread
+	)
+
+
+
+#---------------------------------------------------------------------------------------
+# end of CMakeList.txt
+#---------------------------------------------------------------------------------------

cmake/FindEigen3.cmake

@@ -0,0 +1,87 @@
+# - 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)
+

cmake/FindSuiteSparse.cmake

@@ -0,0 +1,515 @@
+# 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)

config/horizon_config.yaml

@@ -0,0 +1,21 @@
+%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: 0.1 # if(depth < LidarNearestDis) do not use this point 
+KdTreeCornerOutlierDis: 0.2 # corner filter threshold
+Use_seg: 1 # use segment algorithm
+map_skip_frame: 2
+
+ivox_nearby_type: 26 #6  18 26
+max_ivox_node: 500
+ivox_node_solution: 0.2

define/typedef.h

@@ -0,0 +1,36 @@
+//
+// Created by zx on 22-11-2.
+//
+
+#ifndef SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_
+#define SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+typedef pcl::PointXYZINormal PointType;
+typedef pcl::PointCloud<PointType> PointCloud;
+typedef struct
+{
+    double timebase;
+    pcl::PointCloud<PointType>::Ptr cloud;
+}TimeCloud;
+
+typedef void(*odomCallback)(Eigen::Matrix4d,double);
+typedef void(*CloudMappedCallback)(pcl::PointCloud<PointType>::Ptr,double);
+
+
+typedef struct
+{
+    double x;
+    double y;
+    double z;
+}Data3d;
+typedef struct
+{
+    double timebase;
+    Data3d angular_velocity;
+    Data3d linear_acceleration;
+
+} ImuData;
+
+#endif //SRC_LIO_LIVOX_INCLUDE_UTILS_TYPEDEF_H_

lio/include/ivox3d/eigen_types.h

@@ -0,0 +1,86 @@
+//
+// Created by xiang on 2021/7/16.
+//
+
+#ifndef FASTER_LIO_EIGEN_TYPES_H
+#define FASTER_LIO_EIGEN_TYPES_H
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+/// alias for eigen
+using Vec2i = Eigen::Vector2i;
+using Vec3i = Eigen::Vector3i;
+
+using Vec2d = Eigen::Vector2d;
+using Vec2f = Eigen::Vector2f;
+using Vec3d = Eigen::Vector3d;
+using Vec3f = Eigen::Vector3f;
+using Vec5d = Eigen::Matrix<double, 5, 1>;
+using Vec5f = Eigen::Matrix<float, 5, 1>;
+using Vec6d = Eigen::Matrix<double, 6, 1>;
+using Vec6f = Eigen::Matrix<float, 6, 1>;
+using Vec15d = Eigen::Matrix<double, 15, 15>;
+
+using Mat1d = Eigen::Matrix<double, 1, 1>;
+using Mat3d = Eigen::Matrix3d;
+using Mat3f = Eigen::Matrix3f;
+using Mat4d = Eigen::Matrix4d;
+using Mat4f = Eigen::Matrix4f;
+using Mat5d = Eigen::Matrix<double, 5, 5>;
+using Mat5f = Eigen::Matrix<float, 5, 5>;
+using Mat6d = Eigen::Matrix<double, 6, 6>;
+using Mat6f = Eigen::Matrix<float, 6, 6>;
+using Mat15d = Eigen::Matrix<double, 15, 15>;
+
+using Quatd = Eigen::Quaterniond;
+using Quatf = Eigen::Quaternionf;
+
+namespace faster_lio {
+
+/// less of vector
+template <int N>
+struct less_vec {
+    inline bool operator()(const Eigen::Matrix<int, N, 1>& v1, const Eigen::Matrix<int, N, 1>& v2) const;
+};
+
+/// hash of vector
+template <int N>
+struct hash_vec {
+    inline size_t operator()(const Eigen::Matrix<int, N, 1>& v) const;
+};
+
+/// implementation
+template <>
+inline bool less_vec<2>::operator()(const Eigen::Matrix<int, 2, 1>& v1, const Eigen::Matrix<int, 2, 1>& v2) const {
+    return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]);
+}
+
+template <>
+inline bool less_vec<3>::operator()(const Eigen::Matrix<int, 3, 1>& v1, const Eigen::Matrix<int, 3, 1>& v2) const {
+    return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]) && (v1[0] == v2[0] && v1[1] == v2[1] && v1[2] < v2[2]);
+}
+
+/// vec 2 hash
+/// @see Optimized Spatial Hashing for Collision Detection of Deformable Objects, Matthias Teschner et. al., VMV 2003
+template <>
+inline size_t hash_vec<2>::operator()(const Eigen::Matrix<int, 2, 1>& v) const {
+    return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943)) % 10000000;
+    //  100011001101111010001011101
+    //  000000001110011001110000111  * v[1]
+}
+
+/// vec 3 hash
+template <>
+inline size_t hash_vec<3>::operator()(const Eigen::Matrix<int, 3, 1>& v) const {
+    return size_t(((v[0]) * 73856093) ^ ((v[1]) * 471943) ^ ((v[2]) * 83492791)) % 10000000;
+}
+
+constexpr auto less_vec2i = [](const Vec2i& v1, const Vec2i& v2) {
+    return v1[0] < v2[0] || (v1[0] == v2[0] && v1[1] < v2[1]);
+};
+
+}  // namespace faster_lio
+
+#endif

lio/include/ivox3d/hilbert.hpp

@@ -0,0 +1,830 @@
+// 2021-09-18, https://github.com/spectral3d/hilbert_hpp is under MIT license.
+
+//Copyright (c) 2019 David Beynon
+//
+//Permission is hereby granted, free of charge, to any person obtaining a copy
+//of this software and associated documentation files (the "Software"), to deal
+//in the Software without restriction, including without limitation the rights
+//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//copies of the Software, and to permit persons to whom the Software is
+//furnished to do so, subject to the following conditions:
+//
+//The above copyright notice and this permission notice shall be included in all
+//copies or substantial portions of the Software.
+//
+//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+//SOFTWARE.
+
+#ifndef INCLUDED_HILBERT_HPP
+#define INCLUDED_HILBERT_HPP
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <cstdint>
+#include <limits>
+#include <type_traits>
+
+// Version 1.1 - 29 August 2019
+
+//
+// N dimensional hilbert curve encoding & decoding based on the paper
+// "Programming the Hilbert Curve" by John Skilling.
+//
+// The interface assumes an std::array of some unsigned integer type.  This
+// contains an index in lexographic order, or a set of coordinates on the
+// hilbert curve.
+//
+// Two implementations are included.
+//
+// hilbert::v1 contains a fairly straightforward implementation of the paper,
+// with standard looping constructs.
+//
+// hilbert::v2 performs uses template metaprogramming to unroll loops and
+// theoretically improve performance on some systems.
+//
+// v1 produces smaller code, which may be more performant on machines with
+// small caches.  v2 produces relatively large code that seems more efficient
+// on modern systems for dimension up to about 100.
+//
+// v2 should be built with -O3 for best results.  -O0 is extremely slow
+// on most systems.
+//
+
+
+// Interface is as follows:
+//
+// Find the position of a point on an N dimensional Hilbert Curve.
+//
+// Based on the paper "Programming the Hilbert Curve" by John Skilling.
+//
+// Index is encoded with most significant objects first.  Lexographic
+// sort order.
+//template<typename T, size_t N>
+//std::array<T, N>
+//IndexToPosition(std::array<T, N> const &in);
+
+// Find the index of a point on an N dimensional Hilbert Curve.
+//
+// Based on the paper "Programming the Hilbert Curve" by John Skilling.
+//
+// Index is encoded with most significant objects first.  Lexographic
+//  sort order.
+//template<typename T, size_t N>
+//std::array<T, N>
+//PositionToIndex(std::array<T, N> const &in);
+//
+
+
+namespace hilbert
+{
+    // Fairly straightforward implementation.  Loops are loops and code mostly
+    // does what one would expect.
+    namespace v1
+    {
+        namespace internal
+        {
+            // Extract bits from transposed form.
+            //
+            // e.g.
+            //
+            // a d g j    a b c d
+            // b e h k -> e f g h
+            // c f i l    i j k l
+            //
+            template<typename T, size_t N>
+            std::array<T, N>
+            UntransposeBits(std::array<T, N> const &in)
+            {
+                const size_t bits = std::numeric_limits<T>::digits;
+                const T high_bit(T(1) << (bits - 1));
+                const size_t bit_count(bits * N);
+
+                std::array<T, N> out;
+
+                std::fill(out.begin(), out.end(), 0);
+
+                // go through all bits in input, msb first.  Shift distances are
+                // from msb.
+                for(size_t b=0;b<bit_count;b++)
+                {
+                    size_t src_bit, dst_bit, src, dst;
+                    src = b % N;
+                    dst = b / bits;
+                    src_bit = b / N;
+                    dst_bit = b % bits;
+
+                    out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit);
+                }
+
+                return out;
+            }
+
+            // Pack bits into transposed form.
+            //
+            // e.g.
+            //
+            // a b c d    a d g j
+            // e f g h -> b e h k
+            // i j k l    c f i l
+            //
+            template<typename T, size_t N>
+            std::array<T, N>
+            TransposeBits(std::array<T, N> const &in)
+            {
+                const size_t bits = std::numeric_limits<T>::digits;
+                const T high_bit(T(1) << (bits - 1));
+                const size_t bit_count(bits * N);
+
+                std::array<T, N> out;
+
+                std::fill(out.begin(), out.end(), 0);
+
+                // go through all bits in input, msb first.  Shift distances
+                // are from msb.
+                for(size_t b=0;b<bit_count;b++)
+                {
+                    size_t src_bit, dst_bit, src, dst;
+                    src = b / bits;
+                    dst = b % N;
+                    src_bit = b % bits;
+                    dst_bit = b / N;
+
+                    out[dst] |= (((in[src] << src_bit) & high_bit) >> dst_bit);
+                }
+
+                return out;
+            }
+        }
+
+        //
+        // Public interfaces.
+        //
+
+        // Find the position of a point on an N dimensional Hilbert Curve.
+        //
+        // Based on the paper "Programming the Hilbert Curve" by John Skilling.
+        //
+        // Index is encoded with most significant objects first.  Lexographic
+        // sort order.
+        template<typename T, size_t N>
+        std::array<T, N>
+        IndexToPosition(
+                std::array<T, N> const &in)
+        {
+            // First convert index to transpose.
+            std::array<T, N> out(internal::TransposeBits(in));
+
+            // Initial gray encoding of transposed vector.
+            {
+                T tmp = out[N-1] >> 1;
+
+                for(size_t n=N-1;n;n--)
+                {
+                    out[n]^= out[n-1];
+                }
+
+                out[0]^= tmp;
+            }
+
+            // Apply transforms to gray code.
+            {
+                T cur_bit(2),
+                  low_bits;
+
+                while(cur_bit)
+                {
+                    low_bits = cur_bit - 1;
+
+                    size_t n(N);
+
+                    do
+                    {
+                        n--;
+                        if(out[n] & cur_bit)
+                        {
+                            // flip low bits of X
+                            out[0]^= low_bits;
+                        }
+                        else
+                        {
+                            // swap low bits with X
+                            T t((out[n] ^ out[0]) & low_bits);
+                            out[n]^= t;
+                            out[0]^= t;
+                        }
+                    }
+                    while(n);
+
+                    cur_bit<<= 1;
+                }
+            }
+
+            return out;
+        }
+
+        // Find the index of a point on an N dimensional Hilbert Curve.
+        //
+        // Based on the paper "Programming the Hilbert Curve" by John Skilling.
+        //
+        // Index is encoded with most significant objects first.  Lexographic
+        // sort order.
+        template<typename T, size_t N>
+        std::array<T, N>
+        PositionToIndex(std::array<T, N> const &in)
+        {
+            const size_t bits = std::numeric_limits<T>::digits;
+
+            std::array<T, N> out(in);
+
+            // reverse transforms to convert into transposed gray code.
+            {
+                T cur_bit(T(1) << (bits - 1)),
+                  low_bits;
+
+                do
+                {
+                    low_bits = cur_bit - 1;
+
+                    for(size_t n=0;n<N;n++)
+                    {
+                        if(out[n] & cur_bit)
+                        {
+                            // flip low bits of X
+                            out[0]^= low_bits;
+                        }
+                        else
+                        {
+                            // swap low bits with X
+                            T t((out[n] ^ out[0]) & low_bits);
+                            out[n]^= t;
+                            out[0]^= t;
+                        }
+                    }
+
+                    cur_bit>>= 1;
+                } while(low_bits > 1);
+            }
+
+            // Remove gray code from transposed vector.
+            {
+                T cur_bit(T(1) << (bits - 1)),
+                  t(0);
+
+                for(size_t n=1;n<N;n++)
+                {
+                    out[n]^= out[n-1];
+                }
+
+                do
+                {
+                    if(out[N-1] & cur_bit)
+                    {
+                        t^= (cur_bit - 1);
+                    }
+                    cur_bit>>= 1;
+                } while(cur_bit > 1);
+
+                for(auto &v : out)
+                {
+                    v^= t;
+                }
+            }
+
+            return internal::UntransposeBits(out);
+        }
+    } // namespace v1
+
+    // Implementation using metaprogramming to unroll most loops.
+    // Optimised performance should be superior to v1 provided all code remains
+    // in cache etc.
+    //
+    // At some value of N v1 should overtake v2.
+    namespace v2
+    {
+        namespace internal
+        {
+            // Metaprogramming guts.  Unrolled loops, abandon all hope etc.
+            namespace tmp
+            {
+                template<typename T, size_t N, size_t D>
+                T
+                TransposeBits2(
+                        std::array<T, N> const &,
+                        std::integral_constant<size_t, D>,
+                        std::integral_constant<size_t, 0>)
+                {
+                    return T(0);
+                }
+
+                template<typename T, size_t N, size_t D, size_t B>
+                T
+                TransposeBits2(
+                        std::array<T, N> const &in,
+                        std::integral_constant<size_t, D>,
+                        std::integral_constant<size_t, B>)
+                {
+                    const size_t size = std::numeric_limits<T>::digits;
+                    const size_t src = ((D + ((size - B) * N)) / size);
+                    const size_t src_bit =
+                            size - (1+((D + ((size - B) * N)) % size));
+                    const T src_bit_val = T(1) << src_bit;
+                    const size_t dst_bit = (B - 1);
+                    const T dst_bit_val = T(1) << dst_bit;
+
+                    // Multiply rather than shift to avoid clang implicit
+                    // conversion warning.
+                    T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val;
+
+                    return bit + TransposeBits2(
+                        in,
+                        std::integral_constant<size_t, D>(),
+                        std::integral_constant<size_t, B-1>());
+                }
+
+                template<typename T, size_t N>
+                void
+                TransposeBits(
+                        std::array<T, N> const &,
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t D>
+                void
+                TransposeBits(
+                        std::array<T, N> const &in,
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, D>)
+                {
+                    out[D-1] = TransposeBits2(
+                        in,
+                        std::integral_constant<size_t, D-1>(),
+                        std::integral_constant<
+                            size_t,
+                            std::numeric_limits<T>::digits>());
+
+                    TransposeBits(
+                        in,
+                        out,
+                        std::integral_constant<size_t, D-1>());
+                }
+
+
+                template<typename T, size_t N, size_t D>
+                T
+                UntransposeBits2(
+                        std::array<T, N> const &,
+                        std::integral_constant<size_t, D>,
+                        std::integral_constant<size_t, 0>)
+                {
+                    return T(0);
+                }
+
+                template<typename T, size_t N, size_t D, size_t B>
+                T
+                UntransposeBits2(
+                        std::array<T, N> const &in,
+                        std::integral_constant<size_t, D>,
+                        std::integral_constant<size_t, B>)
+                {
+                    const size_t size = std::numeric_limits<T>::digits;
+                    const size_t src = ((D * size) + (size - B)) % N;
+                    const size_t src_bit =
+                            size - (((((D * size) + (size - B))) / N) + 1);
+                    const T src_bit_val = T(1) << src_bit;
+                    const size_t dst_bit(B - 1);
+                    const T dst_bit_val = T(1) << dst_bit;
+
+                    // Multiply rather than shift to avoid clang implicit
+                    // conversion warning.
+                    T bit = ((in[src] & src_bit_val) >> src_bit) * dst_bit_val;
+
+                    return bit + UntransposeBits2(
+                        in,
+                        std::integral_constant<size_t, D>(),
+                        std::integral_constant<size_t, size_t(B-1)>());
+                }
+
+                template<typename T, size_t N>
+                void
+                UntransposeBits(
+                        std::array<T, N> const &,
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t D>
+                void
+                UntransposeBits(
+                        std::array<T, N> const &in,
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, D>)
+                {
+                    out[D-1] = UntransposeBits2(
+                        in,
+                        std::integral_constant<size_t, D-1>(),
+                        std::integral_constant<
+                            size_t,
+                            std::numeric_limits<T>::digits>());
+
+                    UntransposeBits(
+                        in,
+                        out,
+                        std::integral_constant<size_t, D-1>());
+                }
+
+                template<typename T, size_t N>
+                void
+                ApplyGrayCode1(
+                        std::array<T, N> const &in,
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, 0>)
+                {
+                    out[0]^= in[N-1] >> 1;
+                }
+
+                template<typename T, size_t N, size_t I>
+                void
+                ApplyGrayCode1(
+                        std::array<T, N> const &in,
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, I>)
+                {
+                    out[I]^= out[I-1];
+
+                    ApplyGrayCode1(
+                        in,
+                        out,
+                        std::integral_constant<size_t, I-1>());
+                }
+
+                // Remove a gray code from a transposed vector
+                template<typename T, size_t N>
+                void
+                RemoveGrayCode1(
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                // xor array values with previous values.
+                template<typename T, size_t N, size_t D>
+                void
+                RemoveGrayCode1(
+                        std::array<T, N> &in,
+                        std::integral_constant<size_t, D>)
+                {
+                    const size_t src_idx = N - (D + 1);
+                    const size_t dst_idx = N - D;
+
+                    in[dst_idx]^= in[src_idx];
+
+                    RemoveGrayCode1(in, std::integral_constant<size_t, D-1>());
+                }
+
+                template<typename T>
+                T RemoveGrayCode2(T, std::integral_constant<size_t, 1>)
+                {
+                    return T(0);
+                }
+
+                template<typename T, size_t B>
+                T RemoveGrayCode2(T v, std::integral_constant<size_t, B>)
+                {
+                    const T cur_bit(T(1) << (B-1));
+                    const T low_bits(cur_bit - 1);
+
+                    if(v & cur_bit)
+                    {
+                        return low_bits ^ RemoveGrayCode2(
+                            v,
+                            std::integral_constant<size_t, B-1>());
+                    }
+                    else
+                    {
+                        return RemoveGrayCode2(
+                            v,
+                            std::integral_constant<size_t, B-1>());
+                    }
+                }
+
+                template<typename T, size_t N, size_t B>
+                void
+                GrayToHilbert2(
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, B>,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t B, size_t I>
+                void
+                GrayToHilbert2(
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, B>,
+                        std::integral_constant<size_t, I>)
+                {
+                    const size_t n(I-1);
+                    const T cur_bit(T(1) << (std::numeric_limits<T>::digits - B));
+                    const T low_bits(cur_bit - 1);
+
+                    if(out[n] & cur_bit)
+                    {
+                        // flip low bits of X
+                        out[0]^= low_bits;
+                    }
+                    else
+                    {
+                        // swap low bits with X
+                        T t((out[n] ^ out[0]) & low_bits);
+                        out[n]^= t;
+                        out[0]^= t;
+                    }
+
+                    GrayToHilbert2(
+                        out,
+                        std::integral_constant<size_t, B>(),
+                        std::integral_constant<size_t, I-1>());
+                }
+
+                template<typename T, size_t N>
+                void
+                GrayToHilbert(
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t B>
+                void
+                GrayToHilbert(
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, B>)
+                {
+                    GrayToHilbert2(
+                        out,
+                        std::integral_constant<size_t, B>(),
+                        std::integral_constant<size_t, N>());
+
+                    GrayToHilbert(out, std::integral_constant<size_t, B-1>());
+                }
+
+                template<typename T, size_t N, size_t B>
+                void
+                HilbertToGray2(
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, B>,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t B, size_t I>
+                void
+                HilbertToGray2(
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, B>,
+                        std::integral_constant<size_t, I>)
+                {
+                    const size_t cur_bit(T(1) << B);
+                    const size_t low_bits(cur_bit-1);
+                    const size_t n(N-I);
+
+                    if(out[n] & cur_bit)
+                    {
+                        // flip low bits of X
+                        out[0]^= low_bits;
+                    }
+                    else
+                    {
+                        // swap low bits with X
+                        T t((out[n] ^ out[0]) & low_bits);
+                        out[n]^= t;
+                        out[0]^= t;
+                    }
+
+                    HilbertToGray2(
+                        out,
+                        std::integral_constant<size_t, B>(),
+                        std::integral_constant<size_t, I-1>());
+                }
+
+                template<typename T, size_t N>
+                void
+                HilbertToGray(
+                        std::array<T, N> &,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t B>
+                void
+                HilbertToGray(
+                        std::array<T, N> &out,
+                        std::integral_constant<size_t, B>)
+                {
+                    HilbertToGray2(
+                        out,
+                        std::integral_constant<size_t, B>(),
+                        std::integral_constant<size_t, N>());
+
+                    HilbertToGray(out, std::integral_constant<size_t, B-1>());
+                }
+
+                template<typename T, size_t N>
+                void
+                ApplyMaskToArray(
+                        std::array<T, N> &,
+                        T,
+                        std::integral_constant<size_t, 0>)
+                {
+                }
+
+                template<typename T, size_t N, size_t I>
+                void
+                ApplyMaskToArray(
+                        std::array<T, N> &a,
+                        T mask, std::integral_constant<size_t, I>)
+                {
+                    a[I-1]^= mask;
+
+                    ApplyMaskToArray(
+                        a,
+                        mask,
+                        std::integral_constant<size_t, I-1>());
+                }
+            } // namespace tmp
+
+
+            // Pack bits into transposed form.
+            //
+            // e.g.
+            //
+            // a b c d    a d g j
+            // e f g h -> b e h k
+            // i j k l    c f i l
+            //
+            template<typename T, size_t N>
+            std::array<T, N>
+            TransposeBits(std::array<T, N> const &in)
+            {
+                std::array<T, N> out;
+
+                std::fill(out.begin(), out.end(), 0);
+
+                tmp::TransposeBits(
+                    in,
+                    out,
+                    std::integral_constant<size_t, N>());
+
+                return out;
+            }
+
+            // Extract bits from transposed form.
+            // e.g.
+            //
+            // a d g j    a b c d
+            // b e h k -> e f g h
+            // c f i l    i j k l
+            //
+            template<typename T, size_t N>
+            std::array<T, N>
+            UntransposeBits(std::array<T, N> const &in)
+            {
+                std::array<T, N> out;
+
+                std::fill(out.begin(), out.end(), 0);
+
+                tmp::UntransposeBits(
+                    in,
+                    out,
+                    std::integral_constant<size_t, N>());
+
+                return out;
+            }
+
+
+            // Apply a gray code to a transformed vector.
+            template<typename T, size_t N>
+            std::array<T, N>
+            ApplyGrayCode(std::array<T, N> const &in)
+            {
+                std::array<T, N> out(in);
+
+                tmp::ApplyGrayCode1(
+                    in,
+                    out,
+                    std::integral_constant<size_t, N-1>());
+
+                return out;
+            }
+
+            template<typename T, size_t N>
+            std::array<T, N>
+            RemoveGrayCode(std::array<T, N> const &in)
+            {
+                const size_t bits = std::numeric_limits<T>::digits;
+                std::array<T, N> out(in);
+
+                // Remove gray code from transposed vector.
+                {
+                    // xor values with prev values.
+                    tmp::RemoveGrayCode1(
+                        out,
+                        std::integral_constant<size_t, N-1>());
+
+                    // create a mask.
+                    T t = tmp::RemoveGrayCode2(
+                        out[N-1],
+                        std::integral_constant<size_t, bits>());
+
+                    // Apply mask to output.
+                    tmp::ApplyMaskToArray(
+                        out,
+                        t,
+                        std::integral_constant<size_t, N>());
+                }
+
+                return out;
+            }
+
+            // Generate code to convert from a transposed gray code to a hilbert
+            // code.
+            template<typename T, size_t N>
+            std::array<T, N>
+            GrayToHilbert(std::array<T, N> const &in)
+            {
+                std::array<T, N> out(in);
+
+                tmp::GrayToHilbert(
+                    out,
+                    std::integral_constant<
+                        size_t,
+                        std::numeric_limits<T>::digits - 1>());
+
+                return out;
+            }
+
+            // Generate code to convert from a hilbert code to a transposed gray
+            // code.
+            template<typename T, size_t N>
+            std::array<T, N>
+            HilbertToGray(std::array<T, N> const &in)
+            {
+                std::array<T, N> out(in);
+
+                tmp::HilbertToGray(
+                    out,
+                    std::integral_constant<
+                        size_t,
+                        std::numeric_limits<T>::digits-1>());
+
+                return out;
+            }
+        }
+
+        //
+        // Public interfaces.
+        //
+
+        // Find the position of a point on an N dimensional Hilbert Curve.
+        //
+        // Based on the paper "Programming the Hilbert Curve" by John Skilling.
+        //
+        // Index is encoded with most significant objects first.  Lexographic
+        // sort order.
+        template<typename T, size_t N>
+        std::array<T, N>
+        IndexToPosition(std::array<T, N> const &in)
+        {
+            // First convert index to transpose.
+            return internal::GrayToHilbert(
+                internal::ApplyGrayCode(
+                    internal::TransposeBits(in)));
+        }
+
+        // Find the index of a point on an N dimensional Hilbert Curve.
+        //
+        // Based on the paper "Programming the Hilbert Curve" by John Skilling.
+        //
+        // Index is encoded with most significant objects first.  Lexographic
+        //  sort order.
+        template<typename T, size_t N>
+        std::array<T, N>
+        PositionToIndex(std::array<T, N> const &in)
+        {
+            return internal::UntransposeBits(
+                internal::RemoveGrayCode(
+                    internal::HilbertToGray(in)));
+        }
+    } // namespace v2
+} // namespace hilbert
+
+#endif

lio/include/ivox3d/ivox3d.h

@@ -0,0 +1,347 @@
+//
+// Created by xiang on 2021/9/16.
+//
+
+#ifndef FASTER_LIO_IVOX3D_H
+#define FASTER_LIO_IVOX3D_H
+
+#include <glog/logging.h>
+#include <execution>
+#include <list>
+#include <thread>
+
+#include "eigen_types.h"
+#include "ivox3d_node.hpp"
+
+namespace faster_lio {
+
+enum class IVoxNodeType {
+    DEFAULT,  // linear ivox
+    PHC,      // phc ivox
+};
+
+/// traits for NodeType
+template <IVoxNodeType node_type, typename PointT, int dim>
+struct IVoxNodeTypeTraits {};
+
+template <typename PointT, int dim>
+struct IVoxNodeTypeTraits<IVoxNodeType::DEFAULT, PointT, dim> {
+    using NodeType = IVoxNode<PointT, dim>;
+};
+
+template <typename PointT, int dim>
+struct IVoxNodeTypeTraits<IVoxNodeType::PHC, PointT, dim> {
+    using NodeType = IVoxNodePhc<PointT, dim>;
+};
+
+template <int dim = 3, IVoxNodeType node_type = IVoxNodeType::DEFAULT, typename PointType = pcl::PointXYZ>
+class IVox {
+   public:
+    using KeyType = Eigen::Matrix<int, dim, 1>;
+    using PtType = Eigen::Matrix<float, dim, 1>;
+    using NodeType = typename IVoxNodeTypeTraits<node_type, PointType, dim>::NodeType;
+    using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
+    using DistPoint = typename NodeType::DistPoint;
+
+    enum class NearbyType {
+        CENTER,  // center only
+        NEARBY6,
+        NEARBY18,
+        NEARBY26,
+    };
+
+    struct Options {
+        float resolution_ = 0.2;                        // ivox resolution
+        float inv_resolution_ = 10.0;                   // inverse resolution
+        NearbyType nearby_type_ = NearbyType::NEARBY6;  // nearby range
+        std::size_t capacity_ = 1000000;                // capacity
+    };
+
+    /**
+     * constructor
+     * @param options  ivox options
+     */
+    explicit IVox(Options options) : options_(options) {
+        options_.inv_resolution_ = 1.0 / options_.resolution_;
+        GenerateNearbyGrids();
+    }
+
+    /**
+     * add points
+     * @param points_to_add
+     */
+    void AddPoints(const PointVector& points_to_add);
+
+    void GetPoints(PointVector& points);
+
+    void DownSample(double leafsize=0.02);
+    /// get nn
+    bool GetClosestPoint(const PointType& pt, PointType& closest_pt);
+
+    /// get nn with condition
+    bool GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num = 5, double max_range = 5.0);
+
+    /// get nn in cloud
+    bool GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud);
+
+    /// get number of points
+    size_t NumPoints() const;
+
+    /// get number of valid grids
+    size_t NumValidGrids() const;
+
+    /// get statistics of the points
+    std::vector<float> StatGridPoints() const;
+
+   public:
+    /// generate the nearby grids according to the given options
+    void GenerateNearbyGrids();
+
+    /// position to grid
+    KeyType Pos2Grid(const PtType& pt) const;
+
+    Options options_;
+    std::unordered_map<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, hash_vec<dim>>
+        grids_map_;                                        // voxel hash map
+    std::list<std::pair<KeyType, NodeType>> grids_cache_;  // voxel cache
+    std::vector<KeyType> nearby_grids_;                    // nearbys
+};
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointType& pt, PointType& closest_pt) {
+    std::vector<DistPoint> candidates;
+    auto key = Pos2Grid(ToEigen<float, dim>(pt));
+    std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &candidates, &pt, this](const KeyType& delta) {
+        auto dkey = key + delta;
+        auto iter = grids_map_.find(dkey);
+        if (iter != grids_map_.end()) {
+            DistPoint dist_point;
+            bool found = iter->second->second.NNPoint(pt, dist_point);
+            if (found) {
+                candidates.emplace_back(dist_point);
+            }
+        }
+    });
+
+    if (candidates.empty()) {
+        return false;
+    }
+
+    auto iter = std::min_element(candidates.begin(), candidates.end());
+    closest_pt = iter->Get();
+    return true;
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointType& pt, PointVector& closest_pt, int max_num,
+                                                      double max_range) {
+    std::vector<DistPoint> candidates;
+    candidates.reserve(max_num * nearby_grids_.size());
+
+    auto key = Pos2Grid(ToEigen<float, dim>(pt));
+
+// #define INNER_TIMER
+#ifdef INNER_TIMER
+    static std::unordered_map<std::string, std::vector<int64_t>> stats;
+    if (stats.empty()) {
+        stats["knn"] = std::vector<int64_t>();
+        stats["nth"] = std::vector<int64_t>();
+    }
+#endif
+
+    for (const KeyType& delta : nearby_grids_) {
+        auto dkey = key + delta;
+        auto iter = grids_map_.find(dkey);
+        if (iter != grids_map_.end()) {
+#ifdef INNER_TIMER
+            auto t1 = std::chrono::high_resolution_clock::now();
+#endif
+            auto tmp = iter->second->second.KNNPointByCondition(candidates, pt, max_num, max_range);
+#ifdef INNER_TIMER
+            auto t2 = std::chrono::high_resolution_clock::now();
+            auto knn = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
+            stats["knn"].emplace_back(knn);
+#endif
+        }
+    }
+
+    if (candidates.empty()) {
+        return false;
+    }
+
+#ifdef INNER_TIMER
+    auto t1 = std::chrono::high_resolution_clock::now();
+#endif
+
+    if (candidates.size() <= max_num) {
+    } else {
+        std::nth_element(candidates.begin(), candidates.begin() + max_num - 1, candidates.end());
+        candidates.resize(max_num);
+    }
+    std::nth_element(candidates.begin(), candidates.begin(), candidates.end());
+
+#ifdef INNER_TIMER
+    auto t2 = std::chrono::high_resolution_clock::now();
+    auto nth = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
+    stats["nth"].emplace_back(nth);
+
+    constexpr int STAT_PERIOD = 100000;
+    if (!stats["nth"].empty() && stats["nth"].size() % STAT_PERIOD == 0) {
+        for (auto& it : stats) {
+            const std::string& key = it.first;
+            std::vector<int64_t>& stat = it.second;
+            int64_t sum_ = std::accumulate(stat.begin(), stat.end(), 0);
+            int64_t num_ = stat.size();
+            stat.clear();
+            std::cout << "inner_" << key << "(ns): sum=" << sum_ << " num=" << num_ << " ave=" << 1.0 * sum_ / num_
+                      << " ave*n=" << 1.0 * sum_ / STAT_PERIOD << std::endl;
+        }
+    }
+#endif
+
+    closest_pt.clear();
+    for (auto& it : candidates) {
+        closest_pt.emplace_back(it.Get());
+    }
+    return closest_pt.empty() == false;
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+size_t IVox<dim, node_type, PointType>::NumPoints() const
+{
+    int num=0;
+    for( auto it=grids_map_.begin();it!=grids_map_.end();++it)
+    {
+        num+=it->second->second.Size();
+    }
+    return num;
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+size_t IVox<dim, node_type, PointType>::NumValidGrids() const {
+    return grids_map_.size();
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+void IVox<dim, node_type, PointType>::DownSample(double leafsize)
+{
+    for( auto it=grids_map_.begin();it!=grids_map_.end();++it)
+    {
+        it->second->second.DownSample(leafsize);
+    }
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+void IVox<dim, node_type, PointType>::GenerateNearbyGrids() {
+    if (options_.nearby_type_ == NearbyType::CENTER) {
+        nearby_grids_.emplace_back(KeyType::Zero());
+    } else if (options_.nearby_type_ == NearbyType::NEARBY6) {
+        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
+                KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
+    } else if (options_.nearby_type_ == NearbyType::NEARBY18) {
+        nearby_grids_ = {KeyType(0, 0, 0),  KeyType(-1, 0, 0), KeyType(1, 0, 0),   KeyType(0, 1, 0),
+                KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1),   KeyType(1, 1, 0),
+                KeyType(-1, 1, 0), KeyType(1, -1, 0), KeyType(-1, -1, 0), KeyType(1, 0, 1),
+                KeyType(-1, 0, 1), KeyType(1, 0, -1), KeyType(-1, 0, -1), KeyType(0, 1, 1),
+                KeyType(0, -1, 1), KeyType(0, 1, -1), KeyType(0, -1, -1)};
+    } else if (options_.nearby_type_ == NearbyType::NEARBY26) {
+        nearby_grids_ = {KeyType(0, 0, 0),   KeyType(-1, 0, 0),  KeyType(1, 0, 0),   KeyType(0, 1, 0),
+                KeyType(0, -1, 0),  KeyType(0, 0, -1),  KeyType(0, 0, 1),   KeyType(1, 1, 0),
+                KeyType(-1, 1, 0),  KeyType(1, -1, 0),  KeyType(-1, -1, 0), KeyType(1, 0, 1),
+                KeyType(-1, 0, 1),  KeyType(1, 0, -1),  KeyType(-1, 0, -1), KeyType(0, 1, 1),
+                KeyType(0, -1, 1),  KeyType(0, 1, -1),  KeyType(0, -1, -1), KeyType(1, 1, 1),
+                KeyType(-1, 1, 1),  KeyType(1, -1, 1),  KeyType(1, 1, -1),  KeyType(-1, -1, 1),
+                KeyType(-1, 1, -1), KeyType(1, -1, -1), KeyType(-1, -1, -1)};
+    } else {
+        LOG(ERROR) << "Unknown nearby_type!";
+    }
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+bool IVox<dim, node_type, PointType>::GetClosestPoint(const PointVector& cloud, PointVector& closest_cloud) {
+    std::vector<size_t> index(cloud.size());
+    for (int i = 0; i < cloud.size(); ++i) {
+        index[i] = i;
+    }
+    closest_cloud.resize(cloud.size());
+
+    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&cloud, &closest_cloud, this](size_t idx) {
+        PointType pt;
+        if (GetClosestPoint(cloud[idx], pt)) {
+            closest_cloud[idx] = pt;
+        } else {
+            closest_cloud[idx] = PointType();
+        }
+    });
+    return true;
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+void IVox<dim, node_type, PointType>::GetPoints(PointVector& points)
+{
+    points.clear();
+
+    for(auto it=grids_cache_.begin();it!=grids_cache_.end();it++)
+    {
+        std::vector<PointType> node_points=it->second.Points();
+        points.insert(points.end(),node_points.begin(),node_points.end());
+    }
+
+}
+
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+void IVox<dim, node_type, PointType>::AddPoints(const PointVector& points_to_add)
+{
+    for (int i = 0; i < points_to_add.size(); ++i)
+    {
+        PointType pt = points_to_add[i];
+        auto key = Pos2Grid(ToEigen<float, dim>(pt));
+
+        auto iter = grids_map_.find(key);
+        if (iter == grids_map_.end()) {
+            PointType center;
+            center.getVector3fMap() = key.template cast<float>() * options_.resolution_;
+
+            grids_cache_.push_front({key, NodeType(center, options_.resolution_)});
+            grids_map_.insert({key, grids_cache_.begin()});
+
+            grids_cache_.front().second.InsertPoint(pt);
+
+            if (grids_map_.size() >= options_.capacity_) {
+                grids_map_.erase(grids_cache_.back().first);
+                grids_cache_.pop_back();
+            }
+        } else {
+            iter->second->second.InsertPoint(pt);
+            grids_cache_.splice(grids_cache_.begin(), grids_cache_, iter->second);
+            grids_map_[key] = grids_cache_.begin();
+        }
+    }
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+Eigen::Matrix<int, dim, 1> IVox<dim, node_type, PointType>::Pos2Grid(const IVox::PtType& pt) const {
+    return (pt * options_.inv_resolution_).array().round().template cast<int>();
+}
+
+template <int dim, IVoxNodeType node_type, typename PointType>
+std::vector<float> IVox<dim, node_type, PointType>::StatGridPoints() const {
+    int num = grids_cache_.size(), valid_num = 0, max = 0, min = 100000000;
+    int sum = 0, sum_square = 0;
+    for (auto& it : grids_cache_) {
+        int s = it.second.Size();
+        valid_num += s > 0;
+        max = s > max ? s : max;
+        min = s < min ? s : min;
+        sum += s;
+        sum_square += s * s;
+    }
+    float ave = float(sum) / num;
+    float stddev = num > 1 ? sqrt((float(sum_square) - num * ave * ave) / (num - 1)) : 0;
+    return std::vector<float>{valid_num, ave, max, min, stddev};
+}
+
+}  // namespace faster_lio
+
+#endif

lio/include/ivox3d/ivox3d_node.hpp

@@ -0,0 +1,434 @@
+#include <pcl/common/centroid.h>
+#include <algorithm>
+#include <cmath>
+#include <list>
+#include <vector>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include "hilbert.hpp"
+
+namespace faster_lio {
+
+// squared distance of two pcl points
+template <typename PointT>
+inline double distance2(const PointT& pt1, const PointT& pt2) {
+    Eigen::Vector3f d = pt1.getVector3fMap() - pt2.getVector3fMap();
+    return d.squaredNorm();
+}
+
+// convert from pcl point to eigen
+template <typename T, int dim, typename PointType>
+inline Eigen::Matrix<T, dim, 1> ToEigen(const PointType& pt) {
+    return Eigen::Matrix<T, dim, 1>(pt.x, pt.y, pt.z);
+}
+
+template <>
+inline Eigen::Matrix<float, 3, 1> ToEigen<float, 3, pcl::PointXYZ>(const pcl::PointXYZ& pt) {
+    return pt.getVector3fMap();
+}
+
+template <>
+inline Eigen::Matrix<float, 3, 1> ToEigen<float, 3, pcl::PointXYZI>(const pcl::PointXYZI& pt) {
+    return pt.getVector3fMap();
+}
+
+template <>
+inline Eigen::Matrix<float, 3, 1> ToEigen<float, 3, pcl::PointXYZINormal>(const pcl::PointXYZINormal& pt) {
+    return pt.getVector3fMap();
+}
+
+template <typename PointT, int dim = 3>
+class IVoxNode {
+ public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+    struct DistPoint;
+
+    IVoxNode() = default;
+    IVoxNode(const PointT& center, const float& side_length) {}  /// same with phc
+
+    void InsertPoint(const PointT& pt);
+
+    std::vector<PointT> Points(){return points_;}
+
+    inline bool Empty() const;
+
+    inline std::size_t Size() const;
+
+    void DownSample(double leafSize=0.02);
+
+    inline PointT GetPoint(const std::size_t idx) const;
+
+    int KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& point, const int& K,
+                            const double& max_range);
+
+    int UpdateRecord();
+
+ private:
+
+    std::vector<PointT> points_;
+};
+
+template <typename PointT, int dim = 3>
+class IVoxNodePhc {
+ public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+    struct DistPoint;
+    struct PhcCube;
+
+    IVoxNodePhc() = default;
+    IVoxNodePhc(const PointT& center, const float& side_length, const int& phc_order = 6);
+
+    void InsertPoint(const PointT& pt);
+
+
+
+    void ErasePoint(const PointT& pt, const double erase_distance_th_);
+
+    inline bool Empty() const;
+
+    inline std::size_t Size() const;
+
+    PointT GetPoint(const std::size_t idx) const;
+
+    bool NNPoint(const PointT& cur_pt, DistPoint& dist_point) const;
+
+    int KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& cur_pt, const int& K = 5,
+                            const double& max_range = 5.0);
+
+ private:
+    uint32_t CalculatePhcIndex(const PointT& pt) const;
+
+ private:
+    std::vector<PhcCube> phc_cubes_;
+
+    PointT center_;
+    float side_length_ = 0;
+    int phc_order_ = 6;
+    float phc_side_length_ = 0;
+    float phc_side_length_inv_ = 0;
+    Eigen::Matrix<float, dim, 1> min_cube_;
+};
+
+template <typename PointT, int dim>
+struct IVoxNode<PointT, dim>::DistPoint {
+    double dist = 0;
+    IVoxNode* node = nullptr;
+    int idx = 0;
+
+    DistPoint() = default;
+    DistPoint(const double d, IVoxNode* n, const int i) : dist(d), node(n), idx(i) {}
+
+    PointT Get() { return node->GetPoint(idx); }
+
+    inline bool operator()(const DistPoint& p1, const DistPoint& p2) { return p1.dist < p2.dist; }
+
+    inline bool operator<(const DistPoint& rhs) { return dist < rhs.dist; }
+};
+
+template <typename PointT, int dim>
+void IVoxNode<PointT, dim>::InsertPoint(const PointT& pt) {
+    points_.emplace_back(pt);
+}
+
+
+template <typename PointT, int dim>
+void IVoxNode<PointT, dim>::DownSample(double leafSize)
+{
+    /*pcl::PointCloud<PointT> result;
+    pcl::VoxelGrid<PointT> grid;
+    grid.setLeafSize(leafSize,leafSize,leafSize);
+    grid.setInputCloud(points_.makeShared());
+    grid.filter(result);
+    points_=result;*/
+}
+
+template <typename PointT, int dim>
+bool IVoxNode<PointT, dim>::Empty() const {
+    return points_.empty();
+}
+
+template <typename PointT, int dim>
+std::size_t IVoxNode<PointT, dim>::Size() const {
+    return points_.size();
+}
+
+template <typename PointT, int dim>
+PointT IVoxNode<PointT, dim>::GetPoint(const std::size_t idx) const {
+    return points_[idx];
+}
+
+template <typename PointT, int dim>
+int IVoxNode<PointT, dim>::KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& point, const int& K,
+                                               const double& max_range) {
+    std::size_t old_size = dis_points.size();
+// #define INNER_TIMER
+#ifdef INNER_TIMER
+    static std::unordered_map<std::string, std::vector<int64_t>> stats;
+    if (stats.empty()) {
+        stats["dis"] = std::vector<int64_t>();
+        stats["put"] = std::vector<int64_t>();
+        stats["nth"] = std::vector<int64_t>();
+    }
+#endif
+
+    for (const auto& pt : points_) {
+#ifdef INNER_TIMER
+        auto t0 = std::chrono::high_resolution_clock::now();
+#endif
+        double d = distance2(pt, point);
+#ifdef INNER_TIMER
+        auto t1 = std::chrono::high_resolution_clock::now();
+#endif
+        if (d < max_range * max_range) {
+            dis_points.template emplace_back(DistPoint(d, this, &pt - points_.data()));
+        }
+#ifdef INNER_TIMER
+        auto t2 = std::chrono::high_resolution_clock::now();
+
+        auto dis = std::chrono::duration_cast<std::chrono::nanoseconds>(t1 - t0).count();
+        stats["dis"].emplace_back(dis);
+        auto put = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
+        stats["put"].emplace_back(put);
+#endif
+    }
+
+#ifdef INNER_TIMER
+    auto t1 = std::chrono::high_resolution_clock::now();
+#endif
+    // sort by distance
+    if (old_size + K >= dis_points.size()) {
+    } else {
+        std::nth_element(dis_points.begin() + old_size, dis_points.begin() + old_size + K - 1, dis_points.end());
+        dis_points.resize(old_size + K);
+    }
+
+#ifdef INNER_TIMER
+    auto t2 = std::chrono::high_resolution_clock::now();
+    auto nth = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count();
+    stats["nth"].emplace_back(nth);
+
+    constexpr int STAT_PERIOD = 100000;
+    if (!stats["nth"].empty() && stats["nth"].size() % STAT_PERIOD == 0) {
+        for (auto& it : stats) {
+            const std::string& key = it.first;
+            std::vector<int64_t>& stat = it.second;
+            int64_t sum_ = std::accumulate(stat.begin(), stat.end(), 0);
+            int64_t num_ = stat.size();
+            stat.clear();
+            std::cout << "inner_" << key << "(ns): sum=" << sum_ << " num=" << num_ << " ave=" << 1.0 * sum_ / num_
+                      << " ave*n=" << 1.0 * sum_ / STAT_PERIOD << std::endl;
+        }
+    }
+#endif
+
+    return dis_points.size();
+}
+
+template <typename PointT, int dim>
+struct IVoxNodePhc<PointT, dim>::DistPoint {
+    double dist = 0;
+    IVoxNodePhc* node = nullptr;
+    int idx = 0;
+
+    DistPoint() {}
+    DistPoint(const double d, IVoxNodePhc* n, const int i) : dist(d), node(n), idx(i) {}
+
+    PointT Get() { return node->GetPoint(idx); }
+
+    inline bool operator()(const DistPoint& p1, const DistPoint& p2) { return p1.dist < p2.dist; }
+
+    inline bool operator<(const DistPoint& rhs) { return dist < rhs.dist; }
+};
+
+template <typename PointT, int dim>
+struct IVoxNodePhc<PointT, dim>::PhcCube {
+    uint32_t idx = 0;
+    pcl::CentroidPoint<PointT> mean;
+
+    PhcCube(uint32_t index, const PointT& pt) { mean.add(pt); }
+
+    void AddPoint(const PointT& pt) { mean.add(pt); }
+
+    PointT GetPoint() const {
+        PointT pt;
+        mean.get(pt);
+        return std::move(pt);
+    }
+};
+
+template <typename PointT, int dim>
+IVoxNodePhc<PointT, dim>::IVoxNodePhc(const PointT& center, const float& side_length, const int& phc_order)
+        : center_(center), side_length_(side_length), phc_order_(phc_order) {
+    assert(phc_order <= 8);
+    phc_side_length_ = side_length_ / (std::pow(2, phc_order_));
+    phc_side_length_inv_ = (std::pow(2, phc_order_)) / side_length_;
+    min_cube_ = center_.getArray3fMap() - side_length / 2.0;
+    phc_cubes_.reserve(64);
+}
+
+template <typename PointT, int dim>
+void IVoxNodePhc<PointT, dim>::InsertPoint(const PointT& pt) {
+    uint32_t idx = CalculatePhcIndex(pt);
+
+    PhcCube cube{idx, pt};
+    auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
+                               [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
+
+    if (it == phc_cubes_.end()) {
+        phc_cubes_.emplace_back(cube);
+    } else {
+        if (it->idx == idx) {
+            it->AddPoint(pt);
+        } else {
+            phc_cubes_.insert(it, cube);
+        }
+    }
+}
+
+template <typename PointT, int dim>
+void IVoxNodePhc<PointT, dim>::ErasePoint(const PointT& pt, const double erase_distance_th_) {
+    uint32_t idx = CalculatePhcIndex(pt);
+
+    PhcCube cube{idx, pt};
+    auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
+                               [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
+
+    if (erase_distance_th_ > 0) {
+    }
+    if (it != phc_cubes_.end() && it->idx == idx) {
+        phc_cubes_.erase(it);
+    }
+}
+
+template <typename PointT, int dim>
+bool IVoxNodePhc<PointT, dim>::Empty() const {
+    return phc_cubes_.empty();
+}
+
+template <typename PointT, int dim>
+std::size_t IVoxNodePhc<PointT, dim>::Size() const {
+    return phc_cubes_.size();
+}
+
+template <typename PointT, int dim>
+PointT IVoxNodePhc<PointT, dim>::GetPoint(const std::size_t idx) const {
+    return phc_cubes_[idx].GetPoint();
+}
+
+template <typename PointT, int dim>
+bool IVoxNodePhc<PointT, dim>::NNPoint(const PointT& cur_pt, DistPoint& dist_point) const {
+    if (phc_cubes_.empty()) {
+        return false;
+    }
+    uint32_t cur_idx = CalculatePhcIndex(cur_pt);
+    PhcCube cube{cur_idx, cur_pt};
+    auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
+                               [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
+
+    if (it == phc_cubes_.end()) {
+        it--;
+        dist_point = DistPoint(distance2(cur_pt, it->GetPoint()), this, it - phc_cubes_.begin());
+    } else if (it == phc_cubes_.begin()) {
+        dist_point = DistPoint(distance2(cur_pt, it->GetPoint()), this, it - phc_cubes_.begin());
+    } else {
+        auto last_it = it;
+        last_it--;
+        double d1 = distance2(cur_pt, it->GetPoint());
+        double d2 = distance2(cur_pt, last_it->GetPoint());
+        if (d1 > d2) {
+            dist_point = DistPoint(d2, this, it - phc_cubes_.begin());
+        } else {
+            dist_point = DistPoint(d1, this, it - phc_cubes_.begin());
+        }
+    }
+
+    return true;
+}
+
+template <typename PointT, int dim>
+int IVoxNodePhc<PointT, dim>::KNNPointByCondition(std::vector<DistPoint>& dis_points, const PointT& cur_pt,
+                                                  const int& K, const double& max_range) {
+    uint32_t cur_idx = CalculatePhcIndex(cur_pt);
+    PhcCube cube{cur_idx, cur_pt};
+    auto it = std::lower_bound(phc_cubes_.begin(), phc_cubes_.end(), cube,
+                               [](const PhcCube& a, const PhcCube& b) { return a.idx < b.idx; });
+
+    const int max_search_cube_side_length = std::pow(2, std::ceil(std::log2(max_range * phc_side_length_inv_)));
+    const int max_search_idx_th =
+            8 * max_search_cube_side_length * max_search_cube_side_length * max_search_cube_side_length;
+
+    auto create_dist_point = [&cur_pt, this](typename std::vector<PhcCube>::const_iterator forward_it) {
+      double d = distance2(forward_it->GetPoint(), cur_pt);
+      return DistPoint(d, this, forward_it - phc_cubes_.begin());
+    };
+
+    typename std::vector<PhcCube>::const_iterator forward_it(it);
+    typename std::vector<PhcCube>::const_reverse_iterator backward_it(it);
+    if (it != phc_cubes_.end()) {
+        dis_points.emplace_back(create_dist_point(it));
+        forward_it++;
+    }
+    if (backward_it != phc_cubes_.rend()) {
+        backward_it++;
+    }
+
+    auto forward_reach_boundary = [&]() {
+      return forward_it == phc_cubes_.end() || forward_it->idx - cur_idx > max_search_idx_th;
+    };
+    auto backward_reach_boundary = [&]() {
+      return backward_it == phc_cubes_.rend() || cur_idx - backward_it->idx > max_search_idx_th;
+    };
+
+    while (!forward_reach_boundary() && !backward_reach_boundary()) {
+        if (forward_it->idx - cur_idx > cur_idx - backward_it->idx) {
+            dis_points.emplace_back(create_dist_point(forward_it));
+            forward_it++;
+        } else {
+            dis_points.emplace_back(create_dist_point(backward_it.base()));
+            backward_it++;
+        }
+        if (dis_points.size() > K) {
+            break;
+        }
+    }
+
+    if (forward_reach_boundary()) {
+        while (!backward_reach_boundary() && dis_points.size() < K) {
+            dis_points.emplace_back(create_dist_point(backward_it.base()));
+            backward_it++;
+        }
+    }
+
+    if (backward_reach_boundary()) {
+        while (!forward_reach_boundary() && dis_points.size() < K) {
+            dis_points.emplace_back(create_dist_point(forward_it));
+            forward_it++;
+        }
+    }
+
+    return dis_points.size();
+}
+
+template <typename PointT, int dim>
+uint32_t IVoxNodePhc<PointT, dim>::CalculatePhcIndex(const PointT& pt) const {
+    Eigen::Matrix<float, dim, 1> eposf = (pt.getVector3fMap() - min_cube_) * phc_side_length_inv_;
+    Eigen::Matrix<int, dim, 1> eposi = eposf.template cast<int>();
+    for (int i = 0; i < dim; ++i) {
+        if (eposi(i, 0) < 0) {
+            eposi(i, 0) = 0;
+        }
+        if (eposi(i, 0) > std::pow(2, phc_order_)) {
+            eposi(i, 0) = std::pow(2, phc_order_) - 1;
+        }
+    }
+    std::array<uint8_t, 3> apos{eposi(0), eposi(1), eposi(2)};
+    std::array<uint8_t, 3> tmp = hilbert::v2::PositionToIndex(apos);
+
+    uint32_t idx = (uint32_t(tmp[0]) << 16) + (uint32_t(tmp[1]) << 8) + (uint32_t(tmp[2]));
+    return idx;
+}
+
+}  // namespace faster_lio

lio/include/sophus/average.hpp

@@ -0,0 +1,231 @@
+/// @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

lio/include/sophus/common.hpp

@@ -0,0 +1,201 @@
+/// @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

lio/include/sophus/example_ensure_handler.cpp

@@ -0,0 +1,14 @@
+#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

lio/include/sophus/formatstring.hpp

@@ -0,0 +1,72 @@
+/// @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

lio/include/sophus/geometry.hpp

@@ -0,0 +1,179 @@
+/// @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

lio/include/sophus/interpolate.hpp

@@ -0,0 +1,38 @@
+/// @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

lio/include/sophus/interpolate_details.hpp

@@ -0,0 +1,104 @@
+#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

lio/include/sophus/num_diff.hpp

@@ -0,0 +1,93 @@
+/// @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

lio/include/sophus/rotation_matrix.hpp

@@ -0,0 +1,84 @@
+/// @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

lio/include/sophus/rxso2.hpp

@@ -0,0 +1,660 @@
+/// @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

lio/include/sophus/rxso3.hpp

@@ -0,0 +1,730 @@
+/// @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

lio/include/sophus/se2.hpp

@@ -0,0 +1,840 @@
+/// @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

lio/include/sophus/sim2.hpp

@@ -0,0 +1,727 @@
+/// @file
+/// Similarity group Sim(2) - scaling, rotation and translation in 2d.
+
+#ifndef SOPHUS_SIM2_HPP
+#define SOPHUS_SIM2_HPP
+
+#include "rxso2.hpp"
+#include "sim_details.hpp"
+
+namespace Sophus {
+template <class Scalar_, int Options = 0>
+class Sim2;
+using Sim2d = Sim2<double>;
+using Sim2f = Sim2<float>;
+}  // namespace Sophus
+
+namespace Eigen {
+namespace internal {
+
+template <class Scalar_, int Options>
+struct traits<Sophus::Sim2<Scalar_, Options>> {
+  using Scalar = Scalar_;
+  using TranslationType = Sophus::Vector2<Scalar, Options>;
+  using RxSO2Type = Sophus::RxSO2<Scalar, Options>;
+};
+
+template <class Scalar_, int Options>
+struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
+    : traits<Sophus::Sim2<Scalar_, Options>> {
+  using Scalar = Scalar_;
+  using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
+  using RxSO2Type = Map<Sophus::RxSO2<Scalar>, Options>;
+};
+
+template <class Scalar_, int Options>
+struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
+    : traits<Sophus::Sim2<Scalar_, Options> const> {
+  using Scalar = Scalar_;
+  using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
+  using RxSO2Type = Map<Sophus::RxSO2<Scalar> const, Options>;
+};
+}  // namespace internal
+}  // namespace Eigen
+
+namespace Sophus {
+
+/// Sim2 base type - implements Sim2 class but is storage agnostic.
+///
+/// Sim(2) is the group of rotations  and translation and scaling in 2d. It is
+/// the semi-direct product of R+xSO(2) and the 2d Euclidean vector space. The
+/// class is represented using a composition of RxSO2  for scaling plus
+/// rotation and a 2-vector for translation.
+///
+/// Sim(2) is neither compact, nor a commutative group.
+///
+/// See RxSO2 for more details of the scaling + rotation representation in 2d.
+///
+template <class Derived>
+class Sim2Base {
+ public:
+  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
+  using TranslationType =
+      typename Eigen::internal::traits<Derived>::TranslationType;
+  using RxSO2Type = typename Eigen::internal::traits<Derived>::RxSO2Type;
+
+  /// Degrees of freedom of manifold, number of dimensions in tangent space
+  /// (two for translation, one for rotation and one for scaling).
+  static int constexpr DoF = 4;
+  /// Number of internal parameters used (2-tuple for complex number, two for
+  /// translation).
+  static int constexpr num_parameters = 4;
+  /// Group transformations are 3x3 matrices.
+  static int constexpr N = 3;
+  using Transformation = Matrix<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 SIM2 operations.
+  template <typename OtherDerived>
+  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
+      Scalar, typename OtherDerived::Scalar>::ReturnType;
+
+  template <typename OtherDerived>
+  using Sim2Product = Sim2<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 {
+    Adjoint res;
+    res.setZero();
+    res.template block<2, 2>(0, 0) = rxso2().matrix();
+    res(0, 2) = translation()[1];
+    res(1, 2) = -translation()[0];
+    res.template block<2, 1>(0, 3) = -translation();
+
+    res(2, 2) = Scalar(1);
+
+    res(3, 3) = Scalar(1);
+    return res;
+  }
+
+  /// Returns copy of instance casted to NewScalarType.
+  ///
+  template <class NewScalarType>
+  SOPHUS_FUNC Sim2<NewScalarType> cast() const {
+    return Sim2<NewScalarType>(rxso2().template cast<NewScalarType>(),
+                               translation().template cast<NewScalarType>());
+  }
+
+  /// Returns group inverse.
+  ///
+  SOPHUS_FUNC Sim2<Scalar> inverse() const {
+    RxSO2<Scalar> invR = rxso2().inverse();
+    return Sim2<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 Sim(2).
+  ///
+  SOPHUS_FUNC Tangent log() const {
+    /// The derivation of the closed-form Sim(2) logarithm for is done
+    /// analogously to the closed-form solution of the SE(2) logarithm, see
+    /// J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
+    /// and logarithms of orthogonal matrices", IJRA 2002.
+    /// https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
+    /// (Sec. 6., pp. 8)
+    Tangent res;
+    Vector2<Scalar> const theta_sigma = rxso2().log();
+    Scalar const theta = theta_sigma[0];
+    Scalar const sigma = theta_sigma[1];
+    Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
+    Matrix2<Scalar> const W_inv =
+        details::calcWInv<Scalar, 2>(Omega, theta, sigma, scale());
+
+    res.segment(0, 2) = W_inv * translation();
+    res[2] = theta;
+    res[3] = sigma;
+    return res;
+  }
+
+  /// Returns 3x3 matrix representation of the instance.
+  ///
+  /// It has the following form:
+  ///
+  ///   | s*R t |
+  ///   |  o  1 |
+  ///
+  /// where ``R`` is a 2x2 rotation matrix, ``s`` a scale factor, ``t`` a
+  /// translation 2-vector and ``o`` a 2-column vector of zeros.
+  ///
+  SOPHUS_FUNC Transformation matrix() const {
+    Transformation homogenious_matrix;
+    homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3();
+    homogenious_matrix.row(2) =
+        Matrix<Scalar, 3, 1>(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>() = rxso2().matrix();
+    matrix.col(2) = translation();
+    return matrix;
+  }
+
+  /// Assignment operator.
+  ///
+  SOPHUS_FUNC Sim2Base& operator=(Sim2Base const& other) = default;
+
+  /// Assignment-like operator from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC Sim2Base<Derived>& operator=(
+      Sim2Base<OtherDerived> const& other) {
+    rxso2() = other.rxso2();
+    translation() = other.translation();
+    return *this;
+  }
+
+  /// Group multiplication, which is rotation plus scaling concatenation.
+  ///
+  /// Note: That scaling is calculated with saturation. See RxSO2 for
+  /// details.
+  ///
+  template <typename OtherDerived>
+  SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
+      Sim2Base<OtherDerived> const& other) const {
+    return Sim2Product<OtherDerived>(
+        rxso2() * other.rxso2(), translation() + rxso2() * other.translation());
+  }
+
+  /// Group action on 2-points.
+  ///
+  /// This function rotates, scales and translates a two dimensional point
+  /// ``p`` by the Sim(2) element ``(bar_sR_foo, t_bar)`` (= similarity
+  /// transformation):
+  ///
+  ///   ``p_bar = bar_sR_foo * p_foo + t_bar``.
+  ///
+  template <typename PointDerived,
+            typename = typename std::enable_if<
+                IsFixedSizeVector<PointDerived, 2>::value>::type>
+  SOPHUS_FUNC PointProduct<PointDerived> operator*(
+      Eigen::MatrixBase<PointDerived> const& p) const {
+    return rxso2() * 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 =
+        rxso2() * p.template head<2>() + p(2) * translation();
+    return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2));
+  }
+
+  /// Group action on lines.
+  ///
+  /// This function rotates, scales and translates a parametrized line
+  /// ``l(t) = o + t * d`` by the Sim(2) element:
+  ///
+  /// Origin ``o`` is rotated, scaled and translated
+  /// Direction ``d`` is rotated
+  ///
+  SOPHUS_FUNC Line operator*(Line const& l) const {
+    Line rotatedLine = rxso2() * l;
+    return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
+  }
+
+  /// Returns internal parameters of Sim(2).
+  ///
+  /// It returns (c[0], c[1], t[0], t[1]),
+  /// with c being the complex number, t the translation 3-vector.
+  ///
+  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
+    Sophus::Vector<Scalar, num_parameters> p;
+    p << rxso2().params(), translation();
+    return p;
+  }
+
+  /// In-place group multiplication. This method is only valid if the return
+  /// type of the multiplication is compatible with this SO2's Scalar type.
+  ///
+  template <typename OtherDerived,
+            typename = typename std::enable_if<
+                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
+  SOPHUS_FUNC Sim2Base<Derived>& operator*=(
+      Sim2Base<OtherDerived> const& other) {
+    *static_cast<Derived*>(this) = *this * other;
+    return *this;
+  }
+
+  /// Setter of non-zero complex number.
+  ///
+  /// Precondition: ``z`` must not be close to zero.
+  ///
+  SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
+    rxso2().setComplex(z);
+  }
+
+  /// Accessor of complex number.
+  ///
+  SOPHUS_FUNC
+  typename Eigen::internal::traits<Derived>::RxSO2Type::ComplexType const&
+  complex() const {
+    return rxso2().complex();
+  }
+
+  /// Returns Rotation matrix
+  ///
+  SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
+    return rxso2().rotationMatrix();
+  }
+
+  /// Mutator of SO2 group.
+  ///
+  SOPHUS_FUNC RxSO2Type& rxso2() {
+    return static_cast<Derived*>(this)->rxso2();
+  }
+
+  /// Accessor of SO2 group.
+  ///
+  SOPHUS_FUNC RxSO2Type const& rxso2() const {
+    return static_cast<Derived const*>(this)->rxso2();
+  }
+
+  /// Returns scale.
+  ///
+  SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); }
+
+  /// Setter of complex number using rotation matrix ``R``, leaves scale as is.
+  ///
+  SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar>& R) {
+    rxso2().setRotationMatrix(R);
+  }
+
+  /// Sets scale and leaves rotation as is.
+  ///
+  /// Note: This function as a significant computational cost, since it has to
+  /// call the square root twice.
+  ///
+  SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scale); }
+
+  /// Setter of complexnumber using scaled rotation matrix ``sR``.
+  ///
+  /// Precondition: The 2x2 matrix must be "scaled orthogonal"
+  ///               and have a positive determinant.
+  ///
+  SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) {
+    rxso2().setScaledRotationMatrix(sR);
+  }
+
+  /// 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();
+  }
+};
+
+/// Sim2 using default storage; derived from Sim2Base.
+template <class Scalar_, int Options>
+class Sim2 : public Sim2Base<Sim2<Scalar_, Options>> {
+ public:
+  using Base = Sim2Base<Sim2<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 RxSo2Member = RxSO2<Scalar, Options>;
+  using TranslationMember = Vector2<Scalar, Options>;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Default constructor initializes similarity transform to the identity.
+  ///
+  SOPHUS_FUNC Sim2();
+
+  /// Copy constructor
+  ///
+  SOPHUS_FUNC Sim2(Sim2 const& other) = default;
+
+  /// Copy-like constructor from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC Sim2(Sim2Base<OtherDerived> const& other)
+      : rxso2_(other.rxso2()), translation_(other.translation()) {
+    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
+                  "must be same Scalar type");
+  }
+
+  /// Constructor from RxSO2 and translation vector
+  ///
+  template <class OtherDerived, class D>
+  SOPHUS_FUNC Sim2(RxSO2Base<OtherDerived> const& rxso2,
+                   Eigen::MatrixBase<D> const& translation)
+      : rxso2_(rxso2), 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 complex number and translation vector.
+  ///
+  /// Precondition: complex number must not be close to zero.
+  ///
+  template <class D>
+  SOPHUS_FUNC Sim2(Vector2<Scalar> const& complex_number,
+                   Eigen::MatrixBase<D> const& translation)
+      : rxso2_(complex_number), translation_(translation) {
+    static_assert(std::is_same<typename D::Scalar, Scalar>::value,
+                  "must be same Scalar type");
+  }
+
+  /// Constructor from 3x3 matrix
+  ///
+  /// Precondition: Top-left 2x2 matrix needs to be "scaled-orthogonal" with
+  ///               positive determinant. The last row must be ``(0, 0, 1)``.
+  ///
+  SOPHUS_FUNC explicit Sim2(Matrix<Scalar, 3, 3> const& T)
+      : rxso2_((T.template topLeftCorner<2, 2>()).eval()),
+        translation_(T.template block<2, 1>(0, 2)) {}
+
+  /// This provides unsafe read/write access to internal data. Sim(2) is
+  /// represented by a complex number (two parameters) and a 2-vector. When
+  /// using direct write access, the user needs to take care of that the
+  /// complex number is not set close to zero.
+  ///
+  SOPHUS_FUNC Scalar* data() {
+    // rxso2_ and translation_ are laid out sequentially with no padding
+    return rxso2_.data();
+  }
+
+  /// Const version of data() above.
+  ///
+  SOPHUS_FUNC Scalar const* data() const {
+    // rxso2_ and translation_ are laid out sequentially with no padding
+    return rxso2_.data();
+  }
+
+  /// Accessor of RxSO2
+  ///
+  SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; }
+
+  /// Mutator of RxSO2
+  ///
+  SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; }
+
+  /// Mutator of translation vector
+  ///
+  SOPHUS_FUNC TranslationMember& translation() { return translation_; }
+
+  /// Accessor of translation vector
+  ///
+  SOPHUS_FUNC TranslationMember const& translation() const {
+    return translation_;
+  }
+
+  /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
+  ///
+  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
+    return generator(i);
+  }
+
+  /// Derivative of Lie bracket with respect to first element.
+  ///
+  /// This function returns ``D_a [a, b]`` with ``D_a`` being the
+  /// differential operator with respect to ``a``, ``[a, b]`` being the lie
+  /// bracket of the Lie algebra sim(2).
+  /// See ``lieBracket()`` below.
+  ///
+
+  /// Group exponential
+  ///
+  /// This functions takes in an element of tangent space and returns the
+  /// corresponding element of the group Sim(2).
+  ///
+  /// The first two components of ``a`` represent the translational part
+  /// ``upsilon`` in the tangent space of Sim(2), the following two components
+  /// of ``a`` represents the rotation ``theta`` and the final component
+  /// represents the logarithm of the scaling factor ``sigma``.
+  /// To be more specific, this function computes ``expmat(hat(a))`` with
+  /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
+  /// of Sim(2), see below.
+  ///
+  SOPHUS_FUNC static Sim2<Scalar> exp(Tangent const& a) {
+    // For the derivation of the exponential map of Sim(N) see
+    // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual
+    // SLAM", PhD thesis, 2012.
+    // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186)
+    Vector2<Scalar> const upsilon = a.segment(0, 2);
+    Scalar const theta = a[2];
+    Scalar const sigma = a[3];
+    RxSO2<Scalar> rxso2 = RxSO2<Scalar>::exp(a.template tail<2>());
+    Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
+    Matrix2<Scalar> const W = details::calcW<Scalar, 2>(Omega, theta, sigma);
+    return Sim2<Scalar>(rxso2, W * upsilon);
+  }
+
+  /// Returns the ith infinitesimal generators of Sim(2).
+  ///
+  /// The infinitesimal generators of Sim(2) are:
+  ///
+  /// ```
+  ///         |  0  0  1 |
+  ///   G_0 = |  0  0  0 |
+  ///         |  0  0  0 |
+  ///
+  ///         |  0  0  0 |
+  ///   G_1 = |  0  0  1 |
+  ///         |  0  0  0 |
+  ///
+  ///         |  0 -1  0 |
+  ///   G_2 = |  1  0  0 |
+  ///         |  0  0  0 |
+  ///
+  ///         |  1  0  0 |
+  ///   G_3 = |  0  1  0 |
+  ///         |  0  0  0 |
+  /// ```
+  ///
+  /// Precondition: ``i`` must be in [0, 3].
+  ///
+  SOPHUS_FUNC static Transformation generator(int i) {
+    SOPHUS_ENSURE(i >= 0 || i <= 3, "i should be in range [0,3].");
+    Tangent e;
+    e.setZero();
+    e[i] = Scalar(1);
+    return hat(e);
+  }
+
+  /// hat-operator
+  ///
+  /// It takes in the 4-vector representation and returns the corresponding
+  /// matrix representation of Lie algebra element.
+  ///
+  /// Formally, the hat()-operator of Sim(2) is defined as
+  ///
+  ///   ``hat(.): R^4 -> R^{3x3},  hat(a) = sum_i a_i * G_i``  (for i=0,...,6)
+  ///
+  /// with ``G_i`` being the ith infinitesimal generator of Sim(2).
+  ///
+  /// The corresponding inverse is the vee()-operator, see below.
+  ///
+  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
+    Transformation Omega;
+    Omega.template topLeftCorner<2, 2>() =
+        RxSO2<Scalar>::hat(a.template tail<2>());
+    Omega.col(2).template head<2>() = a.template head<2>();
+    Omega.row(2).setZero();
+    return Omega;
+  }
+
+  /// Lie bracket
+  ///
+  /// It computes the Lie bracket of Sim(2). To be more specific, it computes
+  ///
+  ///   ``[omega_1, omega_2]_sim2 := vee([hat(omega_1), hat(omega_2)])``
+  ///
+  /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
+  /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(2).
+  ///
+  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
+    Vector2<Scalar> const upsilon1 = a.template head<2>();
+    Vector2<Scalar> const upsilon2 = b.template head<2>();
+    Scalar const theta1 = a[2];
+    Scalar const theta2 = b[2];
+    Scalar const sigma1 = a[3];
+    Scalar const sigma2 = b[3];
+
+    Tangent res;
+    res[0] = -theta1 * upsilon2[1] + theta2 * upsilon1[1] +
+             sigma1 * upsilon2[0] - sigma2 * upsilon1[0];
+    res[1] = theta1 * upsilon2[0] - theta2 * upsilon1[0] +
+             sigma1 * upsilon2[1] - sigma2 * upsilon1[1];
+    res[2] = Scalar(0);
+    res[3] = Scalar(0);
+
+    return res;
+  }
+
+  /// Draw uniform sample from Sim(2) manifold.
+  ///
+  /// Translations are drawn component-wise from the range [-1, 1].
+  /// The scale factor is drawn uniformly in log2-space from [-1, 1],
+  /// hence the scale is in [0.5, 2].
+  ///
+  template <class UniformRandomBitGenerator>
+  static Sim2 sampleUniform(UniformRandomBitGenerator& generator) {
+    std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
+    return Sim2(RxSO2<Scalar>::sampleUniform(generator),
+                Vector2<Scalar>(uniform(generator), uniform(generator)));
+  }
+
+  /// vee-operator
+  ///
+  /// It takes the 3x3-matrix representation ``Omega`` and maps it to the
+  /// corresponding 4-vector representation of Lie algebra.
+  ///
+  /// This is the inverse of the hat()-operator, see above.
+  ///
+  /// Precondition: ``Omega`` must have the following structure:
+  ///
+  ///                |  d -c  a |
+  ///                |  c  d  b |
+  ///                |  0  0  0 |
+  ///
+  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
+    Tangent upsilon_omega_sigma;
+    upsilon_omega_sigma.template head<2>() = Omega.col(2).template head<2>();
+    upsilon_omega_sigma.template tail<2>() =
+        RxSO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
+    return upsilon_omega_sigma;
+  }
+
+ protected:
+  RxSo2Member rxso2_;
+  TranslationMember translation_;
+};
+
+template <class Scalar, int Options>
+Sim2<Scalar, Options>::Sim2() : translation_(TranslationMember::Zero()) {
+  static_assert(std::is_standard_layout<Sim2>::value,
+                "Assume standard layout for the use of offsetof check below.");
+  static_assert(
+      offsetof(Sim2, rxso2_) + sizeof(Scalar) * RxSO2<Scalar>::num_parameters ==
+          offsetof(Sim2, translation_),
+      "This class assumes packed storage and hence will only work "
+      "correctly depending on the compiler (options) - in "
+      "particular when using [this->data(), this-data() + "
+      "num_parameters] to access the raw data in a contiguous fashion.");
+}
+
+}  // namespace Sophus
+
+namespace Eigen {
+
+/// Specialization of Eigen::Map for ``Sim2``; derived from Sim2Base.
+///
+/// Allows us to wrap Sim2 objects around POD array.
+template <class Scalar_, int Options>
+class Map<Sophus::Sim2<Scalar_>, Options>
+    : public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, Options>> {
+ public:
+  using Base = Sophus::Sim2Base<Map<Sophus::Sim2<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)
+      : rxso2_(coeffs),
+        translation_(coeffs + Sophus::RxSO2<Scalar>::num_parameters) {}
+
+  /// Mutator of RxSO2
+  ///
+  SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rxso2_; }
+
+  /// Accessor of RxSO2
+  ///
+  SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options> const& rxso2() const {
+    return rxso2_;
+  }
+
+  /// 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::RxSO2<Scalar>, Options> rxso2_;
+  Map<Sophus::Vector2<Scalar>, Options> translation_;
+};
+
+/// Specialization of Eigen::Map for ``Sim2 const``; derived from Sim2Base.
+///
+/// Allows us to wrap RxSO2 objects around POD array.
+template <class Scalar_, int Options>
+class Map<Sophus::Sim2<Scalar_> const, Options>
+    : public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, Options>> {
+ public:
+  using Base = Sophus::Sim2Base<Map<Sophus::Sim2<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)
+      : rxso2_(coeffs),
+        translation_(coeffs + Sophus::RxSO2<Scalar>::num_parameters) {}
+
+  /// Accessor of RxSO2
+  ///
+  SOPHUS_FUNC Map<Sophus::RxSO2<Scalar> const, Options> const& rxso2() const {
+    return rxso2_;
+  }
+
+  /// Accessor of translation vector
+  ///
+  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
+      const {
+    return translation_;
+  }
+
+ protected:
+  Map<Sophus::RxSO2<Scalar> const, Options> const rxso2_;
+  Map<Sophus::Vector2<Scalar> const, Options> const translation_;
+};
+}  // namespace Eigen
+
+#endif

lio/include/sophus/sim3.hpp

@@ -0,0 +1,745 @@
+/// @file
+/// Similarity group Sim(3) - scaling, rotation and translation in 3d.
+
+#ifndef SOPHUS_SIM3_HPP
+#define SOPHUS_SIM3_HPP
+
+#include "rxso3.hpp"
+#include "sim_details.hpp"
+
+namespace Sophus {
+template <class Scalar_, int Options = 0>
+class Sim3;
+using Sim3d = Sim3<double>;
+using Sim3f = Sim3<float>;
+}  // namespace Sophus
+
+namespace Eigen {
+namespace internal {
+
+template <class Scalar_, int Options>
+struct traits<Sophus::Sim3<Scalar_, Options>> {
+  using Scalar = Scalar_;
+  using TranslationType = Sophus::Vector3<Scalar, Options>;
+  using RxSO3Type = Sophus::RxSO3<Scalar, Options>;
+};
+
+template <class Scalar_, int Options>
+struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
+    : traits<Sophus::Sim3<Scalar_, Options>> {
+  using Scalar = Scalar_;
+  using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
+  using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
+};
+
+template <class Scalar_, int Options>
+struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
+    : traits<Sophus::Sim3<Scalar_, Options> const> {
+  using Scalar = Scalar_;
+  using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
+  using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>;
+};
+}  // namespace internal
+}  // namespace Eigen
+
+namespace Sophus {
+
+/// Sim3 base type - implements Sim3 class but is storage agnostic.
+///
+/// Sim(3) is the group of rotations  and translation and scaling in 3d. It is
+/// the semi-direct product of R+xSO(3) and the 3d Euclidean vector space.  The
+/// class is represented using a composition of RxSO3  for scaling plus
+/// rotation and a 3-vector for translation.
+///
+/// Sim(3) is neither compact, nor a commutative group.
+///
+/// See RxSO3 for more details of the scaling + rotation representation in 3d.
+///
+template <class Derived>
+class Sim3Base {
+ public:
+  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
+  using TranslationType =
+      typename Eigen::internal::traits<Derived>::TranslationType;
+  using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type;
+  using QuaternionType = typename RxSO3Type::QuaternionType;
+
+  /// Degrees of freedom of manifold, number of dimensions in tangent space
+  /// (three for translation, three for rotation and one for scaling).
+  static int constexpr DoF = 7;
+  /// Number of internal parameters used (4-tuple for quaternion, three for
+  /// translation).
+  static int constexpr num_parameters = 7;
+  /// Group transformations are 4x4 matrices.
+  static int constexpr N = 4;
+  using Transformation = Matrix<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 Sim3 operations.
+  template <typename OtherDerived>
+  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
+      Scalar, typename OtherDerived::Scalar>::ReturnType;
+
+  template <typename OtherDerived>
+  using Sim3Product = Sim3<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 {
+    Matrix3<Scalar> const R = rxso3().rotationMatrix();
+    Adjoint res;
+    res.setZero();
+    res.template block<3, 3>(0, 0) = rxso3().matrix();
+    res.template block<3, 3>(0, 3) = SO3<Scalar>::hat(translation()) * R;
+    res.template block<3, 1>(0, 6) = -translation();
+
+    res.template block<3, 3>(3, 3) = R;
+
+    res(6, 6) = Scalar(1);
+    return res;
+  }
+
+  /// Returns copy of instance casted to NewScalarType.
+  ///
+  template <class NewScalarType>
+  SOPHUS_FUNC Sim3<NewScalarType> cast() const {
+    return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
+                               translation().template cast<NewScalarType>());
+  }
+
+  /// Returns group inverse.
+  ///
+  SOPHUS_FUNC Sim3<Scalar> inverse() const {
+    RxSO3<Scalar> invR = rxso3().inverse();
+    return Sim3<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 Sim(3).
+  ///
+  SOPHUS_FUNC Tangent log() const {
+    // The derivation of the closed-form Sim(3) logarithm for is done
+    // analogously to the closed-form solution of the SE(3) logarithm, see
+    // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
+    // and logarithms of orthogonal matrices", IJRA 2002.
+    // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
+    // (Sec. 6., pp. 8)
+    Tangent res;
+    auto omega_sigma_and_theta = rxso3().logAndTheta();
+    Vector3<Scalar> const omega =
+        omega_sigma_and_theta.tangent.template head<3>();
+    Scalar sigma = omega_sigma_and_theta.tangent[3];
+    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
+    Matrix3<Scalar> const W_inv = details::calcWInv<Scalar, 3>(
+        Omega, omega_sigma_and_theta.theta, sigma, scale());
+
+    res.segment(0, 3) = W_inv * translation();
+    res.segment(3, 3) = omega;
+    res[6] = sigma;
+    return res;
+  }
+
+  /// Returns 4x4 matrix representation of the instance.
+  ///
+  /// It has the following form:
+  ///
+  ///     | s*R t |
+  ///     |  o  1 |
+  ///
+  /// where ``R`` is a 3x3 rotation matrix, ``s`` a scale factor, ``t`` a
+  /// translation 3-vector and ``o`` a 3-column vector of zeros.
+  ///
+  SOPHUS_FUNC Transformation matrix() const {
+    Transformation homogenious_matrix;
+    homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
+    homogenious_matrix.row(3) =
+        Matrix<Scalar, 4, 1>(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>() = rxso3().matrix();
+    matrix.col(3) = translation();
+    return matrix;
+  }
+
+  /// Assignment operator.
+  ///
+  SOPHUS_FUNC Sim3Base& operator=(Sim3Base const& other) = default;
+
+  /// Assignment-like operator from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC Sim3Base<Derived>& operator=(
+      Sim3Base<OtherDerived> const& other) {
+    rxso3() = other.rxso3();
+    translation() = other.translation();
+    return *this;
+  }
+
+  /// Group multiplication, which is rotation plus scaling concatenation.
+  ///
+  /// Note: That scaling is calculated with saturation. See RxSO3 for
+  /// details.
+  ///
+  template <typename OtherDerived>
+  SOPHUS_FUNC Sim3Product<OtherDerived> operator*(
+      Sim3Base<OtherDerived> const& other) const {
+    return Sim3Product<OtherDerived>(
+        rxso3() * other.rxso3(), translation() + rxso3() * other.translation());
+  }
+
+  /// Group action on 3-points.
+  ///
+  /// This function rotates, scales and translates a three dimensional point
+  /// ``p`` by the Sim(3) element ``(bar_sR_foo, t_bar)`` (= similarity
+  /// transformation):
+  ///
+  ///   ``p_bar = bar_sR_foo * p_foo + t_bar``.
+  ///
+  template <typename PointDerived,
+            typename = typename std::enable_if<
+                IsFixedSizeVector<PointDerived, 3>::value>::type>
+  SOPHUS_FUNC PointProduct<PointDerived> operator*(
+      Eigen::MatrixBase<PointDerived> const& p) const {
+    return rxso3() * 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 =
+        rxso3() * 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, scales and translates a parametrized line
+  /// ``l(t) = o + t * d`` by the Sim(3) element:
+  ///
+  /// Origin ``o`` is rotated, scaled and translated
+  /// Direction ``d`` is rotated
+  ///
+  SOPHUS_FUNC Line operator*(Line const& l) const {
+    Line rotatedLine = rxso3() * l;
+    return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
+  }
+
+  /// In-place group multiplication. This method is only valid if the return
+  /// type of the multiplication is compatible with this SO3's Scalar type.
+  ///
+  template <typename OtherDerived,
+            typename = typename std::enable_if<
+                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
+  SOPHUS_FUNC Sim3Base<Derived>& operator*=(
+      Sim3Base<OtherDerived> const& other) {
+    *static_cast<Derived*>(this) = *this * other;
+    return *this;
+  }
+
+  /// Returns internal parameters of Sim(3).
+  ///
+  /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]),
+  /// with q being the quaternion, t the translation 3-vector.
+  ///
+  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
+    Sophus::Vector<Scalar, num_parameters> p;
+    p << rxso3().params(), translation();
+    return p;
+  }
+
+  /// Setter of non-zero quaternion.
+  ///
+  /// Precondition: ``quat`` must not be close to zero.
+  ///
+  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
+    rxso3().setQuaternion(quat);
+  }
+
+  /// Accessor of quaternion.
+  ///
+  SOPHUS_FUNC QuaternionType const& quaternion() const {
+    return rxso3().quaternion();
+  }
+
+  /// Returns Rotation matrix
+  ///
+  SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
+    return rxso3().rotationMatrix();
+  }
+
+  /// Mutator of SO3 group.
+  ///
+  SOPHUS_FUNC RxSO3Type& rxso3() {
+    return static_cast<Derived*>(this)->rxso3();
+  }
+
+  /// Accessor of SO3 group.
+  ///
+  SOPHUS_FUNC RxSO3Type const& rxso3() const {
+    return static_cast<Derived const*>(this)->rxso3();
+  }
+
+  /// Returns scale.
+  ///
+  SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
+
+  /// Setter of quaternion using rotation matrix ``R``, leaves scale as is.
+  ///
+  SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
+    rxso3().setRotationMatrix(R);
+  }
+
+  /// Sets scale and leaves rotation as is.
+  ///
+  /// Note: This function as a significant computational cost, since it has to
+  /// call the square root twice.
+  ///
+  SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); }
+
+  /// Setter of quaternion using scaled rotation matrix ``sR``.
+  ///
+  /// Precondition: The 3x3 matrix must be "scaled orthogonal"
+  ///               and have a positive determinant.
+  ///
+  SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
+    rxso3().setScaledRotationMatrix(sR);
+  }
+
+  /// 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();
+  }
+};
+
+/// Sim3 using default storage; derived from Sim3Base.
+template <class Scalar_, int Options>
+class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
+ public:
+  using Base = Sim3Base<Sim3<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 RxSo3Member = RxSO3<Scalar, Options>;
+  using TranslationMember = Vector3<Scalar, Options>;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Default constructor initializes similarity transform to the identity.
+  ///
+  SOPHUS_FUNC Sim3();
+
+  /// Copy constructor
+  ///
+  SOPHUS_FUNC Sim3(Sim3 const& other) = default;
+
+  /// Copy-like constructor from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other)
+      : rxso3_(other.rxso3()), translation_(other.translation()) {
+    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
+                  "must be same Scalar type");
+  }
+
+  /// Constructor from RxSO3 and translation vector
+  ///
+  template <class OtherDerived, class D>
+  SOPHUS_FUNC Sim3(RxSO3Base<OtherDerived> const& rxso3,
+                   Eigen::MatrixBase<D> const& translation)
+      : rxso3_(rxso3), 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 quaternion and translation vector.
+  ///
+  /// Precondition: quaternion must not be close to zero.
+  ///
+  template <class D1, class D2>
+  SOPHUS_FUNC Sim3(Eigen::QuaternionBase<D1> const& quaternion,
+                   Eigen::MatrixBase<D2> const& translation)
+      : rxso3_(quaternion), translation_(translation) {
+    static_assert(std::is_same<typename D1::Scalar, Scalar>::value,
+                  "must be same Scalar type");
+    static_assert(std::is_same<typename D2::Scalar, Scalar>::value,
+                  "must be same Scalar type");
+  }
+
+  /// Constructor from 4x4 matrix
+  ///
+  /// Precondition: Top-left 3x3 matrix needs to be "scaled-orthogonal" with
+  ///               positive determinant. The last row must be ``(0, 0, 0, 1)``.
+  ///
+  SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const& T)
+      : rxso3_(T.template topLeftCorner<3, 3>()),
+        translation_(T.template block<3, 1>(0, 3)) {}
+
+  /// This provides unsafe read/write access to internal data. Sim(3) is
+  /// represented by an Eigen::Quaternion (four parameters) and a 3-vector. When
+  /// using direct write access, the user needs to take care of that the
+  /// quaternion is not set close to zero.
+  ///
+  SOPHUS_FUNC Scalar* data() {
+    // rxso3_ and translation_ are laid out sequentially with no padding
+    return rxso3_.data();
+  }
+
+  /// Const version of data() above.
+  ///
+  SOPHUS_FUNC Scalar const* data() const {
+    // rxso3_ and translation_ are laid out sequentially with no padding
+    return rxso3_.data();
+  }
+
+  /// Accessor of RxSO3
+  ///
+  SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }
+
+  /// Mutator of RxSO3
+  ///
+  SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }
+
+  /// Mutator of translation vector
+  ///
+  SOPHUS_FUNC TranslationMember& translation() { return translation_; }
+
+  /// Accessor of translation vector
+  ///
+  SOPHUS_FUNC TranslationMember const& translation() const {
+    return translation_;
+  }
+
+  /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
+  ///
+  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
+    return generator(i);
+  }
+
+  /// Group exponential
+  ///
+  /// This functions takes in an element of tangent space and returns the
+  /// corresponding element of the group Sim(3).
+  ///
+  /// The first three components of ``a`` represent the translational part
+  /// ``upsilon`` in the tangent space of Sim(3), the following three components
+  /// of ``a`` represents the rotation vector ``omega`` and the final component
+  /// represents the logarithm of the scaling factor ``sigma``.
+  /// To be more specific, this function computes ``expmat(hat(a))`` with
+  /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
+  /// of Sim(3), see below.
+  ///
+  SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) {
+    // For the derivation of the exponential map of Sim(3) see
+    // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual
+    // SLAM", PhD thesis, 2012.
+    // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186)
+    Vector3<Scalar> const upsilon = a.segment(0, 3);
+    Vector3<Scalar> const omega = a.segment(3, 3);
+    Scalar const sigma = a[6];
+    Scalar theta;
+    RxSO3<Scalar> rxso3 =
+        RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta);
+    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
+
+    Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma);
+    return Sim3<Scalar>(rxso3, W * upsilon);
+  }
+
+  /// Returns the ith infinitesimal generators of Sim(3).
+  ///
+  /// The infinitesimal generators of Sim(3) are:
+  ///
+  /// ```
+  ///         |  0  0  0  1 |
+  ///   G_0 = |  0  0  0  0 |
+  ///         |  0  0  0  0 |
+  ///         |  0  0  0  0 |
+  ///
+  ///         |  0  0  0  0 |
+  ///   G_1 = |  0  0  0  1 |
+  ///         |  0  0  0  0 |
+  ///         |  0  0  0  0 |
+  ///
+  ///         |  0  0  0  0 |
+  ///   G_2 = |  0  0  0  0 |
+  ///         |  0  0  0  1 |
+  ///         |  0  0  0  0 |
+  ///
+  ///         |  0  0  0  0 |
+  ///   G_3 = |  0  0 -1  0 |
+  ///         |  0  1  0  0 |
+  ///         |  0  0  0  0 |
+  ///
+  ///         |  0  0  1  0 |
+  ///   G_4 = |  0  0  0  0 |
+  ///         | -1  0  0  0 |
+  ///         |  0  0  0  0 |
+  ///
+  ///         |  0 -1  0  0 |
+  ///   G_5 = |  1  0  0  0 |
+  ///         |  0  0  0  0 |
+  ///         |  0  0  0  0 |
+  ///
+  ///         |  1  0  0  0 |
+  ///   G_6 = |  0  1  0  0 |
+  ///         |  0  0  1  0 |
+  ///         |  0  0  0  0 |
+  /// ```
+  ///
+  /// Precondition: ``i`` must be in [0, 6].
+  ///
+  SOPHUS_FUNC static Transformation generator(int i) {
+    SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6].");
+    Tangent e;
+    e.setZero();
+    e[i] = Scalar(1);
+    return hat(e);
+  }
+
+  /// hat-operator
+  ///
+  /// It takes in the 7-vector representation and returns the corresponding
+  /// matrix representation of Lie algebra element.
+  ///
+  /// Formally, the hat()-operator of Sim(3) is defined as
+  ///
+  ///   ``hat(.): R^7 -> R^{4x4},  hat(a) = sum_i a_i * G_i``  (for i=0,...,6)
+  ///
+  /// with ``G_i`` being the ith infinitesimal generator of Sim(3).
+  ///
+  /// The corresponding inverse is the vee()-operator, see below.
+  ///
+  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
+    Transformation Omega;
+    Omega.template topLeftCorner<3, 3>() =
+        RxSO3<Scalar>::hat(a.template tail<4>());
+    Omega.col(3).template head<3>() = a.template head<3>();
+    Omega.row(3).setZero();
+    return Omega;
+  }
+
+  /// Lie bracket
+  ///
+  /// It computes the Lie bracket of Sim(3). To be more specific, it computes
+  ///
+  ///   ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])``
+  ///
+  /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
+  /// hat()-operator and ``vee(.)`` the vee()-operator of Sim(3).
+  ///
+  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
+    Vector3<Scalar> const upsilon1 = a.template head<3>();
+    Vector3<Scalar> const upsilon2 = b.template head<3>();
+    Vector3<Scalar> const omega1 = a.template segment<3>(3);
+    Vector3<Scalar> const omega2 = b.template segment<3>(3);
+    Scalar sigma1 = a[6];
+    Scalar sigma2 = b[6];
+
+    Tangent res;
+    res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 +
+                             SO3<Scalar>::hat(upsilon1) * omega2 +
+                             sigma1 * upsilon2 - sigma2 * upsilon1;
+    res.template segment<3>(3) = omega1.cross(omega2);
+    res[6] = Scalar(0);
+
+    return res;
+  }
+
+  /// Draw uniform sample from Sim(3) manifold.
+  ///
+  /// Translations are drawn component-wise from the range [-1, 1].
+  /// The scale factor is drawn uniformly in log2-space from [-1, 1],
+  /// hence the scale is in [0.5, 2].
+  ///
+  template <class UniformRandomBitGenerator>
+  static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
+    std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
+    return Sim3(RxSO3<Scalar>::sampleUniform(generator),
+                Vector3<Scalar>(uniform(generator), uniform(generator),
+                                uniform(generator)));
+  }
+
+  /// vee-operator
+  ///
+  /// It takes the 4x4-matrix representation ``Omega`` and maps it to the
+  /// corresponding 7-vector representation of Lie algebra.
+  ///
+  /// This is the inverse of the hat()-operator, see above.
+  ///
+  /// Precondition: ``Omega`` must have the following structure:
+  ///
+  ///                |  g -f  e  a |
+  ///                |  f  g -d  b |
+  ///                | -e  d  g  c |
+  ///                |  0  0  0  0 |
+  ///
+  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
+    Tangent upsilon_omega_sigma;
+    upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
+    upsilon_omega_sigma.template tail<4>() =
+        RxSO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
+    return upsilon_omega_sigma;
+  }
+
+ protected:
+  RxSo3Member rxso3_;
+  TranslationMember translation_;
+};
+
+template <class Scalar, int Options>
+Sim3<Scalar, Options>::Sim3() : translation_(TranslationMember::Zero()) {
+  static_assert(std::is_standard_layout<Sim3>::value,
+                "Assume standard layout for the use of offsetof check below.");
+  static_assert(
+      offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::num_parameters ==
+          offsetof(Sim3, translation_),
+      "This class assumes packed storage and hence will only work "
+      "correctly depending on the compiler (options) - in "
+      "particular when using [this->data(), this-data() + "
+      "num_parameters] to access the raw data in a contiguous fashion.");
+}
+
+}  // namespace Sophus
+
+namespace Eigen {
+
+/// Specialization of Eigen::Map for ``Sim3``; derived from Sim3Base.
+///
+/// Allows us to wrap Sim3 objects around POD array.
+template <class Scalar_, int Options>
+class Map<Sophus::Sim3<Scalar_>, Options>
+    : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> {
+ public:
+  using Base = Sophus::Sim3Base<Map<Sophus::Sim3<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)
+      : rxso3_(coeffs),
+        translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}
+
+  /// Mutator of RxSO3
+  ///
+  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rxso3_; }
+
+  /// Accessor of RxSO3
+  ///
+  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3() const {
+    return rxso3_;
+  }
+
+  /// Mutator of translation vector
+  ///
+  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation() {
+    return translation_;
+  }
+
+  /// Accessor of translation vector
+  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation() const {
+    return translation_;
+  }
+
+ protected:
+  Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
+  Map<Sophus::Vector3<Scalar>, Options> translation_;
+};
+
+/// Specialization of Eigen::Map for ``Sim3 const``; derived from Sim3Base.
+///
+/// Allows us to wrap RxSO3 objects around POD array.
+template <class Scalar_, int Options>
+class Map<Sophus::Sim3<Scalar_> const, Options>
+    : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> {
+  using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>;
+
+ public:
+  using Scalar = Scalar_;
+  using Transformation = typename Base::Transformation;
+  using Point = typename Base::Point;
+  using HomogeneousPoint = typename Base::HomogeneousPoint;
+  using Tangent = typename Base::Tangent;
+  using Adjoint = typename Base::Adjoint;
+
+  using Base::operator*=;
+  using Base::operator*;
+
+  SOPHUS_FUNC Map(Scalar const* coeffs)
+      : rxso3_(coeffs),
+        translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}
+
+  /// Accessor of RxSO3
+  ///
+  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const& rxso3() const {
+    return rxso3_;
+  }
+
+  /// Accessor of translation vector
+  ///
+  SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
+      const {
+    return translation_;
+  }
+
+ protected:
+  Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_;
+  Map<Sophus::Vector3<Scalar> const, Options> const translation_;
+};
+}  // namespace Eigen
+
+#endif

lio/include/sophus/sim_details.hpp

@@ -0,0 +1,103 @@
+#ifndef SOPHUS_SIM_DETAILS_HPP
+#define SOPHUS_SIM_DETAILS_HPP
+
+#include "types.hpp"
+
+namespace Sophus {
+namespace details {
+
+template <class Scalar, int N>
+Matrix<Scalar, N, N> calcW(Matrix<Scalar, N, N> const& Omega,
+                           Scalar const theta, Scalar const sigma) {
+  using std::abs;
+  using std::cos;
+  using std::exp;
+  using std::sin;
+  static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity();
+  static Scalar const one(1);
+  static Scalar const half(0.5);
+  Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
+  Scalar const scale = exp(sigma);
+  Scalar A, B, C;
+  if (abs(sigma) < Constants<Scalar>::epsilon()) {
+    C = one;
+    if (abs(theta) < Constants<Scalar>::epsilon()) {
+      A = half;
+      B = Scalar(1. / 6.);
+    } else {
+      Scalar theta_sq = theta * theta;
+      A = (one - cos(theta)) / theta_sq;
+      B = (theta - sin(theta)) / (theta_sq * theta);
+    }
+  } else {
+    C = (scale - one) / sigma;
+    if (abs(theta) < Constants<Scalar>::epsilon()) {
+      Scalar sigma_sq = sigma * sigma;
+      A = ((sigma - one) * scale + one) / sigma_sq;
+      B = (scale * half * sigma_sq + scale - one - sigma * scale) /
+          (sigma_sq * sigma);
+    } else {
+      Scalar theta_sq = theta * theta;
+      Scalar a = scale * sin(theta);
+      Scalar b = scale * cos(theta);
+      Scalar c = theta_sq + sigma * sigma;
+      A = (a * sigma + (one - b) * theta) / (theta * c);
+      B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq);
+    }
+  }
+  return A * Omega + B * Omega2 + C * I;
+}
+
+template <class Scalar, int N>
+Matrix<Scalar, N, N> calcWInv(Matrix<Scalar, N, N> const& Omega,
+                              Scalar const theta, Scalar const sigma,
+                              Scalar const scale) {
+  using std::abs;
+  using std::cos;
+  using std::sin;
+  static Matrix<Scalar, N, N> const I = Matrix<Scalar, N, N>::Identity();
+  static Scalar const half(0.5);
+  static Scalar const one(1);
+  static Scalar const two(2);
+  Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
+  Scalar const scale_sq = scale * scale;
+  Scalar const theta_sq = theta * theta;
+  Scalar const sin_theta = sin(theta);
+  Scalar const cos_theta = cos(theta);
+
+  Scalar a, b, c;
+  if (abs(sigma * sigma) < Constants<Scalar>::epsilon()) {
+    c = one - half * sigma;
+    a = -half;
+    if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
+      b = Scalar(1. / 12.);
+    } else {
+      b = (theta * sin_theta + two * cos_theta - two) /
+          (two * theta_sq * (cos_theta - one));
+    }
+  } else {
+    Scalar const scale_cu = scale_sq * scale;
+    c = sigma / (scale - one);
+    if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
+      a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one));
+      b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) /
+          (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two);
+    } else {
+      Scalar const s_sin_theta = scale * sin_theta;
+      Scalar const s_cos_theta = scale * cos_theta;
+      a = (theta * s_cos_theta - theta - sigma * s_sin_theta) /
+          (theta * (scale_sq - two * s_cos_theta + one));
+      b = -scale *
+          (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta -
+           scale * sigma + sigma * cos_theta - sigma) /
+          (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq +
+                       two * s_cos_theta + scale - one));
+    }
+  }
+  return a * Omega + b * Omega2 + c * I;
+}
+
+}  // namespace details
+}  // namespace Sophus
+
+#endif

lio/include/sophus/so2.hpp

@@ -0,0 +1,626 @@
+/// @file
+/// Special orthogonal group SO(2) - rotation in 2d.
+
+#ifndef SOPHUS_SO2_HPP
+#define SOPHUS_SO2_HPP
+
+#include <complex>
+#include <type_traits>
+
+// Include only the selective set of Eigen headers that we need.
+// This helps when using Sophus with unusual compilers, like nvcc.
+#include <Eigen/LU>
+
+#include "rotation_matrix.hpp"
+#include "types.hpp"
+
+namespace Sophus {
+template <class Scalar_, int Options = 0>
+class SO2;
+using SO2d = SO2<double>;
+using SO2f = SO2<float>;
+}  // namespace Sophus
+
+namespace Eigen {
+namespace internal {
+
+template <class Scalar_, int Options_>
+struct traits<Sophus::SO2<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::SO2<Scalar_>, Options_>>
+    : traits<Sophus::SO2<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::SO2<Scalar_> const, Options_>>
+    : traits<Sophus::SO2<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 {
+
+/// SO2 base type - implements SO2 class but is storage agnostic.
+///
+/// SO(2) is the group of rotations in 2d. As a matrix group, it is the set of
+/// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being
+/// the transpose of ``R``) and have a positive determinant. In particular, the
+/// determinant is 1. Let ``theta`` be the rotation angle, the rotation matrix
+/// can be written in close form:
+///
+///      | cos(theta) -sin(theta) |
+///      | sin(theta)  cos(theta) |
+///
+/// As a matter of fact, the first column of those matrices is isomorph to the
+/// set of unit complex numbers U(1). Thus, internally, SO2 is represented as
+/// complex number with length 1.
+///
+/// SO(2) is a compact and commutative group. First it is compact since the set
+/// of rotation matrices is a closed and bounded set. Second it is commutative
+/// since ``R(alpha) * R(beta) = R(beta) * R(alpha)``,  simply because ``alpha +
+/// beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles
+/// (about the same axis).
+///
+/// Class invariant: The 2-norm of ``unit_complex`` must be close to 1.
+/// Technically speaking, it must hold that:
+///
+///   ``|unit_complex().squaredNorm() - 1| <= Constants::epsilon()``.
+template <class Derived>
+class SO2Base {
+ public:
+  static constexpr int Options = Eigen::internal::traits<Derived>::Options;
+  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
+  using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;
+  using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
+
+  /// Degrees of freedom of manifold, number of dimensions in tangent space (one
+  /// since we only have in-plane rotations).
+  static int constexpr DoF = 1;
+  /// Number of internal parameters used (complex numbers are a tuples).
+  static int constexpr num_parameters = 2;
+  /// Group transformations are 2x2 matrices.
+  static int constexpr N = 2;
+  using Transformation = Matrix<Scalar, N, N>;
+  using Point = Vector2<Scalar>;
+  using HomogeneousPoint = Vector3<Scalar>;
+  using Line = ParametrizedLine2<Scalar>;
+  using Tangent = Scalar;
+  using Adjoint = Scalar;
+
+  /// For binary operations the return type is determined with the
+  /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
+  /// types, as well as other compatible scalar types such as Ceres::Jet and
+  /// double scalars with SO2 operations.
+  template <typename OtherDerived>
+  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
+      Scalar, typename OtherDerived::Scalar>::ReturnType;
+
+  template <typename OtherDerived>
+  using SO2Product = SO2<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.
+  ///
+  /// It simply ``1``, since ``SO(2)`` is a commutative group.
+  ///
+  SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); }
+
+  /// Returns copy of instance casted to NewScalarType.
+  ///
+  template <class NewScalarType>
+  SOPHUS_FUNC SO2<NewScalarType> cast() const {
+    return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
+  }
+
+  /// This provides unsafe read/write access to internal data. SO(2) is
+  /// represented by a unit complex number (two parameters). When using direct
+  /// write access, the user needs to take care of that the complex number stays
+  /// normalized.
+  ///
+  SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
+
+  /// Const version of data() above.
+  ///
+  SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
+
+  /// Returns group inverse.
+  ///
+  SOPHUS_FUNC SO2<Scalar> inverse() const {
+    return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
+  }
+
+  /// Logarithmic map
+  ///
+  /// Computes the logarithm, the inverse of the group exponential which maps
+  /// element of the group (rotation matrices) to elements of the tangent space
+  /// (rotation angles).
+  ///
+  /// To be specific, this function computes ``vee(logmat(.))`` with
+  /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
+  /// of SO(2).
+  ///
+  SOPHUS_FUNC Scalar log() const {
+    using std::atan2;
+    return atan2(unit_complex().y(), unit_complex().x());
+  }
+
+  /// It re-normalizes ``unit_complex`` to unit length.
+  ///
+  /// Note: Because of the class invariant, there is typically no need to call
+  /// this function directly.
+  ///
+  SOPHUS_FUNC void normalize() {
+    using std::sqrt;
+    Scalar length = sqrt(unit_complex().x() * unit_complex().x() +
+                         unit_complex().y() * unit_complex().y());
+    SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
+                  "Complex number should not be close to zero!");
+    unit_complex_nonconst().x() /= length;
+    unit_complex_nonconst().y() /= length;
+  }
+
+  /// Returns 2x2 matrix representation of the instance.
+  ///
+  /// For SO(2), the matrix representation is an orthogonal matrix ``R`` with
+  /// ``det(R)=1``, thus the so-called "rotation matrix".
+  ///
+  SOPHUS_FUNC Transformation matrix() const {
+    Scalar const& real = unit_complex().x();
+    Scalar const& imag = unit_complex().y();
+    Transformation R;
+    // clang-format off
+    R <<
+      real, -imag,
+      imag,  real;
+    // clang-format on
+    return R;
+  }
+
+  /// Assignment operator
+  ///
+  SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) = default;
+
+  /// Assignment-like operator from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {
+    unit_complex_nonconst() = other.unit_complex();
+    return *this;
+  }
+
+  /// Group multiplication, which is rotation concatenation.
+  ///
+  template <typename OtherDerived>
+  SOPHUS_FUNC SO2Product<OtherDerived> operator*(
+      SO2Base<OtherDerived> const& other) const {
+    using ResultT = ReturnScalar<OtherDerived>;
+    Scalar const lhs_real = unit_complex().x();
+    Scalar const lhs_imag = unit_complex().y();
+    typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x();
+    typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y();
+    // complex multiplication
+    ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag;
+    ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real;
+
+    ResultT const squared_norm =
+        result_real * result_real + result_imag * result_imag;
+    // We can assume that the squared-norm is close to 1 since we deal with a
+    // unit complex number. Due to numerical precision issues, there might
+    // be a small drift after pose concatenation. Hence, we need to renormalizes
+    // the complex number here.
+    // Since squared-norm is close to 1, we do not need to calculate the costly
+    // square-root, but can use an approximation around 1 (see
+    // http://stackoverflow.com/a/12934750 for details).
+    if (squared_norm != ResultT(1.0)) {
+      ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm);
+      return SO2Product<OtherDerived>(result_real * scale, result_imag * scale);
+    }
+    return SO2Product<OtherDerived>(result_real, result_imag);
+  }
+
+  /// Group action on 2-points.
+  ///
+  /// This function rotates a 2 dimensional point ``p`` by the SO2 element
+  ///  ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
+  ///
+  template <typename PointDerived,
+            typename = typename std::enable_if<
+                IsFixedSizeVector<PointDerived, 2>::value>::type>
+  SOPHUS_FUNC PointProduct<PointDerived> operator*(
+      Eigen::MatrixBase<PointDerived> const& p) const {
+    Scalar const& real = unit_complex().x();
+    Scalar const& imag = unit_complex().y();
+    return PointProduct<PointDerived>(real * p[0] - imag * p[1],
+                                      imag * p[0] + real * p[1]);
+  }
+
+  /// Group action on homogeneous 2-points.
+  ///
+  /// This function rotates a homogeneous 2 dimensional point ``p`` by the SO2
+  /// element ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
+  ///
+  template <typename HPointDerived,
+            typename = typename std::enable_if<
+                IsFixedSizeVector<HPointDerived, 3>::value>::type>
+  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
+      Eigen::MatrixBase<HPointDerived> const& p) const {
+    Scalar const& real = unit_complex().x();
+    Scalar const& imag = unit_complex().y();
+    return HomogeneousPointProduct<HPointDerived>(
+        real * p[0] - imag * p[1], imag * p[0] + real * p[1], p[2]);
+  }
+
+  /// Group action on lines.
+  ///
+  /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO2
+  /// element:
+  ///
+  /// Both direction ``d`` and origin ``o`` are rotated as a 2 dimensional point
+  ///
+  SOPHUS_FUNC Line operator*(Line const& l) const {
+    return Line((*this) * l.origin(), (*this) * l.direction());
+  }
+
+  /// In-place group multiplication. This method is only valid if the return
+  /// type of the multiplication is compatible with this SO2's Scalar type.
+  ///
+  template <typename OtherDerived,
+            typename = typename std::enable_if<
+                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
+  SOPHUS_FUNC SO2Base<Derived> operator*=(SO2Base<OtherDerived> const& other) {
+    *static_cast<Derived*>(this) = *this * other;
+    return *this;
+  }
+
+  /// Returns derivative of  this * SO2::exp(x)  wrt. x at x=0.
+  ///
+  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
+      const {
+    return Matrix<Scalar, num_parameters, DoF>(-unit_complex()[1],
+                                               unit_complex()[0]);
+  }
+
+  /// Returns internal parameters of SO(2).
+  ///
+  /// It returns (c[0], c[1]), with c being the unit complex number.
+  ///
+  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
+    return unit_complex();
+  }
+
+  /// Takes in complex number / tuple and normalizes it.
+  ///
+  /// Precondition: The complex number must not be close to zero.
+  ///
+  SOPHUS_FUNC void setComplex(Point const& complex) {
+    unit_complex_nonconst() = complex;
+    normalize();
+  }
+
+  /// Accessor of unit quaternion.
+  ///
+  SOPHUS_FUNC
+  ComplexT const& unit_complex() const {
+    return static_cast<Derived const*>(this)->unit_complex();
+  }
+
+ private:
+  /// Mutator of unit_complex is private to ensure class invariant. That is
+  /// the complex number must stay close to unit length.
+  ///
+  SOPHUS_FUNC
+  ComplexT& unit_complex_nonconst() {
+    return static_cast<Derived*>(this)->unit_complex_nonconst();
+  }
+};
+
+/// SO2 using  default storage; derived from SO2Base.
+template <class Scalar_, int Options>
+class SO2 : public SO2Base<SO2<Scalar_, Options>> {
+ public:
+  using Base = SO2Base<SO2<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 ComplexMember = Vector2<Scalar, Options>;
+
+  /// ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
+  friend class SO2Base<SO2<Scalar, Options>>;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Default constructor initializes unit complex number to identity rotation.
+  ///
+  SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
+
+  /// Copy constructor
+  ///
+  SOPHUS_FUNC SO2(SO2 const& other) = default;
+
+  /// Copy-like constructor from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC SO2(SO2Base<OtherDerived> const& other)
+      : unit_complex_(other.unit_complex()) {}
+
+  /// Constructor from rotation matrix
+  ///
+  /// Precondition: rotation matrix need to be orthogonal with determinant of 1.
+  ///
+  SOPHUS_FUNC explicit SO2(Transformation const& R)
+      : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)),
+                      Scalar(0.5) * (R(1, 0) - R(0, 1))) {
+    SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
+    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
+                  R.determinant());
+  }
+
+  /// Constructor from pair of real and imaginary number.
+  ///
+  /// Precondition: The pair must not be close to zero.
+  ///
+  SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
+      : unit_complex_(real, imag) {
+    Base::normalize();
+  }
+
+  /// Constructor from 2-vector.
+  ///
+  /// Precondition: The vector must not be close to zero.
+  ///
+  template <class D>
+  SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
+      : unit_complex_(complex) {
+    static_assert(std::is_same<typename D::Scalar, Scalar>::value,
+                  "must be same Scalar type");
+    Base::normalize();
+  }
+
+  /// Constructor from an rotation angle.
+  ///
+  SOPHUS_FUNC explicit SO2(Scalar theta) {
+    unit_complex_nonconst() = SO2<Scalar>::exp(theta).unit_complex();
+  }
+
+  /// Accessor of unit complex number
+  ///
+  SOPHUS_FUNC ComplexMember const& unit_complex() const {
+    return unit_complex_;
+  }
+
+  /// Group exponential
+  ///
+  /// This functions takes in an element of tangent space (= rotation angle
+  /// ``theta``) and returns the corresponding element of the group SO(2).
+  ///
+  /// To be more specific, this function computes ``expmat(hat(omega))``
+  /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
+  /// hat()-operator of SO(2).
+  ///
+  SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
+    using std::cos;
+    using std::sin;
+    return SO2<Scalar>(cos(theta), sin(theta));
+  }
+
+  /// Returns derivative of exp(x) wrt. x.
+  ///
+  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
+      Tangent const& theta) {
+    using std::cos;
+    using std::sin;
+    return Sophus::Matrix<Scalar, num_parameters, DoF>(-sin(theta), cos(theta));
+  }
+
+  /// 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() {
+    return Sophus::Matrix<Scalar, num_parameters, DoF>(Scalar(0), Scalar(1));
+  }
+
+  /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
+  ///
+  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {
+    return generator();
+  }
+
+  /// Returns the infinitesimal generators of SO(2).
+  ///
+  /// The infinitesimal generators of SO(2) is:
+  ///
+  ///     |  0  1 |
+  ///     | -1  0 |
+  ///
+  SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); }
+
+  /// hat-operator
+  ///
+  /// It takes in the scalar representation ``theta`` (= rotation angle) and
+  /// returns the corresponding matrix representation of Lie algebra element.
+  ///
+  /// Formally, the hat()-operator of SO(2) is defined as
+  ///
+  ///   ``hat(.): R^2 -> R^{2x2},  hat(theta) = theta * G``
+  ///
+  /// with ``G`` being the infinitesimal generator of SO(2).
+  ///
+  /// The corresponding inverse is the vee()-operator, see below.
+  ///
+  SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
+    Transformation Omega;
+    // clang-format off
+    Omega <<
+        Scalar(0),   -theta,
+            theta, Scalar(0);
+    // clang-format on
+    return Omega;
+  }
+
+  /// Returns closed SO2 given arbitrary 2x2 matrix.
+  ///
+  template <class S = Scalar>
+  static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO2>
+  fitToSO2(Transformation const& R) {
+    return SO2(makeRotationMatrix(R));
+  }
+
+  /// Lie bracket
+  ///
+  /// It returns the Lie bracket of SO(2). Since SO(2) is a commutative group,
+  /// the Lie bracket is simple ``0``.
+  ///
+  SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
+    return Scalar(0);
+  }
+
+  /// Draw uniform sample from SO(2) manifold.
+  ///
+  template <class UniformRandomBitGenerator>
+  static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
+    static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
+                  "generator must meet the UniformRandomBitGenerator concept");
+    std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),
+                                                   Constants<Scalar>::pi());
+    return SO2(uniform(generator));
+  }
+
+  /// vee-operator
+  ///
+  /// It takes the 2x2-matrix representation ``Omega`` and maps it to the
+  /// corresponding scalar representation of Lie algebra.
+  ///
+  /// This is the inverse of the hat()-operator, see above.
+  ///
+  /// Precondition: ``Omega`` must have the following structure:
+  ///
+  ///                |  0 -a |
+  ///                |  a  0 |
+  ///
+  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
+    using std::abs;
+    return Omega(1, 0);
+  }
+
+ protected:
+  /// Mutator of complex number is protected to ensure class invariant.
+  ///
+  SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; }
+
+  ComplexMember unit_complex_;
+};
+
+}  // namespace Sophus
+
+namespace Eigen {
+
+/// Specialization of Eigen::Map for ``SO2``; derived from SO2Base.
+///
+/// Allows us to wrap SO2 objects around POD array (e.g. external c style
+/// complex number / tuple).
+template <class Scalar_, int Options>
+class Map<Sophus::SO2<Scalar_>, Options>
+    : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
+ public:
+  using Base = Sophus::SO2Base<Map<Sophus::SO2<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 unit_complex_nonconst can be accessed from ``Base``.
+  friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
+
+  // LCOV_EXCL_START
+  SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map);
+  // LCOV_EXCL_STOP
+
+  using Base::operator*=;
+  using Base::operator*;
+
+  SOPHUS_FUNC
+  Map(Scalar* coeffs) : unit_complex_(coeffs) {}
+
+  /// Accessor of unit complex number.
+  ///
+  SOPHUS_FUNC
+  Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
+    return unit_complex_;
+  }
+
+ protected:
+  /// Mutator of unit_complex is protected to ensure class invariant.
+  ///
+  SOPHUS_FUNC
+  Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
+    return unit_complex_;
+  }
+
+  Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;
+};
+
+/// Specialization of Eigen::Map for ``SO2 const``; derived from SO2Base.
+///
+/// Allows us to wrap SO2 objects around POD array (e.g. external c style
+/// complex number / tuple).
+template <class Scalar_, int Options>
+class Map<Sophus::SO2<Scalar_> const, Options>
+    : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
+ public:
+  using Base = Sophus::SO2Base<Map<Sophus::SO2<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) : unit_complex_(coeffs) {}
+
+  /// Accessor of unit complex number.
+  ///
+  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
+      const {
+    return unit_complex_;
+  }
+
+ protected:
+  /// Mutator of unit_complex is protected to ensure class invariant.
+  ///
+  Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
+};
+}  // namespace Eigen
+
+#endif  // SOPHUS_SO2_HPP

lio/include/sophus/so3.hpp

@@ -0,0 +1,877 @@
+/// @file
+/// Special orthogonal group SO(3) - rotation in 3d.
+
+#ifndef SOPHUS_SO3_HPP
+#define SOPHUS_SO3_HPP
+
+#include "rotation_matrix.hpp"
+#include "so2.hpp"
+#include "types.hpp"
+
+// Include only the selective set of Eigen headers that we need.
+// This helps when using Sophus with unusual compilers, like nvcc.
+#include <Eigen/src/Geometry/OrthoMethods.h>
+#include <Eigen/src/Geometry/Quaternion.h>
+#include <Eigen/src/Geometry/RotationBase.h>
+
+namespace Sophus {
+template <class Scalar_, int Options = 0>
+class SO3;
+using SO3d = SO3<double>;
+using SO3f = SO3<float>;
+}  // namespace Sophus
+
+namespace Eigen {
+namespace internal {
+
+template <class Scalar_, int Options_>
+struct traits<Sophus::SO3<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::SO3<Scalar_>, Options_>>
+    : traits<Sophus::SO3<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::SO3<Scalar_> const, Options_>>
+    : traits<Sophus::SO3<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 {
+
+/// SO3 base type - implements SO3 class but is storage agnostic.
+///
+/// SO(3) is the group of rotations in 3d. As a matrix group, it is the set of
+/// matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being
+/// the transpose of ``R``) and have a positive determinant. In particular, the
+/// determinant is 1. Internally, the group is represented as a unit quaternion.
+/// Unit quaternion can be seen as members of the special unitary group SU(2).
+/// SU(2) is a double cover of SO(3). Hence, for every rotation matrix ``R``,
+/// there exist two unit quaternions: ``(r, v)`` and ``(-r, -v)``, with ``r``
+/// the real part and ``v`` being the imaginary 3-vector part of the quaternion.
+///
+/// SO(3) is a compact, but non-commutative group. First it is compact since the
+/// set of rotation matrices is a closed and bounded set. Second it is
+/// non-commutative since the equation ``R_1 * R_2 = R_2 * R_1`` does not hold
+/// in general. For example rotating an object by some degrees about its
+/// ``x``-axis and then by some degrees about its y axis, does not lead to the
+/// same orientation when rotation first about ``y`` and then about ``x``.
+///
+/// Class invariant: The 2-norm of ``unit_quaternion`` must be close to 1.
+/// Technically speaking, it must hold that:
+///
+///   ``|unit_quaternion().squaredNorm() - 1| <= Constants::epsilon()``.
+template <class Derived>
+class SO3Base {
+ 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 group, number of dimensions in tangent space.
+  static int constexpr DoF = 3;
+  /// Number of internal parameters used (quaternion is a 4-tuple).
+  static int constexpr num_parameters = 4;
+  /// Group transformations are 3x3 matrices.
+  static int constexpr N = 3;
+  using Transformation = Matrix<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 SO3 operations.
+  template <typename OtherDerived>
+  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
+      Scalar, typename OtherDerived::Scalar>::ReturnType;
+
+  template <typename OtherDerived>
+  using SO3Product = SO3<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 SO(3), it simply returns the rotation matrix corresponding to ``A``.
+  ///
+  SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
+
+  /// Extract rotation angle about canonical X-axis
+  ///
+  template <class S = Scalar>
+  SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX() const {
+    Sophus::Matrix3<Scalar> R = matrix();
+    Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);
+    return SO2<Scalar>(makeRotationMatrix(Rx)).log();
+  }
+
+  /// Extract rotation angle about canonical Y-axis
+  ///
+  template <class S = Scalar>
+  SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY() const {
+    Sophus::Matrix3<Scalar> R = matrix();
+    Sophus::Matrix2<Scalar> Ry;
+    // clang-format off
+    Ry <<
+      R(0, 0), R(2, 0),
+      R(0, 2), R(2, 2);
+    // clang-format on
+    return SO2<Scalar>(makeRotationMatrix(Ry)).log();
+  }
+
+  /// Extract rotation angle about canonical Z-axis
+  ///
+  template <class S = Scalar>
+  SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ() const {
+    Sophus::Matrix3<Scalar> R = matrix();
+    Sophus::Matrix2<Scalar> Rz = R.template block<2, 2>(0, 0);
+    return SO2<Scalar>(makeRotationMatrix(Rz)).log();
+  }
+
+  /// Returns copy of instance casted to NewScalarType.
+  ///
+  template <class NewScalarType>
+  SOPHUS_FUNC SO3<NewScalarType> cast() const {
+    return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());
+  }
+
+  /// This provides unsafe read/write access to internal data. SO(3) is
+  /// represented by an Eigen::Quaternion (four parameters). When using direct
+  /// write access, the user needs to take care of that the quaternion stays
+  /// normalized.
+  ///
+  /// Note: The first three Scalars represent the imaginary parts, while the
+  /// forth Scalar represent the real part.
+  ///
+  SOPHUS_FUNC Scalar* data() {
+    return unit_quaternion_nonconst().coeffs().data();
+  }
+
+  /// Const version of data() above.
+  ///
+  SOPHUS_FUNC Scalar const* data() const {
+    return unit_quaternion().coeffs().data();
+  }
+
+  /// Returns derivative of  this * SO3::exp(x)  wrt. x at x=0.
+  ///
+  SOPHUS_FUNC Matrix<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;
+    J(0, 0) = c0;
+    J(0, 1) = c2;
+    J(0, 2) = c3;
+    J(1, 0) = c1;
+    J(1, 1) = c0;
+    J(1, 2) = c5;
+    J(2, 0) = c6;
+    J(2, 1) = c4;
+    J(2, 2) = c0;
+    J(3, 0) = c5;
+    J(3, 1) = c6;
+    J(3, 2) = c2;
+
+    return J;
+  }
+
+  /// Returns internal parameters of SO(3).
+  ///
+  /// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the
+  /// unit quaternion.
+  ///
+  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
+    return unit_quaternion().coeffs();
+  }
+
+  /// Returns group inverse.
+  ///
+  SOPHUS_FUNC SO3<Scalar> inverse() const {
+    return SO3<Scalar>(unit_quaternion().conjugate());
+  }
+
+  /// Logarithmic map
+  ///
+  /// Computes the logarithm, the inverse of the group exponential which maps
+  /// element of the group (rotation matrices) to elements of the tangent space
+  /// (rotation-vector).
+  ///
+  /// To be specific, this function computes ``vee(logmat(.))`` with
+  /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
+  /// of SO(3).
+  ///
+  SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
+
+  /// As above, but also returns ``theta = |omega|``.
+  ///
+  SOPHUS_FUNC TangentAndTheta logAndTheta() const {
+    TangentAndTheta J;
+    using std::abs;
+    using std::atan;
+    using std::sqrt;
+    Scalar squared_n = unit_quaternion().vec().squaredNorm();
+    Scalar w = unit_quaternion().w();
+
+    Scalar two_atan_nbyw_by_n;
+
+    /// Atan-based log thanks to
+    ///
+    /// C. Hertzberg et al.:
+    /// "Integrating Generic Sensor Fusion Algorithms with Sound State
+    /// Representation through Encapsulation of Manifolds"
+    /// Information Fusion, 2011
+
+    if (squared_n < Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
+      // If quaternion is normalized and n=0, then w should be 1;
+      // w=0 should never happen here!
+      SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::epsilon(),
+                    "Quaternion (%) should be normalized!",
+                    unit_quaternion().coeffs().transpose());
+      Scalar squared_w = w * w;
+      two_atan_nbyw_by_n =
+          Scalar(2) / w - Scalar(2.0/3.0) * (squared_n) / (w * squared_w);
+      J.theta = Scalar(2) * squared_n / w;
+    } else {
+      Scalar n = sqrt(squared_n);
+      if (abs(w) < Constants<Scalar>::epsilon()) {
+        if (w > Scalar(0)) {
+          two_atan_nbyw_by_n = Constants<Scalar>::pi() / n;
+        } else {
+          two_atan_nbyw_by_n = -Constants<Scalar>::pi() / n;
+        }
+      } else {
+        two_atan_nbyw_by_n = Scalar(2) * atan(n / w) / n;
+      }
+      J.theta = two_atan_nbyw_by_n * n;
+    }
+
+    J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec();
+    return J;
+  }
+
+  /// It re-normalizes ``unit_quaternion`` to unit length.
+  ///
+  /// Note: Because of the class invariant, there is typically no need to call
+  /// this function directly.
+  ///
+  SOPHUS_FUNC void normalize() {
+    Scalar length = unit_quaternion_nonconst().norm();
+    SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
+                  "Quaternion (%) should not be close to zero!",
+                  unit_quaternion_nonconst().coeffs().transpose());
+    unit_quaternion_nonconst().coeffs() /= length;
+  }
+
+  /// Returns 3x3 matrix representation of the instance.
+  ///
+  /// For SO(3), the matrix representation is an orthogonal matrix ``R`` with
+  /// ``det(R)=1``, thus the so-called "rotation matrix".
+  ///
+  SOPHUS_FUNC Transformation matrix() const {
+    return unit_quaternion().toRotationMatrix();
+  }
+
+  /// Assignment operator.
+  ///
+  SOPHUS_FUNC SO3Base& operator=(SO3Base const& other) = default;
+
+  /// Assignment-like operator from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& other) {
+    unit_quaternion_nonconst() = other.unit_quaternion();
+    return *this;
+  }
+
+  /// Group multiplication, which is rotation concatenation.
+  ///
+  template <typename OtherDerived>
+  SOPHUS_FUNC SO3Product<OtherDerived> operator*(
+      SO3Base<OtherDerived> const& other) const {
+    using QuaternionProductType =
+        typename SO3Product<OtherDerived>::QuaternionType;
+    const QuaternionType& a = unit_quaternion();
+    const typename OtherDerived::QuaternionType& b = other.unit_quaternion();
+    /// NOTE: We cannot use Eigen's Quaternion multiplication because it always
+    /// returns a Quaternion of the same Scalar as this object, so it is not
+    /// able to multiple Jets and doubles correctly.
+    return SO3Product<OtherDerived>(QuaternionProductType(
+        a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
+        a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
+        a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
+        a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()));
+  }
+
+  /// Group action on 3-points.
+  ///
+  /// This function rotates a 3 dimensional point ``p`` by the SO3 element
+  ///  ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
+  ///
+  /// Since SO3 is internally represented by a unit quaternion ``q``, it is
+  /// implemented as ``p_bar = q * p_foo * q^{*}``
+  /// with ``q^{*}`` being the quaternion conjugate of ``q``.
+  ///
+  /// Geometrically, ``p``  is rotated by angle ``|omega|`` around the
+  /// axis ``omega/|omega|`` with ``omega := vee(log(bar_R_foo))``.
+  ///
+  /// For ``vee``-operator, see below.
+  ///
+  template <typename PointDerived,
+            typename = typename std::enable_if<
+                IsFixedSizeVector<PointDerived, 3>::value>::type>
+  SOPHUS_FUNC PointProduct<PointDerived> operator*(
+      Eigen::MatrixBase<PointDerived> const& p) const {
+    /// NOTE: We cannot use Eigen's Quaternion transformVector because it always
+    /// returns a Vector3 of the same Scalar as this quaternion, so it is not
+    /// able to be applied to Jets and doubles correctly.
+    const QuaternionType& q = unit_quaternion();
+    PointProduct<PointDerived> uv = q.vec().cross(p);
+    uv += uv;
+    return p + q.w() * uv + q.vec().cross(uv);
+  }
+
+  /// Group action on homogeneous 3-points. See above for more details.
+  template <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 rp = *this * p.template head<3>();
+    return HomogeneousPointProduct<HPointDerived>(rp(0), rp(1), rp(2), p(3));
+  }
+
+  /// Group action on lines.
+  ///
+  /// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3
+  /// element:
+  ///
+  /// Both direction ``d`` and origin ``o`` are rotated as a 3 dimensional point
+  ///
+  SOPHUS_FUNC Line operator*(Line const& l) const {
+    return Line((*this) * l.origin(), (*this) * l.direction());
+  }
+
+  /// In-place group multiplication. This method is only valid if the return
+  /// type of the multiplication is compatible with this SO3's Scalar type.
+  ///
+  template <typename OtherDerived,
+            typename = typename std::enable_if<
+                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
+  SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& other) {
+    *static_cast<Derived*>(this) = *this * other;
+    return *this;
+  }
+
+  /// Takes in quaternion, and normalizes it.
+  ///
+  /// Precondition: The quaternion must not be close to zero.
+  ///
+  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quaternion) {
+    unit_quaternion_nonconst() = quaternion;
+    normalize();
+  }
+
+  /// Accessor of unit quaternion.
+  ///
+  SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
+    return static_cast<Derived const*>(this)->unit_quaternion();
+  }
+
+ private:
+  /// Mutator of unit_quaternion is private to ensure class invariant. That is
+  /// the quaternion must stay close to unit length.
+  ///
+  SOPHUS_FUNC QuaternionType& unit_quaternion_nonconst() {
+    return static_cast<Derived*>(this)->unit_quaternion_nonconst();
+  }
+};
+
+/// SO3 using default storage; derived from SO3Base.
+template <class Scalar_, int Options>
+class SO3 : public SO3Base<SO3<Scalar_, Options>> {
+ public:
+  using Base = SO3Base<SO3<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 QuaternionMember = Eigen::Quaternion<Scalar, Options>;
+
+  /// ``Base`` is friend so unit_quaternion_nonconst can be accessed from
+  /// ``Base``.
+  friend class SO3Base<SO3<Scalar, Options>>;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Default constructor initializes unit quaternion to identity rotation.
+  ///
+  SOPHUS_FUNC SO3()
+      : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
+
+  /// Copy constructor
+  ///
+  SOPHUS_FUNC SO3(SO3 const& other) = default;
+
+  /// Copy-like constructor from OtherDerived.
+  ///
+  template <class OtherDerived>
+  SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
+      : unit_quaternion_(other.unit_quaternion()) {}
+
+  /// Constructor from rotation matrix
+  ///
+  /// Precondition: rotation matrix needs to be orthogonal with determinant
+  /// of 1.
+  ///
+  SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) {
+    SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %",
+                  R * R.transpose());
+    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
+                  R.determinant());
+  }
+
+  /// Constructor from quaternion
+  ///
+  /// Precondition: quaternion must not be close to zero.
+  ///
+  template <class D>
+  SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
+      : unit_quaternion_(quat) {
+    static_assert(
+        std::is_same<typename Eigen::QuaternionBase<D>::Scalar, Scalar>::value,
+        "Input must be of same scalar type");
+    Base::normalize();
+  }
+
+  /// Accessor of unit quaternion.
+  ///
+  SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
+    return unit_quaternion_;
+  }
+
+  /// Returns derivative of exp(x) wrt. x.
+  ///
+  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
+      Tangent const& omega) {
+    using std::cos;
+    using std::exp;
+    using std::sin;
+    using std::sqrt;
+    Scalar const c0 = omega[0] * omega[0];
+    Scalar const c1 = omega[1] * omega[1];
+    Scalar const c2 = omega[2] * omega[2];
+    Scalar const c3 = c0 + c1 + c2;
+
+    if (c3 < Constants<Scalar>::epsilon()) {
+      return Dx_exp_x_at_0();
+    }
+
+    Scalar const c4 = sqrt(c3);
+    Scalar const c5 = 1.0 / c4;
+    Scalar const c6 = 0.5 * c4;
+    Scalar const c7 = sin(c6);
+    Scalar const c8 = c5 * c7;
+    Scalar const c9 = pow(c3, -3.0L / 2.0L);
+    Scalar const c10 = c7 * c9;
+    Scalar const c11 = Scalar(1.0) / c3;
+    Scalar const c12 = cos(c6);
+    Scalar const c13 = Scalar(0.5) * c11 * c12;
+    Scalar const c14 = c7 * c9 * omega[0];
+    Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
+    Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
+    Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
+    Scalar const c18 = omega[1] * omega[2];
+    Scalar const c19 = -c10 * c18 + c13 * c18;
+    Scalar const c20 = Scalar(0.5) * c5 * c7;
+    Sophus::Matrix<Scalar, num_parameters, DoF> J;
+    J(0, 0) = -c0 * c10 + c0 * c13 + c8;
+    J(0, 1) = c16;
+    J(0, 2) = c17;
+    J(1, 0) = c16;
+    J(1, 1) = -c1 * c10 + c1 * c13 + c8;
+    J(1, 2) = c19;
+    J(2, 0) = c17;
+    J(2, 1) = c19;
+    J(2, 2) = -c10 * c2 + c13 * c2 + c8;
+    J(3, 0) = -c20 * omega[0];
+    J(3, 1) = -c20 * omega[1];
+    J(3, 2) = -c20 * omega[2];
+    return J;
+  }
+
+  /// Returns derivative of exp(x) wrt. x_i at x=0.
+  ///
+  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
+  Dx_exp_x_at_0() {
+    Sophus::Matrix<Scalar, num_parameters, DoF> J;
+    // clang-format off
+    J <<  Scalar(0.5),   Scalar(0),   Scalar(0),
+            Scalar(0), Scalar(0.5),   Scalar(0),
+            Scalar(0),   Scalar(0), Scalar(0.5),
+            Scalar(0),   Scalar(0),   Scalar(0);
+    // clang-format on
+    return J;
+  }
+
+  /// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
+  ///
+  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
+    return generator(i);
+  }
+
+  /// Group exponential
+  ///
+  /// This functions takes in an element of tangent space (= rotation vector
+  /// ``omega``) and returns the corresponding element of the group SO(3).
+  ///
+  /// To be more specific, this function computes ``expmat(hat(omega))``
+  /// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
+  /// hat()-operator of SO(3).
+  ///
+  SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
+    Scalar theta;
+    return expAndTheta(omega, &theta);
+  }
+
+  /// As above, but also returns ``theta = |omega|`` as out-parameter.
+  ///
+  /// Precondition: ``theta`` must not be ``nullptr``.
+  ///
+  SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
+                                             Scalar* theta) {
+    SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
+    using std::abs;
+    using std::cos;
+    using std::sin;
+    using std::sqrt;
+    Scalar theta_sq = omega.squaredNorm();
+
+    Scalar imag_factor;
+    Scalar real_factor;
+    if (theta_sq <
+        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
+      *theta = Scalar(0);
+      Scalar theta_po4 = theta_sq * theta_sq;
+      imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
+                    Scalar(1.0 / 3840.0) * theta_po4;
+      real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq +
+                    Scalar(1.0 / 384.0) * theta_po4;
+    } else {
+      *theta = sqrt(theta_sq);
+      Scalar half_theta = Scalar(0.5) * (*theta);
+      Scalar sin_half_theta = sin(half_theta);
+      imag_factor = sin_half_theta / (*theta);
+      real_factor = cos(half_theta);
+    }
+
+    SO3 q;
+    q.unit_quaternion_nonconst() =
+        QuaternionMember(real_factor, imag_factor * omega.x(),
+                         imag_factor * omega.y(), imag_factor * omega.z());
+    SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <
+                      Sophus::Constants<Scalar>::epsilon(),
+                  "SO3::exp failed! omega: %, real: %, img: %",
+                  omega.transpose(), real_factor, imag_factor);
+    return q;
+  }
+
+  /// Returns closest SO3 given arbitrary 3x3 matrix.
+  ///
+  template <class S = Scalar>
+  static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3>
+  fitToSO3(Transformation const& R) {
+    return SO3(::Sophus::makeRotationMatrix(R));
+  }
+
+  /// Returns the ith infinitesimal generators of SO(3).
+  ///
+  /// The infinitesimal generators of SO(3) are:
+  ///
+  /// ```
+  ///         |  0  0  0 |
+  ///   G_0 = |  0  0 -1 |
+  ///         |  0  1  0 |
+  ///
+  ///         |  0  0  1 |
+  ///   G_1 = |  0  0  0 |
+  ///         | -1  0  0 |
+  ///
+  ///         |  0 -1  0 |
+  ///   G_2 = |  1  0  0 |
+  ///         |  0  0  0 |
+  /// ```
+  ///
+  /// Precondition: ``i`` must be 0, 1 or 2.
+  ///
+  SOPHUS_FUNC static Transformation generator(int i) {
+    SOPHUS_ENSURE(i >= 0 && i <= 2, "i should be in range [0,2].");
+    Tangent e;
+    e.setZero();
+    e[i] = Scalar(1);
+    return hat(e);
+  }
+
+  /// hat-operator
+  ///
+  /// It takes in the 3-vector representation ``omega`` (= rotation vector) and
+  /// returns the corresponding matrix representation of Lie algebra element.
+  ///
+  /// Formally, the hat()-operator of SO(3) is defined as
+  ///
+  ///   ``hat(.): R^3 -> R^{3x3},  hat(omega) = sum_i omega_i * G_i``
+  ///   (for i=0,1,2)
+  ///
+  /// with ``G_i`` being the ith infinitesimal generator of SO(3).
+  ///
+  /// The corresponding inverse is the vee()-operator, see below.
+  ///
+  SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
+    Transformation Omega;
+    // clang-format off
+    Omega <<
+        Scalar(0), -omega(2),  omega(1),
+         omega(2), Scalar(0), -omega(0),
+        -omega(1),  omega(0), Scalar(0);
+    // clang-format on
+    return Omega;
+  }
+
+  /// Lie bracket
+  ///
+  /// It computes the Lie bracket of SO(3). To be more specific, it computes
+  ///
+  ///   ``[omega_1, omega_2]_so3 := vee([hat(omega_1), hat(omega_2)])``
+  ///
+  /// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
+  /// hat()-operator and ``vee(.)`` the vee()-operator of SO3.
+  ///
+  /// For the Lie algebra so3, the Lie bracket is simply the cross product:
+  ///
+  /// ``[omega_1, omega_2]_so3 = omega_1 x omega_2.``
+  ///
+  SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
+                                        Tangent const& omega2) {
+    return omega1.cross(omega2);
+  }
+
+  /// Construct x-axis rotation.
+  ///
+  static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
+    return SO3::exp(Sophus::Vector3<Scalar>(x, Scalar(0), Scalar(0)));
+  }
+
+  /// Construct y-axis rotation.
+  ///
+  static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
+    return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), y, Scalar(0)));
+  }
+
+  /// Construct z-axis rotation.
+  ///
+  static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
+    return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), Scalar(0), z));
+  }
+
+  /// Draw uniform sample from SO(3) manifold.
+  /// Based on: http://planning.cs.uiuc.edu/node198.html
+  ///
+  template <class UniformRandomBitGenerator>
+  static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
+    static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
+                  "generator must meet the UniformRandomBitGenerator concept");
+
+    std::uniform_real_distribution<Scalar> uniform(Scalar(0), Scalar(1));
+    std::uniform_real_distribution<Scalar> uniform_twopi(
+        Scalar(0), 2 * Constants<Scalar>::pi());
+
+    const Scalar u1 = uniform(generator);
+    const Scalar u2 = uniform_twopi(generator);
+    const Scalar u3 = uniform_twopi(generator);
+
+    const Scalar a = sqrt(1 - u1);
+    const Scalar b = sqrt(u1);
+
+    return SO3(
+        QuaternionMember(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)));
+  }
+
+  /// vee-operator
+  ///
+  /// It takes the 3x3-matrix representation ``Omega`` and maps it to the
+  /// corresponding vector representation of Lie algebra.
+  ///
+  /// This is the inverse of the hat()-operator, see above.
+  ///
+  /// Precondition: ``Omega`` must have the following structure:
+  ///
+  ///                |  0 -c  b |
+  ///                |  c  0 -a |
+  ///                | -b  a  0 |
+  ///
+  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
+    return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0));
+  }
+
+  SOPHUS_FUNC static Transformation JacobianRInv(Tangent const& w) {
+    Transformation Jrinv = Transformation::Identity();
+    Scalar theta = w.norm();
+    Scalar theta2 = theta * theta;
+    Scalar s = ( 1.0 - (1.0+cos(theta))*theta / (2.0*sin(theta)) );
+
+    // very small angle
+    if(theta < 0.00001)
+    {
+      return Jrinv;
+    }
+    else
+    {
+      Tangent k = w.normalized();
+      Transformation K = SO3<Scalar>::hat(k);
+      Jrinv = Transformation::Identity()
+              + 0.5*SO3<Scalar>::hat(w)
+              + ( 1.0 - (1.0+cos(theta))*theta / (2.0*sin(theta)) ) *K*K;
+    }
+    return Jrinv;
+  }
+
+ protected:
+  /// Mutator of unit_quaternion is protected to ensure class invariant.
+  ///
+  SOPHUS_FUNC QuaternionMember& unit_quaternion_nonconst() {
+    return unit_quaternion_;
+  }
+
+  QuaternionMember unit_quaternion_;
+};
+
+}  // namespace Sophus
+
+namespace Eigen {
+/// Specialization of Eigen::Map for ``SO3``; derived from SO3Base.
+///
+/// Allows us to wrap SO3 objects around POD array (e.g. external c style
+/// quaternion).
+template <class Scalar_, int Options>
+class Map<Sophus::SO3<Scalar_>, Options>
+    : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>> {
+ public:
+  using Base = Sophus::SO3Base<Map<Sophus::SO3<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 unit_quaternion_nonconst can be accessed from
+  /// ``Base``.
+  friend class Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>>;
+
+  // LCOV_EXCL_START
+  SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Map);
+  // LCOV_EXCL_STOP
+
+  using Base::operator*=;
+  using Base::operator*;
+
+  SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
+
+  /// Accessor of unit quaternion.
+  ///
+  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options> const& unit_quaternion()
+      const {
+    return unit_quaternion_;
+  }
+
+ protected:
+  /// Mutator of unit_quaternion is protected to ensure class invariant.
+  ///
+  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&
+  unit_quaternion_nonconst() {
+    return unit_quaternion_;
+  }
+
+  Map<Eigen::Quaternion<Scalar>, Options> unit_quaternion_;
+};
+
+/// Specialization of Eigen::Map for ``SO3 const``; derived from SO3Base.
+///
+/// Allows us to wrap SO3 objects around POD array (e.g. external c style
+/// quaternion).
+template <class Scalar_, int Options>
+class Map<Sophus::SO3<Scalar_> const, Options>
+    : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>> {
+ public:
+  using Base = Sophus::SO3Base<Map<Sophus::SO3<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) : unit_quaternion_(coeffs) {}
+
+  /// Accessor of unit quaternion.
+  ///
+  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar> const, Options> const&
+  unit_quaternion() const {
+    return unit_quaternion_;
+  }
+
+ protected:
+  /// Mutator of unit_quaternion is protected to ensure class invariant.
+  ///
+  Map<Eigen::Quaternion<Scalar> const, Options> const unit_quaternion_;
+};
+}  // namespace Eigen
+
+#endif

lio/include/sophus/test_macros.hpp

@@ -0,0 +1,129 @@
+#ifndef SOPUHS_TESTS_MACROS_HPP
+#define SOPUHS_TESTS_MACROS_HPP
+
+#include <sophus/types.hpp>
+#include <sophus/formatstring.hpp>
+
+namespace Sophus {
+namespace details {
+
+template <class Scalar>
+class Pretty {
+ public:
+  static std::string impl(Scalar s) { return FormatString("%", s); }
+};
+
+template <class Scalar, int M, int N>
+class Pretty<Eigen::Matrix<Scalar, M, N>> {
+ public:
+  static std::string impl(Matrix<Scalar, M, N> const& v) {
+    return FormatString("\n%\n", v);
+  }
+};
+
+template <class T>
+std::string pretty(T const& v) {
+  return Pretty<T>::impl(v);
+}
+
+template <class... Args>
+void testFailed(bool& passed, char const* func, char const* file, int line,
+                std::string const& msg) {
+  std::cerr << FormatString("Test failed in function %, file %, line %\n", func,
+                            file, line);
+  std::cerr << msg << "\n\n";
+  passed = false;
+}
+}  // namespace details
+
+void processTestResult(bool passed) {
+  if (!passed) {
+    // LCOV_EXCL_START
+    std::cerr << "failed!" << std::endl << std::endl;
+    exit(-1);
+    // LCOV_EXCL_STOP
+  }
+  std::cerr << "passed." << std::endl << std::endl;
+}
+}  // namespace Sophus
+
+#define SOPHUS_STRINGIFY(x) #x
+
+/// GenericTests whether condition is true.
+/// The in-out parameter passed will be set to false if test fails.
+#define SOPHUS_TEST(passed, condition, ...)                                    \
+  do {                                                                         \
+    if (!(condition)) {                                                        \
+      std::string msg = Sophus::details::FormatString(                         \
+          "condition ``%`` is false\n", SOPHUS_STRINGIFY(condition));          \
+      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
+      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
+                                  msg);                                        \
+    }                                                                          \
+  } while (false)
+
+/// GenericTests whether left is equal to right given a threshold.
+/// The in-out parameter passed will be set to false if test fails.
+#define SOPHUS_TEST_EQUAL(passed, left, right, ...)                            \
+  do {                                                                         \
+    if (left != right) {                                                       \
+      std::string msg = Sophus::details::FormatString(                         \
+          "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left),           \
+          Sophus::details::pretty(left), SOPHUS_STRINGIFY(right),              \
+          Sophus::details::pretty(right));                                     \
+      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
+      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
+                                  msg);                                        \
+    }                                                                          \
+  } while (false)
+
+/// GenericTests whether left is equal to right given a threshold.
+/// The in-out parameter passed will be set to false if test fails.
+#define SOPHUS_TEST_NEQ(passed, left, right, ...)                              \
+  do {                                                                         \
+    if (left == right) {                                                       \
+      std::string msg = Sophus::details::FormatString(                         \
+          "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left),    \
+          Sophus::details::pretty(left), SOPHUS_STRINGIFY(right),              \
+          Sophus::details::pretty(right));                                     \
+      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
+      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
+                                  msg);                                        \
+    }                                                                          \
+  } while (false)
+
+/// GenericTests whether left is approximatly equal to right given a threshold.
+/// The in-out parameter passed will be set to false if test fails.
+#define SOPHUS_TEST_APPROX(passed, left, right, thr, ...)                      \
+  do {                                                                         \
+    auto nrm = Sophus::maxMetric((left), (right));                             \
+    if (!(nrm < (thr))) {                                                      \
+      std::string msg = Sophus::details::FormatString(                         \
+          "% (=%) is not approx % (=%); % is %; nrm is %\n",                   \
+          SOPHUS_STRINGIFY(left), Sophus::details::pretty(left),               \
+          SOPHUS_STRINGIFY(right), Sophus::details::pretty(right),             \
+          SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm);           \
+      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
+      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
+                                  msg);                                        \
+    }                                                                          \
+  } while (false)
+
+/// GenericTests whether left is NOT approximatly equal to right given a
+/// threshold. The in-out parameter passed will be set to false if test fails.
+#define SOPHUS_TEST_NOT_APPROX(passed, left, right, thr, ...)                  \
+  do {                                                                         \
+    auto nrm = Sophus::maxMetric((left), (right));                             \
+    if (nrm < (thr)) {                                                         \
+      std::string msg = Sophus::details::FormatString(                         \
+          "% (=%) is approx % (=%), but it should not!\n % is %; nrm is %\n",  \
+          SOPHUS_STRINGIFY(left), Sophus::details::pretty(left),               \
+          SOPHUS_STRINGIFY(right), Sophus::details::pretty(right),             \
+          SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm);           \
+      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
+      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
+                                  msg);                                        \
+    }                                                                          \
+  } while (false)
+
+#endif  // SOPUHS_TESTS_MACROS_HPP

lio/include/sophus/types.hpp

@@ -0,0 +1,241 @@
+/// @file
+/// Common type aliases.
+
+#ifndef SOPHUS_TYPES_HPP
+#define SOPHUS_TYPES_HPP
+
+#include <type_traits>
+#include "common.hpp"
+
+namespace Sophus {
+
+template <class Scalar, int M, int Options = 0>
+using Vector = Eigen::Matrix<Scalar, M, 1, Options>;
+
+template <class Scalar, int Options = 0>
+using Vector2 = Vector<Scalar, 2, Options>;
+using Vector2f = Vector2<float>;
+using Vector2d = Vector2<double>;
+
+template <class Scalar, int Options = 0>
+using Vector3 = Vector<Scalar, 3, Options>;
+using Vector3f = Vector3<float>;
+using Vector3d = Vector3<double>;
+
+template <class Scalar>
+using Vector4 = Vector<Scalar, 4>;
+using Vector4f = Vector4<float>;
+using Vector4d = Vector4<double>;
+
+template <class Scalar>
+using Vector6 = Vector<Scalar, 6>;
+using Vector6f = Vector6<float>;
+using Vector6d = Vector6<double>;
+
+template <class Scalar>
+using Vector7 = Vector<Scalar, 7>;
+using Vector7f = Vector7<float>;
+using Vector7d = Vector7<double>;
+
+template <class Scalar, int M, int N>
+using Matrix = Eigen::Matrix<Scalar, M, N>;
+
+template <class Scalar>
+using Matrix2 = Matrix<Scalar, 2, 2>;
+using Matrix2f = Matrix2<float>;
+using Matrix2d = Matrix2<double>;
+
+template <class Scalar>
+using Matrix3 = Matrix<Scalar, 3, 3>;
+using Matrix3f = Matrix3<float>;
+using Matrix3d = Matrix3<double>;
+
+template <class Scalar>
+using Matrix4 = Matrix<Scalar, 4, 4>;
+using Matrix4f = Matrix4<float>;
+using Matrix4d = Matrix4<double>;
+
+template <class Scalar>
+using Matrix6 = Matrix<Scalar, 6, 6>;
+using Matrix6f = Matrix6<float>;
+using Matrix6d = Matrix6<double>;
+
+template <class Scalar>
+using Matrix7 = Matrix<Scalar, 7, 7>;
+using Matrix7f = Matrix7<float>;
+using Matrix7d = Matrix7<double>;
+
+template <class Scalar, int N, int Options = 0>
+using ParametrizedLine = Eigen::ParametrizedLine<Scalar, N, Options>;
+
+template <class Scalar, int Options = 0>
+using ParametrizedLine3 = ParametrizedLine<Scalar, 3, Options>;
+using ParametrizedLine3f = ParametrizedLine3<float>;
+using ParametrizedLine3d = ParametrizedLine3<double>;
+
+template <class Scalar, int Options = 0>
+using ParametrizedLine2 = ParametrizedLine<Scalar, 2, Options>;
+using ParametrizedLine2f = ParametrizedLine2<float>;
+using ParametrizedLine2d = ParametrizedLine2<double>;
+
+namespace details {
+template <class Scalar>
+class MaxMetric {
+ public:
+  static Scalar impl(Scalar s0, Scalar s1) {
+    using std::abs;
+    return abs(s0 - s1);
+  }
+};
+
+template <class Scalar, int M, int N>
+class MaxMetric<Matrix<Scalar, M, N>> {
+ public:
+  static Scalar impl(Matrix<Scalar, M, N> const& p0,
+                     Matrix<Scalar, M, N> const& p1) {
+    return (p0 - p1).template lpNorm<Eigen::Infinity>();
+  }
+};
+
+template <class Scalar>
+class SetToZero {
+ public:
+  static void impl(Scalar& s) { s = Scalar(0); }
+};
+
+template <class Scalar, int M, int N>
+class SetToZero<Matrix<Scalar, M, N>> {
+ public:
+  static void impl(Matrix<Scalar, M, N>& v) { v.setZero(); }
+};
+
+template <class T1, class Scalar>
+class SetElementAt;
+
+template <class Scalar>
+class SetElementAt<Scalar, Scalar> {
+ public:
+  static void impl(Scalar& s, Scalar value, int at) {
+    SOPHUS_ENSURE(at == 0, "is %", at);
+    s = value;
+  }
+};
+
+template <class Scalar, int N>
+class SetElementAt<Vector<Scalar, N>, Scalar> {
+ public:
+  static void impl(Vector<Scalar, N>& v, Scalar value, int at) {
+    SOPHUS_ENSURE(at >= 0 && at < N, "is %", at);
+    v[at] = value;
+  }
+};
+
+template <class Scalar>
+class SquaredNorm {
+ public:
+  static Scalar impl(Scalar const& s) { return s * s; }
+};
+
+template <class Scalar, int N>
+class SquaredNorm<Matrix<Scalar, N, 1>> {
+ public:
+  static Scalar impl(Matrix<Scalar, N, 1> const& s) { return s.squaredNorm(); }
+};
+
+template <class Scalar>
+class Transpose {
+ public:
+  static Scalar impl(Scalar const& s) { return s; }
+};
+
+template <class Scalar, int M, int N>
+class Transpose<Matrix<Scalar, M, N>> {
+ public:
+  static Matrix<Scalar, M, N> impl(Matrix<Scalar, M, N> const& s) {
+    return s.transpose();
+  }
+};
+}  // namespace details
+
+/// Returns maximum metric between two points ``p0`` and ``p1``, with ``p0, p1``
+/// being matrices or a scalars.
+///
+template <class T>
+auto maxMetric(T const& p0, T const& p1)
+    -> decltype(details::MaxMetric<T>::impl(p0, p1)) {
+  return details::MaxMetric<T>::impl(p0, p1);
+}
+
+/// Sets point ``p`` to zero, with ``p`` being a matrix or a scalar.
+///
+template <class T>
+void setToZero(T& p) {
+  return details::SetToZero<T>::impl(p);
+}
+
+/// Sets ``i``th component of ``p`` to ``value``, with ``p`` being a
+/// matrix or a scalar. If ``p`` is a scalar, ``i`` must be ``0``.
+///
+template <class T, class Scalar>
+void setElementAt(T& p, Scalar value, int i) {
+  return details::SetElementAt<T, Scalar>::impl(p, value, i);
+}
+
+/// Returns the squared 2-norm of ``p``, with ``p`` being a vector or a scalar.
+///
+template <class T>
+auto squaredNorm(T const& p) -> decltype(details::SquaredNorm<T>::impl(p)) {
+  return details::SquaredNorm<T>::impl(p);
+}
+
+/// Returns ``p.transpose()`` if ``p`` is a matrix, and simply ``p`` if m is a
+/// scalar.
+///
+template <class T>
+auto transpose(T const& p) -> decltype(details::Transpose<T>::impl(T())) {
+  return details::Transpose<T>::impl(p);
+}
+
+template <class Scalar>
+struct IsFloatingPoint {
+  static bool const value = std::is_floating_point<Scalar>::value;
+};
+
+template <class Scalar, int M, int N>
+struct IsFloatingPoint<Matrix<Scalar, M, N>> {
+  static bool const value = std::is_floating_point<Scalar>::value;
+};
+
+template <class Scalar_>
+struct GetScalar {
+  using Scalar = Scalar_;
+};
+
+template <class Scalar_, int M, int N>
+struct GetScalar<Matrix<Scalar_, M, N>> {
+  using Scalar = Scalar_;
+};
+
+/// If the Vector type is of fixed size, then IsFixedSizeVector::value will be
+/// true.
+template <typename Vector, int NumDimensions,
+          typename = typename std::enable_if<
+              Vector::RowsAtCompileTime == NumDimensions &&
+              Vector::ColsAtCompileTime == 1>::type>
+struct IsFixedSizeVector : std::true_type {};
+
+/// Planes in 3d are hyperplanes.
+template <class T>
+using Plane3 = Eigen::Hyperplane<T, 3>;
+using Plane3d = Plane3<double>;
+using Plane3f = Plane3<float>;
+
+/// Lines in 2d are hyperplanes.
+template <class T>
+using Line2 = Eigen::Hyperplane<T, 2>;
+using Line2d = Line2<double>;
+using Line2f = Line2<float>;
+
+}  // namespace Sophus
+
+#endif  // SOPHUS_TYPES_HPP

lio/include/sophus/velocities.hpp

@@ -0,0 +1,74 @@
+#ifndef SOPHUS_VELOCITIES_HPP
+#define SOPHUS_VELOCITIES_HPP
+
+#include <functional>
+
+#include "num_diff.hpp"
+#include "se3.hpp"
+
+namespace Sophus {
+namespace experimental {
+// Experimental since the API will certainly change drastically in the future.
+
+// Transforms velocity vector by rotation ``foo_R_bar``.
+//
+// Note: vel_bar can be either a linear or a rotational velocity vector.
+//
+template <class Scalar>
+Vector3<Scalar> transformVelocity(SO3<Scalar> const& foo_R_bar,
+                                  Vector3<Scalar> const& vel_bar) {
+  // For rotational velocities note that:
+  //
+  //   vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T)
+  //           = vee(hat(Adj(foo_R_bar) * vel_bar))
+  //           = Adj(foo_R_bar) * vel_bar
+  //           = foo_R_bar * vel_bar.
+  //
+  return foo_R_bar * vel_bar;
+}
+
+// Transforms velocity vector by pose ``foo_T_bar``.
+//
+// Note: vel_bar can be either a linear or a rotational velocity vector.
+//
+template <class Scalar>
+Vector3<Scalar> transformVelocity(SE3<Scalar> const& foo_T_bar,
+                                  Vector3<Scalar> const& vel_bar) {
+  return transformVelocity(foo_T_bar.so3(), vel_bar);
+}
+
+// finite difference approximation of instantanious velocity in frame foo
+//
+template <class Scalar>
+Vector3<Scalar> finiteDifferenceRotationalVelocity(
+    std::function<SO3<Scalar>(Scalar)> const& foo_R_bar, Scalar t,
+    Scalar h = Constants<Scalar>::epsilon()) {
+  // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor
+  //
+  // W = dR(t)/dt * R^{-1}(t)
+  Matrix3<Scalar> dR_dt_in_frame_foo = curveNumDiff(
+      [&foo_R_bar](Scalar t0) -> Matrix3<Scalar> {
+        return foo_R_bar(t0).matrix();
+      },
+      t, h);
+  // velocity tensor
+  Matrix3<Scalar> W_in_frame_foo =
+      dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix();
+  return SO3<Scalar>::vee(W_in_frame_foo);
+}
+
+// finite difference approximation of instantanious velocity in frame foo
+//
+template <class Scalar>
+Vector3<Scalar> finiteDifferenceRotationalVelocity(
+    std::function<SE3<Scalar>(Scalar)> const& foo_T_bar, Scalar t,
+    Scalar h = Constants<Scalar>::epsilon()) {
+  return finiteDifferenceRotationalVelocity<Scalar>(
+      [&foo_T_bar](Scalar t) -> SO3<Scalar> { return foo_T_bar(t).so3(); }, t,
+      h);
+}
+
+}  // namespace experimental
+}  // namespace Sophus
+
+#endif  // SOPHUS_VELOCITIES_HPP

lio/include/utils/math_utils.hpp

@@ -0,0 +1,206 @@
+/*
+ * COPYRIGHT AND PERMISSION NOTICE
+ * Penn Software MSCKF_VIO
+ * Copyright (C) 2017 The Trustees of the University of Pennsylvania
+ * All rights reserved.
+ */
+
+// The original file belongs to MSCKF_VIO (https://github.com/KumarRobotics/msckf_vio/)
+// Some changes have been made to use it in livox_slam_ware
+
+#ifndef MATH_UTILS_HPP
+#define MATH_UTILS_HPP
+
+#include <cmath>
+#include <Eigen/Dense>
+
+namespace livox_slam_ware {
+
+/*
+ *  @brief Create a skew-symmetric matrix from a 3-element vector.
+ *  @note Performs the operation:
+ *  w   ->  [  0 -w3  w2]
+ *          [ w3   0 -w1]
+ *          [-w2  w1   0]
+ */
+inline Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d& w) {
+  Eigen::Matrix3d w_hat;
+  w_hat(0, 0) = 0;
+  w_hat(0, 1) = -w(2);
+  w_hat(0, 2) = w(1);
+  w_hat(1, 0) = w(2);
+  w_hat(1, 1) = 0;
+  w_hat(1, 2) = -w(0);
+  w_hat(2, 0) = -w(1);
+  w_hat(2, 1) = w(0);
+  w_hat(2, 2) = 0;
+  return w_hat;
+}
+
+/*
+ * @brief Normalize the given quaternion to unit quaternion.
+ */
+inline void quaternionNormalize(Eigen::Vector4d& q) {
+  double norm = q.norm();
+  q = q / norm;
+  return;
+}
+
+/*
+ * @brief Perform q1 * q2.
+ *  
+ *    Format of q1 and q2 is as [x,y,z,w]
+ */
+inline Eigen::Vector4d quaternionMultiplication(
+    const Eigen::Vector4d& q1,
+    const Eigen::Vector4d& q2) {
+  Eigen::Matrix4d L;
+
+  // QXC: Hamilton
+  L(0, 0) =  q1(3); L(0, 1) = -q1(2); L(0, 2) =  q1(1); L(0, 3) =  q1(0);
+  L(1, 0) =  q1(2); L(1, 1) =  q1(3); L(1, 2) = -q1(0); L(1, 3) =  q1(1);
+  L(2, 0) = -q1(1); L(2, 1) =  q1(0); L(2, 2) =  q1(3); L(2, 3) =  q1(2);
+  L(3, 0) = -q1(0); L(3, 1) = -q1(1); L(3, 2) = -q1(2); L(3, 3) =  q1(3);
+
+  Eigen::Vector4d q = L * q2;
+  quaternionNormalize(q);
+  return q;
+}
+
+/*
+ * @brief Convert the vector part of a quaternion to a
+ *    full quaternion.
+ * @note This function is useful to convert delta quaternion
+ *    which is usually a 3x1 vector to a full quaternion.
+ *    For more details, check Section 3.2 "Kalman Filter Update" in
+ *    "Indirect Kalman Filter for 3D Attitude Estimation:
+ *    A Tutorial for quaternion Algebra".
+ */
+inline Eigen::Vector4d smallAngleQuaternion(
+    const Eigen::Vector3d& dtheta) {
+
+  Eigen::Vector3d dq = dtheta / 2.0;
+  Eigen::Vector4d q;
+  double dq_square_norm = dq.squaredNorm();
+
+  if (dq_square_norm <= 1) {
+    q.head<3>() = dq;
+    q(3) = std::sqrt(1-dq_square_norm);
+  } else {
+    q.head<3>() = dq;
+    q(3) = 1;
+    q = q / std::sqrt(1+dq_square_norm);
+  }
+
+  return q;
+}
+
+/*
+ * @brief Convert the vector part of a quaternion to a
+ *    full quaternion.
+ * @note This function is useful to convert delta quaternion
+ *    which is usually a 3x1 vector to a full quaternion.
+ *    For more details, check Section 3.2 "Kalman Filter Update" in
+ *    "Indirect Kalman Filter for 3D Attitude Estimation:
+ *    A Tutorial for quaternion Algebra".
+ */
+inline Eigen::Quaterniond getSmallAngleQuaternion(
+    const Eigen::Vector3d& dtheta) {
+
+  Eigen::Vector3d dq = dtheta / 2.0;
+  Eigen::Quaterniond q;
+  double dq_square_norm = dq.squaredNorm();
+
+  if (dq_square_norm <= 1) {
+    q.x() = dq(0);
+    q.y() = dq(1);
+    q.z() = dq(2);
+    q.w() = std::sqrt(1-dq_square_norm);
+  } else {
+    q.x() = dq(0);
+    q.y() = dq(1);
+    q.z() = dq(2);
+    q.w() = 1;
+    q.normalize();
+  }
+
+  return q;
+}
+
+/*
+ * @brief Convert a quaternion to the corresponding rotation matrix
+ * @note Pay attention to the convention used. The function follows the
+ *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
+ *    A Tutorial for Quaternion Algebra", Equation (78).
+ *
+ *    The input quaternion should be in the form
+ *      [q1, q2, q3, q4(scalar)]^T
+ */
+inline Eigen::Matrix3d quaternionToRotation(
+    const Eigen::Vector4d& q) {
+  // QXC: Hamilton
+  const double& qw = q(3);
+  const double& qx = q(0);
+  const double& qy = q(1);
+  const double& qz = q(2);
+  Eigen::Matrix3d R;
+  R(0, 0) = 1-2*(qy*qy+qz*qz);  R(0, 1) =   2*(qx*qy-qw*qz);  R(0, 2) =   2*(qx*qz+qw*qy);
+  R(1, 0) =   2*(qx*qy+qw*qz);  R(1, 1) = 1-2*(qx*qx+qz*qz);  R(1, 2) =   2*(qy*qz-qw*qx);
+  R(2, 0) =   2*(qx*qz-qw*qy);  R(2, 1) =   2*(qy*qz+qw*qx);  R(2, 2) = 1-2*(qx*qx+qy*qy);
+
+  return R;
+}
+
+/*
+ * @brief Convert a rotation matrix to a quaternion.
+ * @note Pay attention to the convention used. The function follows the
+ *    conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
+ *    A Tutorial for Quaternion Algebra", Equation (78).
+ *
+ *    The input quaternion should be in the form
+ *      [q1, q2, q3, q4(scalar)]^T
+ */
+inline Eigen::Vector4d rotationToQuaternion(
+    const Eigen::Matrix3d& R) {
+  Eigen::Vector4d score;
+  score(0) = R(0, 0);
+  score(1) = R(1, 1);
+  score(2) = R(2, 2);
+  score(3) = R.trace();
+
+  int max_row = 0, max_col = 0;
+  score.maxCoeff(&max_row, &max_col);
+
+  Eigen::Vector4d q = Eigen::Vector4d::Zero();
+
+  // QXC: Hamilton
+  if (max_row == 0) {
+    q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;
+    q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));
+    q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));
+    q(3) = (R(2, 1)-R(1, 2)) / (4*q(0));
+  } else if (max_row == 1) {
+    q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;
+    q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));
+    q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));
+    q(3) = (R(0, 2)-R(2, 0)) / (4*q(1));
+  } else if (max_row == 2) {
+    q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;
+    q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));
+    q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));
+    q(3) = (R(1, 0)-R(0, 1)) / (4*q(2));
+  } else {
+    q(3) = std::sqrt(1+R.trace()) / 2.0;
+    q(0) = (R(2, 1)-R(1, 2)) / (4*q(3));
+    q(1) = (R(0, 2)-R(2, 0)) / (4*q(3));
+    q(2) = (R(1, 0)-R(0, 1)) / (4*q(3));
+  }
+
+  if (q(3) < 0) q = -q;
+  quaternionNormalize(q);
+  return q;
+}
+
+} // end namespace livox_slam_ware
+
+#endif // MATH_UTILS_HPP

lio/src/PoseEstimation.cpp

@@ -0,0 +1,472 @@
+
+
+#include <csignal>
+
+#include "segment/LidarFeatureExtractor.h"
+#include "lio/Estimator.h"
+#include "lio/mapper.h"
+
+#include "lddc.h"
+#include "lds_hub.h"
+#include "lds_lidar.h"
+#include "lds_lvx.h"
+#include "livox_sdk.h"
+
+#include <pangolin/var/var.h>
+#include <pangolin/var/varextra.h>
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/view.h>
+#include <pangolin/display/widgets.h>
+#include <pangolin/display/default_font.h>
+#include <pangolin/handler/handler.h>
+
+
+
+LidarFeatureExtractor* lidarFeatureExtractor;
+pcl::PointCloud<PointType>::Ptr laserCloud;
+pcl::PointCloud<PointType>::Ptr laserConerCloud;
+pcl::PointCloud<PointType>::Ptr laserSurfCloud;
+pcl::PointCloud<PointType>::Ptr laserNonFeatureCloud;
+
+int Lidar_Type=0;
+int N_SCANS = 6;
+bool Feature_Mode = false;
+bool Use_seg = false;
+
+
+Mapper* pMaper= nullptr;
+
+
+bool exit_app=false;
+void SigHandle(int sig) {
+    exit_app=true;
+    printf("catch sig %d\n", sig);
+}
+
+class LockCloud
+{
+ public:
+    LockCloud()
+    {
+        cloud_.reset(new pcl::PointCloud<PointType>);
+    }
+    void Lock(){
+        lock_.lock();
+    }
+    void unlock(){
+        lock_.unlock();
+    }
+    pcl::PointCloud<PointType>::Ptr Get()
+    {
+        return cloud_;
+    }
+    void Set(pcl::PointCloud<PointType>::Ptr cloud)
+    {
+        lock_.lock();
+        cloud_=cloud;
+        lock_.unlock();
+    }
+
+ protected:
+    std::mutex lock_;
+    pcl::PointCloud<PointType>::Ptr cloud_;
+};
+
+
+LockCloud scan_cloud;
+LockCloud localmap_cloud;
+Eigen::Matrix4d cur_pose;
+double pose_line_length=1.0;
+
+
+
+void CloudFullCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
+{
+    scan_cloud.Set(cloud);
+}
+
+void CloudLocalCallback(pcl::PointCloud<PointType>::Ptr cloud,double timebase)
+{
+    localmap_cloud.Set(cloud);
+}
+
+
+void pubOdometry(const Eigen::Matrix4d& newPose, double& timefullCloud)
+{
+
+}
+
+
+void OdometryCallback(Eigen::Matrix4d tranform,double timebase)
+{
+    cur_pose=tranform;
+    //odom_pt=tranform.topRightCorner(3, 1);
+    pubOdometry(tranform, timebase);
+}
+/*
+ImuData msg2Imudata(sensor_msgs::ImuConstPtr msg)
+{
+    ImuData  imu;
+    imu.timebase=msg->header.stamp.toSec();
+    imu.angular_velocity.x=msg->angular_velocity.x;
+    imu.angular_velocity.y=msg->angular_velocity.y;
+    imu.angular_velocity.z=msg->angular_velocity.z;
+    imu.linear_acceleration.x=msg->linear_acceleration.x;
+    imu.linear_acceleration.y=msg->linear_acceleration.y;
+    imu.linear_acceleration.z=msg->linear_acceleration.z;
+    return imu;
+}
+*/
+
+void CloudCallback(TimeCloud tc,int handle)
+{
+    double timeSpan =tc.cloud->points.back().normal_x;
+    laserCloud.reset(new pcl::PointCloud<PointType>);
+    laserCloud->reserve(15000*N_SCANS);
+
+    PointType point;
+    for(const auto& p : tc.cloud->points)
+    {
+
+
+        if (point.normal_y > N_SCANS - 1) continue;
+        if (p.x < 0.01) continue;
+        if (!pcl_isfinite(p.x) ||
+                !pcl_isfinite(p.y) ||
+                !pcl_isfinite(p.z))
+        {
+            continue;
+        }
+        point.normal_x = point.normal_x / timeSpan; //normao_x  time
+        laserCloud->push_back(point);
+    }
+
+
+    if(Use_seg){
+        lidarFeatureExtractor->FeatureExtract_with_segment( laserCloud, laserConerCloud, laserSurfCloud, laserNonFeatureCloud,N_SCANS);
+    }
+    else{
+        lidarFeatureExtractor->FeatureExtract(laserCloud, laserConerCloud, laserSurfCloud,N_SCANS);
+    }
+
+    tc.cloud=laserCloud;
+    pMaper->AddCloud(tc);
+
+}
+
+
+void ImuDataCallback(ImuData imu,int handle){
+  pMaper->AddImu(imu);
+}
+
+void DrawPose(Eigen::Matrix4d pose)
+{
+  double l=pose_line_length<0.1?0.1:pose_line_length;
+  Eigen::Vector3d Ow=pose.topRightCorner(3, 1);
+  Eigen::Vector3d Xw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(1,0,0))+Ow;
+  Eigen::Vector3d Yw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,1,0))+Ow;
+  Eigen::Vector3d Zw=pose.topLeftCorner(3,3)*(l*Eigen::Vector3d(0,0,1))+Ow;
+
+  glBegin(GL_LINES);
+  glColor3f(1.0, 0.0, 0.0);
+  glVertex3d(Ow[0], Ow[1], Ow[2]);
+  glVertex3d(Xw[0], Xw[1], Xw[2]);
+  glColor3f(0.0, 1.0, 0.0);
+  glVertex3d(Ow[0], Ow[1], Ow[2]);
+  glVertex3d(Yw[0], Yw[1], Yw[2]);
+  glColor3f(0.0, 0.0, 1.0);
+  glVertex3d(Ow[0], Ow[1], Ow[2]);
+  glVertex3d(Zw[0], Zw[1], Zw[2]);
+  glEnd();
+
+}
+
+int main(int argc, char** argv)
+{
+
+  //reg参数
+  std::string config_file="../config/horizon_config.yaml";
+
+  cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
+  if (!fsSettings.isOpened())
+  {
+    std::cout << "config_file error: cannot open " << config_file << std::endl;
+    return false;
+  }
+  Lidar_Type = static_cast<int>(fsSettings["Lidar_Type"]);
+  N_SCANS = static_cast<int>(fsSettings["Used_Line"]);
+  Feature_Mode = static_cast<int>(fsSettings["Feature_Mode"]);
+  Use_seg = static_cast<int>(fsSettings["Use_seg"]);
+
+  int NumCurvSize = static_cast<int>(fsSettings["NumCurvSize"]);
+  float DistanceFaraway = static_cast<float>(fsSettings["DistanceFaraway"]);
+  int NumFlat = static_cast<int>(fsSettings["NumFlat"]);
+  int PartNum = static_cast<int>(fsSettings["PartNum"]);
+  float FlatThreshold = static_cast<float>(fsSettings["FlatThreshold"]);
+  float BreakCornerDis = static_cast<float>(fsSettings["BreakCornerDis"]);
+  float LidarNearestDis = static_cast<float>(fsSettings["LidarNearestDis"]);
+  float KdTreeCornerOutlierDis = static_cast<float>(fsSettings["KdTreeCornerOutlierDis"]);
+
+  int ivox_nearby_type = static_cast<int>(fsSettings["ivox_nearby_type"]);
+  int max_ivox_node = static_cast<int>(fsSettings["max_ivox_node"]);
+  float ivox_resolution = static_cast<float>(fsSettings["ivox_node_solution"]);
+
+
+  laserCloud.reset(new pcl::PointCloud<PointType>);
+  laserConerCloud.reset(new pcl::PointCloud<PointType>);
+  laserSurfCloud.reset(new pcl::PointCloud<PointType>);
+  laserNonFeatureCloud.reset(new pcl::PointCloud<PointType>);
+
+
+  lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,
+                                                    NumCurvSize,
+                                                    DistanceFaraway,
+                                                    NumFlat,
+                                                    PartNum,
+                                                    FlatThreshold,
+                                                    BreakCornerDis,
+                                                    LidarNearestDis,
+                                                    KdTreeCornerOutlierDis);
+
+  //map 参数
+  std::string map_dir="../map/";
+  float filter_parameter_corner = 0.2;
+  float filter_parameter_surf = 0.4;
+  int IMU_Mode = 2;
+
+
+  double vecTlb[]={1.0, 0.0, 0.0, -0.05512,
+                    0.0, 1.0, 0.0, -0.02226,
+                    0.0, 0.0, 1.0,  0.0297,
+                    0.0, 0.0, 0.0,  1.0};
+
+
+  // set extrinsic matrix between lidar & IMU
+  Eigen::Matrix3d R;
+  Eigen::Vector3d t;
+  R << vecTlb[0], vecTlb[1], vecTlb[2],
+          vecTlb[4], vecTlb[5], vecTlb[6],
+          vecTlb[8], vecTlb[9], vecTlb[10];
+  t << vecTlb[3], vecTlb[7], vecTlb[11];
+  Eigen::Quaterniond qr(R);
+  R = qr.normalized().toRotationMatrix();
+
+
+
+
+  int WINDOWSIZE = 0;
+  if (IMU_Mode < 2)
+    WINDOWSIZE = 1;
+  else
+    WINDOWSIZE = 20;
+
+  pMaper = new Mapper(WINDOWSIZE, filter_parameter_corner, filter_parameter_surf,
+                      ivox_nearby_type, max_ivox_node, ivox_resolution);
+  pMaper->InitRT(R, t);
+  pMaper->SetOdomCallback(OdometryCallback);
+  pMaper->SetCloudMappedCallback(CloudFullCallback);
+  pMaper->SetLocalMapCloudCallback(CloudLocalCallback);
+  pMaper->LoadMap(map_dir + "/feature.txt");
+
+  double RPI = M_PI / 180.0;
+  Eigen::Vector3d eulerAngle(0 * RPI, 0 * RPI, 5.0 * RPI);
+  Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
+  Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
+  Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));
+
+  Eigen::Matrix3d rotation_matrix;
+  rotation_matrix = yawAngle * pitchAngle * rollAngle;
+  Eigen::Matrix4d transform;
+  transform.topLeftCorner(3, 3) = rotation_matrix;
+  transform.topRightCorner(3, 1) = Eigen::Vector3d(0.1, 0.1, 0);
+  pMaper->SetInitPose(transform);
+
+  printf("  window size :%d\n--------------", WINDOWSIZE);
+
+  signal(SIGINT, SigHandle);
+
+  pangolin::CreateWindowAndBind("Main", 1280, 960);
+
+  // 3D Mouse handler requires depth testing to be enabled
+  glEnable(GL_DEPTH_TEST);
+
+  // Define Camera Render Object (for view / scene browsing)
+  pangolin::OpenGlRenderState s_cam(
+          pangolin::ProjectionMatrix(1280, 960, 840, 840, 640, 480, 0.1, 1000),
+          pangolin::ModelViewLookAt(25, 25, 70, 25, 25, 5, pangolin::AxisY)
+  );
+
+  // Choose a sensible left UI Panel width based on the width of 20
+  // charectors from the default font.
+  const int UI_WIDTH = 20 * pangolin::default_font().MaxWidth();
+
+  // Add named OpenGL viewport to window and provide 3D Handler
+  pangolin::View &d_cam = pangolin::CreateDisplay()
+          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, 1280.0f / 960.0f)
+          .SetHandler(new pangolin::Handler3D(s_cam));
+
+  // Add named Panel and bind to variables beginning 'ui'
+  // A Panel is just a View with a default layout and input handling
+  pangolin::CreatePanel("ui")
+          .SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));
+
+  // Safe and efficient binding of named variables.
+  // Specialisations mean no conversions take place for exact types
+  // and conversions between scalar types are cheap.
+  pangolin::Var<bool> start_button("ui.开始_Button", false, false);
+  pangolin::Var<bool> stop_button("ui.停止_Button", false, false);
+  pangolin::Var<double> a_double("ui.A_Double", 3, 0, 5);
+  pangolin::Var<int> an_int("ui.An_Int", 2, 0, 5);
+  pangolin::Var<double> a_double_log("ui.Log_scale", 3, 1, 1E4, true);
+  pangolin::Var<bool> a_checkbox("ui.A_Checkbox", false, true);
+  pangolin::Var<int> an_int_no_input("ui.An_Int_No_Input", 2);
+  pangolin::Var<std::string> a_string("ui.A_String", "Edit ME!");
+
+  // std::function objects can be used for Var's too. These work great with C++11 closures.
+  pangolin::Var<std::function<void(void)>> save_window("ui.Save_Window", []() {
+    pangolin::SaveWindowOnRender("window");
+  });
+
+  pangolin::Var<std::function<void(void)>> save_cube_view("ui.Save_Cube", [&d_cam]() {
+    pangolin::SaveWindowOnRender("cube", d_cam.v);
+  });
+
+  // Demonstration of how we can register a keyboard hook to alter a Var
+  pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'b', [&]() {
+    //a_double = 3.5;
+
+  });
+
+  // Default hooks for exiting (Esc) and fullscreen (tab).
+
+  glEnable(GL_BLEND);   // 启用混合
+  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+
+  //初始化雷达
+
+  /** Check sdk version */
+  LivoxSdkVersion _sdkversion;
+  GetLivoxSdkVersion(&_sdkversion);
+  const int32_t kSdkVersionMajorLimit = 2;
+  if (_sdkversion.major < kSdkVersionMajorLimit) {
+    printf("The SDK version[%d.%d.%d] is too low\n", _sdkversion.major,
+           _sdkversion.minor, _sdkversion.patch);
+    return 0;
+  }
+
+  double publish_freq  = 10.0; /* Hz */
+  if (publish_freq > 100.0) {
+    publish_freq = 100.0;
+  } else if (publish_freq < 0.1) {
+    publish_freq = 0.1;
+  } else {
+    publish_freq = publish_freq;
+  }
+  livox_ros::Lddc *lddc = new livox_ros::Lddc(publish_freq);
+
+  lddc->SetCloudCallback(CloudCallback);
+  lddc->SetImuCallback(ImuDataCallback);
+
+  double zoom = 1.0;
+  while (!pangolin::ShouldQuit())
+  {
+    // Clear entire screen
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+    if (pangolin::Pushed(start_button))
+      lddc->Start();
+    if (pangolin::Pushed(stop_button))
+      lddc->Stop();
+
+    if (a_double.GuiChanged())
+    {
+      pose_line_length=a_double.Get();
+    }
+    char windows_size_str[64]={0};
+    sprintf(windows_size_str,"window_size:%d",pMaper->WINDOWSIZE);
+    a_string=windows_size_str;
+
+    // Overloading of Var<T> operators allows us to treat them like
+    // their wrapped types, eg:
+    if (a_checkbox)
+      an_int = (int) a_double;
+
+    an_int_no_input = an_int;
+
+    if (d_cam.IsShown())
+    {
+      // Activate efficiently by object
+      //s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(odom_pt[0],odom_pt[1],60, odom_pt[0],odom_pt[1],0, pangolin::AxisY));
+
+      d_cam.Activate(s_cam);
+      pangolin::glDrawAxis(3);//三维坐标轴，红——x轴，绿——y轴，蓝——z轴
+
+      DrawPose(cur_pose);
+
+      glBegin(GL_POINTS);
+      glColor3f(1, 1, 1);
+      glPointSize(0.5);
+      scan_cloud.Lock();
+      for (int i = 0; i < scan_cloud.Get()->size(); ++i)
+      {
+        PointType pt = scan_cloud.Get()->points[i];
+        glVertex3d(pt.x * zoom, pt.y * zoom, pt.z * zoom);
+      }
+      scan_cloud.unlock();
+
+      glColor3f(1, 0, 0);
+
+
+      localmap_cloud.Lock();
+      for (int i = 0; i < localmap_cloud.Get()->size(); ++i)
+      {
+        PointType pt = localmap_cloud.Get()->points[i];
+        glPointSize(3);
+        glVertex3d(pt.x * zoom, pt.y * zoom, pt.z * zoom);
+      }
+      localmap_cloud.unlock();
+
+      glPointSize(0.3);
+      glColor4f(0, 1, 1, 0.2);
+      for (int i = 0; i < pMaper->GetMapCorner()->size(); ++i)
+      {
+        PointType pt = pMaper->GetMapCorner()->points[i];
+        glVertex3d(pt.x * zoom, pt.y * zoom, pt.z * zoom);
+
+      }
+
+      glColor4f(0.1, 1, 0, 0.2);
+      for (int i = 0; i < pMaper->GetMapSurf()->size(); ++i)
+      {
+        PointType pt = pMaper->GetMapSurf()->points[i];
+        glVertex3d(pt.x * zoom, pt.y * zoom, pt.z * zoom);
+
+      }
+
+      glColor4f(0.1, 0, 1, 0.2);
+      for (int i = 0; i < pMaper->GetMapNonf()->size(); ++i)
+      {
+        PointType pt = pMaper->GetMapNonf()->points[i];
+        glVertex3d(pt.x * zoom, pt.y * zoom, pt.z * zoom);
+
+      }
+
+      glEnd();
+
+    }
+
+    // Swap frames and Process Events
+    pangolin::FinishFrame();
+    std::this_thread::sleep_for(std::chrono::microseconds(10));
+  }
+
+  lddc->Stop();
+
+  pMaper->completed();
+  delete pMaper;
+  return 0;
+}
+

lio/src/ScanRegistration.cpp

@@ -0,0 +1,117 @@
+#include "segment/LidarFeatureExtractor.h"
+#include <ros/ros.h>
+#include <livox_ros_driver/CustomMsg.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+ros::Publisher pubFullLaserCloud;
+ros::Publisher pubSharpCloud;
+ros::Publisher pubFlatCloud;
+ros::Publisher pubNonFeature;
+
+LidarFeatureExtractor* lidarFeatureExtractor;
+pcl::PointCloud<PointType>::Ptr laserCloud;
+pcl::PointCloud<PointType>::Ptr laserConerCloud;
+pcl::PointCloud<PointType>::Ptr laserSurfCloud;
+pcl::PointCloud<PointType>::Ptr laserNonFeatureCloud;
+int Lidar_Type = 0;
+int N_SCANS = 6;
+bool Feature_Mode = false;
+bool Use_seg = false;
+
+
+
+
+void lidarCallBackHorizon(const livox_ros_driver::CustomMsgConstPtr &msg)
+{
+
+    double timeSpan = ros::Time().fromNSec(msg->points.back().offset_time).toSec();
+    laserCloud.reset(new pcl::PointCloud<PointType>);
+    laserCloud->reserve(15000*N_SCANS);
+    PointType point;
+    for(const auto& p : msg->points)
+    {
+
+        int line_num = (int) p.line;
+        if (line_num > N_SCANS - 1) continue;
+        if (p.x < 0.01) continue;
+        if (!pcl_isfinite(p.x) ||
+                !pcl_isfinite(p.y) ||
+                !pcl_isfinite(p.z))
+        {
+            continue;
+        }
+        point.x = p.x;
+        point.y = p.y;
+        point.z = p.z;
+        point.intensity = p.reflectivity;
+        point.normal_x = ros::Time().fromNSec(p.offset_time).toSec() / timeSpan; //normao_x  time
+        point.normal_y = LidarFeatureExtractor::_int_as_float(line_num);                               //normal_y  line_num
+        laserCloud->push_back(point);
+    }
+
+
+  if(Use_seg){
+    lidarFeatureExtractor->FeatureExtract_with_segment( laserCloud, laserConerCloud, laserSurfCloud, laserNonFeatureCloud,N_SCANS);
+  }
+  else{
+    lidarFeatureExtractor->FeatureExtract(laserCloud, laserConerCloud, laserSurfCloud,N_SCANS);
+  } 
+
+  sensor_msgs::PointCloud2 laserCloudMsg;
+  pcl::toROSMsg(*laserCloud, laserCloudMsg);
+  laserCloudMsg.header = msg->header;
+  laserCloudMsg.header.stamp.fromNSec(msg->timebase+msg->points.back().offset_time);
+  pubFullLaserCloud.publish(laserCloudMsg);
+
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "ScanRegistration");
+  ros::NodeHandle nodeHandler("~");
+
+  ros::Subscriber customCloud;;
+
+  std::string config_file;
+  nodeHandler.getParam("config_file", config_file);
+
+  cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
+  if (!fsSettings.isOpened()) {
+    std::cout << "config_file error: cannot open " << config_file << std::endl;
+    return false;
+  }
+  Lidar_Type = static_cast<int>(fsSettings["Lidar_Type"]);
+  N_SCANS = static_cast<int>(fsSettings["Used_Line"]);
+  Feature_Mode = static_cast<int>(fsSettings["Feature_Mode"]);
+  Use_seg = static_cast<int>(fsSettings["Use_seg"]);
+
+  int NumCurvSize = static_cast<int>(fsSettings["NumCurvSize"]);
+  float DistanceFaraway = static_cast<float>(fsSettings["DistanceFaraway"]);
+  int NumFlat = static_cast<int>(fsSettings["NumFlat"]);
+  int PartNum = static_cast<int>(fsSettings["PartNum"]);
+  float FlatThreshold = static_cast<float>(fsSettings["FlatThreshold"]);
+  float BreakCornerDis = static_cast<float>(fsSettings["BreakCornerDis"]);
+  float LidarNearestDis = static_cast<float>(fsSettings["LidarNearestDis"]);
+  float KdTreeCornerOutlierDis = static_cast<float>(fsSettings["KdTreeCornerOutlierDis"]);
+
+  laserCloud.reset(new pcl::PointCloud<PointType>);
+  laserConerCloud.reset(new pcl::PointCloud<PointType>);
+  laserSurfCloud.reset(new pcl::PointCloud<PointType>);
+  laserNonFeatureCloud.reset(new pcl::PointCloud<PointType>);
+
+  customCloud = nodeHandler.subscribe<livox_ros_driver::CustomMsg>("/livox/lidar", 100, &lidarCallBackHorizon);
+
+  pubFullLaserCloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_full_cloud", 10);
+  pubSharpCloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_less_sharp_cloud", 10);
+  pubFlatCloud = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_less_flat_cloud", 10);
+  pubNonFeature = nodeHandler.advertise<sensor_msgs::PointCloud2>("/livox_nonfeature_cloud", 10);
+
+  lidarFeatureExtractor = new LidarFeatureExtractor(N_SCANS,NumCurvSize,DistanceFaraway,NumFlat,PartNum,
+                                                    FlatThreshold,BreakCornerDis,LidarNearestDis,KdTreeCornerOutlierDis);
+
+  ros::spin();
+
+  return 0;
+}
+

lio/src/lio/Estimator.h

@@ -0,0 +1,356 @@
+#ifndef LIO_LIVOX_ESTIMATOR_H
+#define LIO_LIVOX_ESTIMATOR_H
+
+#include <Eigen/Core>
+#include <queue>
+#include <iterator>
+#include <future>
+#include "ceresfunc.h"
+#include "IMUIntegrator.h"
+#include <chrono>
+#include <fstream>
+#include "LocalMapIvox.h"
+
+class Estimator{
+	typedef pcl::PointXYZINormal PointType;
+
+    class Local_map
+    {
+     public:
+        Local_map()
+        {
+            laserCloudCornerFromLocal.reset(new pcl::PointCloud<PointType>);
+            laserCloudSurfFromLocal.reset(new pcl::PointCloud<PointType>);
+            laserCloudNonFeatureFromLocal.reset(new pcl::PointCloud<PointType>);
+            kdtreeCornerFromLocal.reset(new pcl::KdTreeFLANN<PointType>);
+            kdtreeSurfFromLocal.reset(new pcl::KdTreeFLANN<PointType>);
+            kdtreeNonFeatureFromLocal.reset(new pcl::KdTreeFLANN<PointType>);
+        }
+        void reset(pcl::PointCloud<PointType>::Ptr corner,pcl::PointCloud<PointType>::Ptr surf,
+                   pcl::PointCloud<PointType>::Ptr nonf)
+        {
+            laserCloudCornerFromLocal=corner;
+            laserCloudSurfFromLocal=surf;
+            laserCloudNonFeatureFromLocal=nonf;
+            kdtreeCornerFromLocal->setInputCloud(corner);
+            kdtreeSurfFromLocal->setInputCloud(surf);
+            kdtreeNonFeatureFromLocal->setInputCloud(nonf);
+        }
+        pcl::PointCloud<PointType>::Ptr full_cloud()
+        {
+            pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
+            *cloud+=(*laserCloudCornerFromLocal);
+            *cloud+=(*laserCloudSurfFromLocal);
+            *cloud+=(*laserCloudNonFeatureFromLocal);
+            return cloud;
+        }
+        static int read_txt_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string txt)
+        {
+            std::ifstream in(txt.c_str(), std::ios::in);
+            if (!in)
+            {
+                return 0;
+            }
+            double x_ = 0, y_ = 0, z_ = 0;
+            int r=0,g=0,b=0;
+            int i = 0;
+            while (!in.eof())
+            {
+                in >> x_ >> y_ >> z_>>r>>g>>b;
+                if (in.fail())
+                {
+                    break;
+                }
+                pcl::PointXYZRGB Pt;
+                Pt.x = x_;
+                Pt.y = y_;
+                Pt.z = z_;
+                Pt.r= r;
+                Pt.g=g;
+                Pt.b=b;
+                cloud->points.push_back(Pt);
+                ++i;
+            }
+            in.close();
+            //std::cout << "Import finished ... ..." << std::endl;
+            return true;
+        }
+
+        bool load_map(std::string cloud_file)
+        {
+            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+            read_txt_cloud(cloud,cloud_file);
+
+            for(int i=0;i<cloud->size();++i)
+            {
+                pcl::PointXYZRGB pt=cloud->points[i];
+                PointType pt_temp;
+                pt_temp.x=pt.x;
+                pt_temp.y=pt.y;
+                pt_temp.z=pt.z;
+                if(pt.r==255)
+                {
+                    laserCloudCornerFromLocal->push_back(pt_temp);
+                }
+                else if(pt.g==255)
+                {
+                    laserCloudSurfFromLocal->push_back(pt_temp);
+                }
+                else
+                {
+                    laserCloudNonFeatureFromLocal->push_back(pt_temp);
+                }
+            }
+            printf(" load map   corner:%d  surf:%d  nonf:%d\n",laserCloudCornerFromLocal->size(),
+                   laserCloudSurfFromLocal->size(),laserCloudNonFeatureFromLocal->size());
+            kdtreeCornerFromLocal->setInputCloud(laserCloudCornerFromLocal);
+            kdtreeSurfFromLocal->setInputCloud(laserCloudSurfFromLocal);
+            kdtreeNonFeatureFromLocal->setInputCloud(laserCloudNonFeatureFromLocal);
+            return true;
+        }
+        /*pcl::PointCloud<PointType>::Ptr& corner_cloud(){return laserCloudCornerFromLocal;}
+        pcl::PointCloud<PointType>::Ptr& surf_cloud(){return laserCloudSurfFromLocal;}
+        pcl::PointCloud<PointType>::Ptr& nonf_cloud(){return laserCloudNonFeatureFromLocal;}*/
+
+     public:
+        pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromLocal;
+        pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromLocal;
+        pcl::KdTreeFLANN<PointType>::Ptr kdtreeNonFeatureFromLocal;
+
+        pcl::PointCloud<PointType>::Ptr laserCloudCornerFromLocal;
+        pcl::PointCloud<PointType>::Ptr laserCloudSurfFromLocal;
+        pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureFromLocal;
+    };
+
+public:
+	/** \brief slide window size */
+	static const int SLIDEWINDOWSIZE = 2;
+
+	/** \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;
+		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;
+			return error;
+		}
+	};
+
+	/** \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;
+			return error;
+		}
+	};
+
+	/** \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();
+			return error;
+		}
+	};
+
+	/** \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;
+			return error;
+		}
+	};
+
+public:
+	/** \brief constructor of Estimator
+	*/
+	Estimator(const float& filter_corner, const float& filter_surf,int ivox_nearby_type,int max_ivox_node,float resolution);
+
+	~Estimator();
+
+	void print();
+
+
+	/** \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 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 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 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 m_global_map.laserCloudCornerFromLocal;
+	}
+	pcl::PointCloud<PointType>::Ptr get_surf_map(){
+		return m_global_map.laserCloudSurfFromLocal;
+	}
+	pcl::PointCloud<PointType>::Ptr get_nonfeature_map(){
+		return m_global_map.laserCloudNonFeatureFromLocal;
+	}
+	void MapIncrementLocal(const pcl::PointCloud<PointType>::Ptr& laserCloudCornerStack,
+						   const pcl::PointCloud<PointType>::Ptr& laserCloudSurfStack,
+						   const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureStack,
+						   const Eigen::Matrix4d& transformTobeMapped);
+
+
+	/** \brief store map points */
+    Local_map m_global_map;
+    LocalMapIvox local_map_;
+
+    pcl::PointCloud<PointType> local_corner_cloud_;
+    pcl::PointCloud<PointType> local_surf_cloud_;
+    pcl::PointCloud<PointType> local_nonf_cloud_;
+
+ private:
+	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 laserCloudCornerForMap;
+	pcl::PointCloud<PointType>::Ptr laserCloudSurfForMap;
+	pcl::PointCloud<PointType>::Ptr laserCloudNonFeatureForMap;
+	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::VoxelGrid<PointType> downSizeFilterCorner;
+	pcl::VoxelGrid<PointType> downSizeFilterSurf;
+	pcl::VoxelGrid<PointType> downSizeFilterNonFeature;
+	std::mutex mtx_Map;
+
+	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 = 0.5;
+};
+
+#endif //LIO_LIVOX_ESTIMATOR_H

lio/src/lio/IMUIntegrator.cpp

@@ -0,0 +1,155 @@
+#include "IMUIntegrator.h"
+
+IMUIntegrator::IMUIntegrator(){
+  Reset();
+  noise.setZero();
+  noise.block<3, 3>(0, 0) =  Eigen::Matrix3d::Identity() * gyr_n * gyr_n;
+  noise.block<3, 3>(3, 3) =  Eigen::Matrix3d::Identity() * acc_n * acc_n;
+  noise.block<3, 3>(6, 6) =  Eigen::Matrix3d::Identity() * gyr_w * gyr_w;
+  noise.block<3, 3>(9, 9) =  Eigen::Matrix3d::Identity() * acc_w * acc_w;
+}
+
+/** \brief constructor of IMUIntegrator
+ * \param[in] vIMU: IMU messages need to be integrated
+ */
+IMUIntegrator::IMUIntegrator(std::vector<ImuData> vIMU):
+vimuMsg(std::move(vIMU)){
+  Reset();
+  noise.setZero();
+  noise.block<3, 3>(0, 0) =  Eigen::Matrix3d::Identity() * gyr_n * gyr_n;
+  noise.block<3, 3>(3, 3) =  Eigen::Matrix3d::Identity() * acc_n * acc_n;
+  noise.block<3, 3>(6, 6) =  Eigen::Matrix3d::Identity() * gyr_w * gyr_w;
+  noise.block<3, 3>(9, 9) =  Eigen::Matrix3d::Identity() * acc_w * acc_w;
+}
+
+void IMUIntegrator::Reset(){
+  dq.setIdentity();
+  dp.setZero();
+  dv.setZero();
+  dtime = 0;
+  covariance.setZero();
+  jacobian.setIdentity();
+  linearized_bg.setZero();
+  linearized_ba.setZero();
+}
+
+const Eigen::Quaterniond & IMUIntegrator::GetDeltaQ() const {return dq;}
+
+const Eigen::Vector3d & IMUIntegrator::GetDeltaP() const {return dp;}
+
+const Eigen::Vector3d & IMUIntegrator::GetDeltaV() const {return dv;}
+
+const double & IMUIntegrator::GetDeltaTime() const {return dtime;}
+
+const Eigen::Vector3d & IMUIntegrator::GetBiasGyr() const {return linearized_bg;}
+
+const Eigen::Vector3d& IMUIntegrator::GetBiasAcc() const {return linearized_ba;}
+
+const Eigen::Matrix<double, 15, 15>& IMUIntegrator::GetCovariance(){return covariance;}
+
+const Eigen::Matrix<double, 15, 15> & IMUIntegrator::GetJacobian() const {return jacobian;}
+
+void IMUIntegrator::PushIMUMsg(const ImuData& imu){
+  vimuMsg.push_back(imu);
+}
+void IMUIntegrator::PushIMUMsg(const std::vector<ImuData>& vimu){
+  vimuMsg.insert(vimuMsg.end(), vimu.begin(), vimu.end());
+}
+const std::vector<ImuData> & IMUIntegrator::GetIMUMsg() const {return vimuMsg;}
+
+void IMUIntegrator::GyroIntegration(double lastTime){
+  double current_time = lastTime;
+  for(auto & imu : vimuMsg)
+  {
+    Eigen::Vector3d gyr;
+    gyr << imu.angular_velocity.x,
+            imu.angular_velocity.y,
+            imu.angular_velocity.z;
+    double dt = imu.timebase - current_time;
+    if(dt < 0)
+        printf(" ERROR IMUIntegrator::GyroIntegration");
+    Eigen::Matrix3d dR = Sophus::SO3d::exp(gyr*dt).matrix();
+    Eigen::Quaterniond qr(dq*dR);
+    if (qr.w()<0)
+      qr.coeffs() *= -1;
+    dq = qr.normalized();
+    current_time = imu.timebase;
+  }
+}
+
+void IMUIntegrator::PreIntegration(double lastTime, const Eigen::Vector3d& bg, const Eigen::Vector3d& ba){
+  Reset();
+  linearized_bg = bg;
+  linearized_ba = ba;
+  double current_time = lastTime;
+  for(auto & imu : vimuMsg)
+  {
+    Eigen::Vector3d gyr;
+    gyr <<  imu.angular_velocity.x,
+            imu.angular_velocity.y,
+            imu.angular_velocity.z;
+    Eigen::Vector3d acc;
+    acc << imu.linear_acceleration.x * gnorm,
+            imu.linear_acceleration.y * gnorm,
+            imu.linear_acceleration.z * gnorm;
+    double dt = imu.timebase - current_time;
+    if(dt <= 0 )
+      printf("dt <= 0");
+    gyr -= bg;
+    acc -= ba;
+    double dt2 = dt*dt;
+    Eigen::Vector3d gyr_dt = gyr*dt;
+    Eigen::Matrix3d dR = Sophus::SO3d::exp(gyr_dt).matrix();
+    Eigen::Matrix3d Jr = Eigen::Matrix3d::Identity();
+    double gyr_dt_norm = gyr_dt.norm();
+    if(gyr_dt_norm > 0.00001)
+    {
+      Eigen::Vector3d k = gyr_dt.normalized();
+      Eigen::Matrix3d K = Sophus::SO3d::hat(k);
+      Jr =   Eigen::Matrix3d::Identity()
+             - (1-cos(gyr_dt_norm))/gyr_dt_norm*K
+             + (1-sin(gyr_dt_norm)/gyr_dt_norm)*K*K;
+    }
+    Eigen::Matrix<double,15,15> A = Eigen::Matrix<double,15,15>::Identity();
+    A.block<3,3>(0,3) = -0.5*dq.matrix()*Sophus::SO3d::hat(acc)*dt2;
+    A.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt;
+    A.block<3,3>(0,12) = -0.5*dq.matrix()*dt2;
+    A.block<3,3>(3,3) = dR.transpose();
+    A.block<3,3>(3,9) = - Jr*dt;
+    A.block<3,3>(6,3) = -dq.matrix()*Sophus::SO3d::hat(acc)*dt;
+    A.block<3,3>(6,12) = -dq.matrix()*dt;
+    Eigen::Matrix<double,15,12> B = Eigen::Matrix<double,15,12>::Zero();
+    B.block<3,3>(0,3) = 0.5*dq.matrix()*dt2;
+    B.block<3,3>(3,0) = Jr*dt;
+    B.block<3,3>(6,3) = dq.matrix()*dt;
+    B.block<3,3>(9,6) = Eigen::Matrix3d::Identity()*dt;
+    B.block<3,3>(12,9) = Eigen::Matrix3d::Identity()*dt;
+    jacobian = A * jacobian;
+    covariance = A * covariance * A.transpose() + B * noise * B.transpose();
+    dp += dv*dt + 0.5*dq.matrix()*acc*dt2;
+    dv += dq.matrix()*acc*dt;
+    Eigen::Matrix3d m3dR = dq.matrix()*dR;
+    Eigen::Quaterniond qtmp(m3dR);
+    if (qtmp.w()<0)
+      qtmp.coeffs() *= -1;
+    dq = qtmp.normalized();
+    dtime += dt;
+    current_time = imu.timebase;
+  }
+}
+
+Eigen::Vector3d IMUIntegrator::GetAverageAcc() {
+  int i = 0;
+  Eigen::Vector3d sum_acc(0, 0, 0);
+  for(auto & imu : vimuMsg){
+    Eigen::Vector3d acc;
+    acc << imu.linear_acceleration.x * gnorm,
+           imu.linear_acceleration.y * gnorm,
+           imu.linear_acceleration.z * gnorm;
+    sum_acc += acc;
+    i++;
+    if(i > 30) break;
+  }
+  return sum_acc / i;
+}
+

lio/src/lio/IMUIntegrator.h

@@ -0,0 +1,108 @@
+#ifndef LIO_LIVOX_IMUINTEGRATOR_H
+#define LIO_LIVOX_IMUINTEGRATOR_H
+
+#include <queue>
+#include <mutex>
+#include <Eigen/Core>
+#include <utility>
+
+#include "sophus/so3.hpp"
+#include "typedef.h"
+
+class IMUIntegrator{
+public:
+    IMUIntegrator();
+
+    /** \brief constructor of IMUIntegrator
+     * \param[in] vIMU: IMU messages need to be integrated
+     */
+    IMUIntegrator(std::vector<ImuData> 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 ImuData& imu);
+    void PushIMUMsg(const std::vector<ImuData>& vimu);
+    const std::vector<ImuData> & 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 = 1.5e-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<ImuData> 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

lio/src/lio/LocalMapIvox.cpp

@@ -0,0 +1,97 @@
+//
+// Created by zx on 22-11-10.
+//
+
+#include "LocalMapIvox.h"
+
+void LocalMapIvox::Init(int ivox_nearby_type,int max_ivox_node,float resolution)
+{
+    if (ivox_nearby_type == 0)
+    {
+        ivox_options_.nearby_type_ = IVoxType::NearbyType::CENTER;
+    }
+    else if (ivox_nearby_type == 6)
+    {
+        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY6;
+    }
+    else if (ivox_nearby_type == 18)
+    {
+        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
+    }
+    else if (ivox_nearby_type == 26)
+    {
+        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY26;
+    }
+    else
+    {
+        LOG(WARNING) << "unknown ivox_nearby_type, use NEARBY18";
+        ivox_options_.nearby_type_ = IVoxType::NearbyType::NEARBY18;
+    }
+    ivox_options_.capacity_=max_ivox_node;
+    ivox_options_.resolution_=resolution;
+
+    corner_ivox_ = std::make_shared<IVoxType>(ivox_options_);
+    surf_ivox_ = std::make_shared<IVoxType>(ivox_options_);
+    nonf_ivox_ = std::make_shared<IVoxType>(ivox_options_);
+
+    downSizeFilterCorner.setLeafSize(0.02, 0.02, 0.02);
+    downSizeFilterSurf.setLeafSize(0.03, 0.03, 0.03);
+    downSizeFilterNonFeature.setLeafSize(0.05, 0.05, 0.05);
+}
+
+LocalMapIvox::~LocalMapIvox()
+{
+}
+
+void LocalMapIvox::MapIncrement(const pcl::PointCloud<PointType>::Ptr& laserCloudCorner,
+                            const pcl::PointCloud<PointType>::Ptr& laserCloudSurf,
+                            const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeature,int scan_id)
+{
+    corner_ivox_->AddPoints(laserCloudCorner->points);
+    surf_ivox_->AddPoints(laserCloudSurf->points);
+    nonf_ivox_->AddPoints(laserCloudNonFeature->points);
+    printf(" corner ivox node:%d   surf ivox node:%d  nonf ivox node:%d\n",
+            corner_ivox_->NumValidGrids(),surf_ivox_->NumValidGrids(),nonf_ivox_->NumValidGrids());
+}
+
+void LocalMapIvox::pointAssociateToMap(PointType const * const pi,
+                                   PointType * const po,
+                                   const Eigen::Matrix4d& _transformTobeMapped){
+    Eigen::Vector3d pin, pout;
+    pin.x() = pi->x;
+    pin.y() = pi->y;
+    pin.z() = pi->z;
+    pout = _transformTobeMapped.topLeftCorner(3,3) * pin + _transformTobeMapped.topRightCorner(3,1);
+    po->x = pout.x();
+    po->y = pout.y();
+    po->z = pout.z();
+    po->intensity = pi->intensity;
+    po->normal_z = pi->normal_z;
+}
+
+void LocalMapIvox::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){
+
+    int laserCloudCornerNum = laserCloudCorner->points.size();
+    int laserCloudSurfNum = laserCloudSurf->points.size();
+    int laserCloudNonFeatureNum = laserCloudNonFeature->points.size();
+    PointType pointSel1,pointSel2,pointSel3;
+    for (int i = 0; i < laserCloudCornerNum; i++) {
+        pointAssociateToMap(&laserCloudCorner->points[i], &pointSel1, transformTobeMapped);
+        laserCloudCornerToMap->push_back(pointSel1);
+    }
+    for (int i = 0; i < laserCloudSurfNum; i++) {
+        pointAssociateToMap(&laserCloudSurf->points[i], &pointSel2, transformTobeMapped);
+        laserCloudSurfToMap->push_back(pointSel2);
+    }
+    for (int i = 0; i < laserCloudNonFeatureNum; i++) {
+        pointAssociateToMap(&laserCloudNonFeature->points[i], &pointSel3, transformTobeMapped);
+        laserCloudNonFeatureToMap->push_back(pointSel3);
+    }
+
+}

lio/src/lio/LocalMapIvox.h

@@ -0,0 +1,76 @@
+//
+// Created by zx on 22-11-10.
+//
+
+#ifndef SRC_LIO_LIVOX_SRC_LIO_LOCALMAPIVOX_H_
+#define SRC_LIO_LIVOX_SRC_LIO_LOCALMAPIVOX_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>
+#include "typedef.h"
+#include "ivox3d/ivox3d.h"
+
+
+class LocalMapIvox
+{
+#ifdef IVOX_NODE_TYPE_PHC
+    using IVoxType = faster_lio::IVox<3, faster_lio::IVoxNodeType::PHC, PointType>;
+#else
+    using IVoxType = faster_lio::IVox<3, faster_lio::IVoxNodeType::DEFAULT, PointType>;
+#endif
+ public:
+    LocalMapIvox()= default;
+    void Init(int ivox_nearby_type,int max_ivox_node,float resolution);
+    ~LocalMapIvox();
+
+    void MapIncrement(const pcl::PointCloud<PointType>::Ptr& laserCloudCornerStack,
+                      const pcl::PointCloud<PointType>::Ptr& laserCloudSurfStack,
+                      const pcl::PointCloud<PointType>::Ptr& laserCloudNonFeatureStack,int scan_id);
+
+
+
+    std::shared_ptr<IVoxType> CornerMapVox(){
+        return corner_ivox_;
+    }
+
+    std::shared_ptr<IVoxType> surfMapVox(){
+        return surf_ivox_;
+    }
+
+    std::shared_ptr<IVoxType> NonfMapVox(){
+        return nonf_ivox_;
+    }
+
+
+    static void pointAssociateToMap(PointType const * const pi,
+                                    PointType * const po,
+                                    const Eigen::Matrix4d& _transformTobeMapped);
+
+    static 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);
+
+
+
+ protected:
+    IVoxType::Options ivox_options_;
+    std::shared_ptr<IVoxType> corner_ivox_ = nullptr;
+    std::shared_ptr<IVoxType> surf_ivox_ = nullptr;
+    std::shared_ptr<IVoxType> nonf_ivox_ = nullptr;
+
+    pcl::VoxelGrid<PointType> downSizeFilterCorner;
+    pcl::VoxelGrid<PointType> downSizeFilterSurf;
+    pcl::VoxelGrid<PointType> downSizeFilterNonFeature;
+
+
+};
+
+
+#endif //SRC_LIO_LIVOX_SRC_LIO_LOCALMAPIVOX_H_

lio/src/lio/ceresfunc.cpp

@@ -0,0 +1,31 @@
+#include "ceresfunc.h"
+
+void* ThreadsConstructA(void* threadsstruct)
+{
+  ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);
+  for (auto it : p->sub_factors)
+  {
+    for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++)
+    {
+      int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];
+      int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];
+      Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);
+      for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++)
+      {
+        int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];
+        int size_j = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])];
+        Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);
+        if (i == j)
+          p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
+        else
+        {
+          p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;
+          p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();
+        }
+      }
+      p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;
+    }
+  }
+  return threadsstruct;
+}
+

lio/src/lio/ceresfunc.h

@@ -0,0 +1,824 @@
+#ifndef LIO_LIVOX_CERESFUNC_H
+#define LIO_LIVOX_CERESFUNC_H
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <utility>
+#include <pthread.h>
+#include <unordered_map>
+#include "sophus/so3.hpp"
+#include "IMUIntegrator.h"
+
+const int NUM_THREADS = 4;
+
+/** \brief Residual Block Used for marginalization
+ */
+struct ResidualBlockInfo
+{
+	ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)
+					: cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(std::move(_parameter_blocks)), drop_set(std::move(_drop_set)) {}
+
+	void Evaluate(){
+		residuals.resize(cost_function->num_residuals());
+
+		std::vector<int> block_sizes = cost_function->parameter_block_sizes();
+		raw_jacobians = new double *[block_sizes.size()];
+		jacobians.resize(block_sizes.size());
+
+		for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
+		{
+			jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);
+			raw_jacobians[i] = jacobians[i].data();
+		}
+		cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);
+
+		if (loss_function)
+		{
+			double residual_scaling_, alpha_sq_norm_;
+
+			double sq_norm, rho[3];
+
+			sq_norm = residuals.squaredNorm();
+			loss_function->Evaluate(sq_norm, rho);
+
+			double sqrt_rho1_ = sqrt(rho[1]);
+
+			if ((sq_norm == 0.0) || (rho[2] <= 0.0))
+			{
+				residual_scaling_ = sqrt_rho1_;
+				alpha_sq_norm_ = 0.0;
+			}
+			else
+			{
+				const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];
+				const double alpha = 1.0 - sqrt(D);
+				residual_scaling_ = sqrt_rho1_ / (1 - alpha);
+				alpha_sq_norm_ = alpha / sq_norm;
+			}
+
+			for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++)
+			{
+				jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));
+			}
+
+			residuals *= residual_scaling_;
+		}
+	}
+
+	ceres::CostFunction *cost_function;
+	ceres::LossFunction *loss_function;
+	std::vector<double *> parameter_blocks;
+	std::vector<int> drop_set;
+
+	double **raw_jacobians{};
+	std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
+	Eigen::VectorXd residuals;
+
+};
+
+struct ThreadsStruct
+{
+	std::vector<ResidualBlockInfo *> sub_factors;
+	Eigen::MatrixXd A;
+	Eigen::VectorXd b;
+	std::unordered_map<long, int> parameter_block_size;
+	std::unordered_map<long, int> parameter_block_idx;
+};
+
+/** \brief Multi-thread to process marginalization
+ */
+void* ThreadsConstructA(void* threadsstruct);
+
+/** \brief marginalization infomation
+ */
+class MarginalizationInfo
+{
+public:
+	~MarginalizationInfo(){
+//			ROS_WARN("release marginlizationinfo");
+
+		for (auto it = parameter_block_data.begin(); it != parameter_block_data.end(); ++it)
+			delete[] it->second;
+
+		for (int i = 0; i < (int)factors.size(); i++)
+		{
+			delete[] factors[i]->raw_jacobians;
+			delete factors[i]->cost_function;
+			delete factors[i];
+		}
+	}
+
+	void addResidualBlockInfo(ResidualBlockInfo *residual_block_info){
+		factors.emplace_back(residual_block_info);
+
+		std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;
+		std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();
+
+		for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
+		{
+			double *addr = parameter_blocks[i];
+			int size = parameter_block_sizes[i];
+			parameter_block_size[reinterpret_cast<long>(addr)] = size;
+		}
+
+		for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
+		{
+			double *addr = parameter_blocks[residual_block_info->drop_set[i]];
+			parameter_block_idx[reinterpret_cast<long>(addr)] = 0;
+		}
+	}
+
+	void preMarginalize(){
+		for (auto it : factors)
+		{
+			it->Evaluate();
+
+			std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();
+			for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
+			{
+				long addr = reinterpret_cast<long>(it->parameter_blocks[i]);
+				int size = block_sizes[i];
+				if (parameter_block_data.find(addr) == parameter_block_data.end())
+				{
+					double *data = new double[size];
+					memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
+					parameter_block_data[addr] = data;
+				}
+			}
+		}
+	}
+
+	void marginalize(){
+		int pos = 0;
+		for (auto &it : parameter_block_idx)
+		{
+			it.second = pos;
+			pos += parameter_block_size[it.first];
+		}
+
+		m = pos;
+
+		for (const auto &it : parameter_block_size)
+		{
+			if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
+			{
+				parameter_block_idx[it.first] = pos;
+				pos += it.second;
+			}
+		}
+
+		n = pos - m;
+
+		Eigen::MatrixXd A(pos, pos);
+		Eigen::VectorXd b(pos);
+		A.setZero();
+		b.setZero();
+
+		pthread_t tids[NUM_THREADS];
+		ThreadsStruct threadsstruct[NUM_THREADS];
+		int i = 0;
+		for (auto it : factors)
+		{
+			threadsstruct[i].sub_factors.push_back(it);
+			i++;
+			i = i % NUM_THREADS;
+		}
+		for (int i = 0; i < NUM_THREADS; i++)
+		{
+			threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
+			threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
+			threadsstruct[i].parameter_block_size = parameter_block_size;
+			threadsstruct[i].parameter_block_idx = parameter_block_idx;
+			int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
+			if (ret != 0)
+			{
+				std::cout<<"pthread_create error"<<std::endl;
+				exit(1);
+			}
+		}
+		for( int i = NUM_THREADS - 1; i >= 0; i--)
+		{
+			pthread_join( tids[i], NULL );
+			A += threadsstruct[i].A;
+			b += threadsstruct[i].b;
+		}
+		Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
+		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
+
+		Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
+
+		Eigen::VectorXd bmm = b.segment(0, m);
+		Eigen::MatrixXd Amr = A.block(0, m, m, n);
+		Eigen::MatrixXd Arm = A.block(m, 0, n, m);
+		Eigen::MatrixXd Arr = A.block(m, m, n, n);
+		Eigen::VectorXd brr = b.segment(m, n);
+		A = Arr - Arm * Amm_inv * Amr;
+		b = brr - Arm * Amm_inv * bmm;
+
+		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
+		Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
+		Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));
+
+		Eigen::VectorXd S_sqrt = S.cwiseSqrt();
+		Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
+
+		linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
+		linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;
+	}
+
+	std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift){
+		std::vector<double *> keep_block_addr;
+		keep_block_size.clear();
+		keep_block_idx.clear();
+		keep_block_data.clear();
+
+		for (const auto &it : parameter_block_idx)
+		{
+			if (it.second >= m)
+			{
+				keep_block_size.push_back(parameter_block_size[it.first]);
+				keep_block_idx.push_back(parameter_block_idx[it.first]);
+				keep_block_data.push_back(parameter_block_data[it.first]);
+				keep_block_addr.push_back(addr_shift[it.first]);
+			}
+		}
+		sum_block_size = std::accumulate(std::begin(keep_block_size), std::end(keep_block_size), 0);
+
+		return keep_block_addr;
+	}
+
+	std::vector<ResidualBlockInfo *> factors;
+	int m, n;
+	std::unordered_map<long, int> parameter_block_size;
+	int sum_block_size;
+	std::unordered_map<long, int> parameter_block_idx;
+	std::unordered_map<long, double *> parameter_block_data;
+
+	std::vector<int> keep_block_size;
+	std::vector<int> keep_block_idx;
+	std::vector<double *> keep_block_data;
+
+	Eigen::MatrixXd linearized_jacobians;
+	Eigen::VectorXd linearized_residuals;
+	const double eps = 1e-8;
+
+};
+
+/** \brief Ceres Cost Funtion Used for Marginalization
+ */
+class MarginalizationFactor : public ceres::CostFunction
+{
+public:
+	explicit MarginalizationFactor(MarginalizationInfo* _marginalization_info):marginalization_info(_marginalization_info){
+		int cnt = 0;
+		for (auto it : marginalization_info->keep_block_size)
+		{
+			mutable_parameter_block_sizes()->push_back(it);
+			cnt += it;
+		}
+		set_num_residuals(marginalization_info->n);
+	};
+
+	bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override{
+		int n = marginalization_info->n;
+		int m = marginalization_info->m;
+		Eigen::VectorXd dx(n);
+		for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
+		{
+			int size = marginalization_info->keep_block_size[i];
+			int idx = marginalization_info->keep_block_idx[i] - m;
+			Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);
+			Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);
+			if(size == 6){
+				dx.segment<3>(idx + 0) = x.segment<3>(0) - x0.segment<3>(0);
+				dx.segment<3>(idx + 3) = (Sophus::SO3d::exp(x.segment<3>(3)).inverse() * Sophus::SO3d::exp(x0.segment<3>(3))).log();
+			}else{
+				dx.segment(idx, size) = x - x0;
+			}
+		}
+		Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
+		if (jacobians)
+		{
+
+			for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++)
+			{
+				if (jacobians[i])
+				{
+					int size = marginalization_info->keep_block_size[i];
+					int idx = marginalization_info->keep_block_idx[i] - m;
+					Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);
+					jacobian.setZero();
+					jacobian.leftCols(size) = marginalization_info->linearized_jacobians.middleCols(idx, size);
+				}
+			}
+		}
+		return true;
+	}
+
+	MarginalizationInfo* marginalization_info;
+};
+
+/** \brief Ceres Cost Funtion between Lidar Pose and IMU Preintegration
+ */
+struct Cost_NavState_PRV_Bias
+{
+	Cost_NavState_PRV_Bias(IMUIntegrator& measure_,
+							Eigen::Vector3d& GravityVec_,
+							Eigen::Matrix<double, 15, 15>  sqrt_information_):
+					imu_measure(measure_),
+					GravityVec(GravityVec_),
+					sqrt_information(std::move(sqrt_information_)){}
+
+	template <typename T>
+	bool operator()( const T *pri_, const T *velobiasi_, const T *prj_, const T *velobiasj_, T *residual) const
+	{
+		Eigen::Map<const Eigen::Matrix<T, 6, 1>> PRi(pri_);
+		Eigen::Matrix<T, 3, 1> Pi = PRi.template segment<3>(0);
+		Sophus::SO3<T> SO3_Ri = Sophus::SO3<T>::exp(PRi.template segment<3>(3));
+
+		Eigen::Map<const Eigen::Matrix<T, 6, 1>> PRj(prj_);
+		Eigen::Matrix<T, 3, 1> Pj = PRj.template segment<3>(0);
+		Sophus::SO3<T> SO3_Rj = Sophus::SO3<T>::exp(PRj.template segment<3>(3));
+
+		Eigen::Map<const Eigen::Matrix<T, 9, 1>> velobiasi(velobiasi_);
+		Eigen::Matrix<T, 3, 1> Vi = velobiasi.template segment<3>(0);
+		Eigen::Matrix<T, 3, 1> dbgi = velobiasi.template segment<3>(3) - imu_measure.GetBiasGyr().cast<T>();
+		Eigen::Matrix<T, 3, 1> dbai = velobiasi.template segment<3>(6) - imu_measure.GetBiasAcc().cast<T>();
+
+		Eigen::Map<const Eigen::Matrix<T, 9, 1>> velobiasj(velobiasj_);
+		Eigen::Matrix<T, 3, 1> Vj = velobiasj.template segment<3>(0);
+
+		Eigen::Map<Eigen::Matrix<T, 15, 1> > eResiduals(residual);
+		eResiduals = Eigen::Matrix<T, 15, 1>::Zero();
+
+		T dTij = T(imu_measure.GetDeltaTime());
+		T dT2 = dTij*dTij;
+		Eigen::Matrix<T, 3, 1> dPij = imu_measure.GetDeltaP().cast<T>();
+		Eigen::Matrix<T, 3, 1> dVij = imu_measure.GetDeltaV().cast<T>();
+		Sophus::SO3<T> dRij = Sophus::SO3<T>(imu_measure.GetDeltaQ().cast<T>());
+		Sophus::SO3<T> RiT = SO3_Ri.inverse();
+
+		Eigen::Matrix<T, 3, 1> rPij = RiT*(Pj - Pi - Vi*dTij - 0.5*GravityVec.cast<T>()*dT2) -
+						(dPij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BG).cast<T>()*dbgi +
+						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BA).cast<T>()*dbai);
+
+		Sophus::SO3<T> dR_dbg = Sophus::SO3<T>::exp(
+						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_R, IMUIntegrator::O_BG).cast<T>()*dbgi);
+		Sophus::SO3<T> rRij = (dRij * dR_dbg).inverse() * RiT * SO3_Rj;
+		Eigen::Matrix<T, 3, 1> rPhiij = rRij.log();
+
+		Eigen::Matrix<T, 3, 1> rVij = RiT*(Vj - Vi - GravityVec.cast<T>()*dTij) -
+						(dVij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BG).cast<T>()*dbgi +
+										imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BA).cast<T>()*dbai);
+
+		eResiduals.template segment<3>(0) = rPij;
+		eResiduals.template segment<3>(3) = rPhiij;
+		eResiduals.template segment<3>(6) = rVij;
+		eResiduals.template segment<6>(9) = velobiasj.template segment<6>(3) - velobiasi.template segment<6>(3);
+
+		eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());
+
+		return true;
+	}
+
+	static ceres::CostFunction *Create(IMUIntegrator& measure_,
+										Eigen::Vector3d& GravityVec_,
+										Eigen::Matrix<double, 15, 15>  sqrt_information_) {
+		return (new ceres::AutoDiffCostFunction<Cost_NavState_PRV_Bias, 15, 6, 9, 6, 9>(
+						new Cost_NavState_PRV_Bias(measure_,
+													GravityVec_,
+													std::move(sqrt_information_))));
+	}
+
+	IMUIntegrator imu_measure;
+	Eigen::Vector3d GravityVec;
+	Eigen::Matrix<double, 15, 15> sqrt_information;
+};
+
+/** \brief Ceres Cost Funtion between PointCloud Sharp Feature and Map Cloud
+ */
+struct Cost_NavState_IMU_Line
+{
+    Cost_NavState_IMU_Line(Eigen::Vector3d  _p, Eigen::Vector3d  _vtx1, Eigen::Vector3d  _vtx2,
+                           const Eigen::Matrix4d& Tbl, Eigen::Matrix<double, 1, 1>  sqrt_information_):
+            point(std::move(_p)), vtx1(std::move(_vtx1)), vtx2(std::move(_vtx2)),
+            sqrt_information(std::move(sqrt_information_))
+    {
+        l12 = std::sqrt((vtx1(0) - vtx2(0)) * (vtx1(0) - vtx2(0)) + (vtx1(1) - vtx2(1)) *
+                (vtx1(1) - vtx2(1)) + (vtx1(2) - vtx2(2)) * (vtx1(2) - vtx2(2)));
+        Eigen::Matrix3d m3d = Tbl.topLeftCorner(3, 3);
+        qbl = Eigen::Quaterniond(m3d).normalized();
+        qlb = qbl.conjugate();
+        Pbl = Tbl.topRightCorner(3, 1);
+        Plb = -(qlb * Pbl);
+    }
+
+    template <typename T>
+    bool operator()(const T *PRi, T *residual) const {
+      Eigen::Matrix<T, 3, 1> cp{T(point.x()), T(point.y()), T(point.z())};
+      Eigen::Matrix<T, 3, 1> lpa{T(vtx1.x()), T(vtx1.y()), T(vtx1.z())};
+      Eigen::Matrix<T, 3, 1> lpb{T(vtx2.x()), T(vtx2.y()), T(vtx2.z())};
+
+      Eigen::Map<const Eigen::Matrix<T, 6, 1>> pri_wb(PRi);
+      //朝向 3-6
+      Eigen::Quaternion<T> q_wb = Sophus::SO3<T>::exp(pri_wb.template segment<3>(3)).unit_quaternion();
+      Eigen::Matrix<T, 3, 1> t_wb = pri_wb.template segment<3>(0);
+      Eigen::Quaternion<T> q_wl = q_wb * qbl.cast<T>();
+      Eigen::Matrix<T, 3, 1> t_wl = q_wb * Pbl.cast<T>() + t_wb;
+      Eigen::Matrix<T, 3, 1> P_to_Map = q_wl * cp + t_wl;
+
+      T a012 = ceres::sqrt(
+              ((P_to_Map(0) - lpa(0)) * (P_to_Map(1) - lpb(1)) - (P_to_Map(0) - lpb(0)) * (P_to_Map(1) - lpa(1)))
+              * ((P_to_Map(0) - lpa(0)) * (P_to_Map(1) - lpb(1)) - (P_to_Map(0) - lpb(0)) * (P_to_Map(1) - lpa(1)))
+              + ((P_to_Map(0) - lpa(0)) * (P_to_Map(2) - lpb(2)) - (P_to_Map(0) - lpb(0)) * (P_to_Map(2) - lpa(2)))
+                * ((P_to_Map(0) - lpa(0)) * (P_to_Map(2) - lpb(2)) - (P_to_Map(0) - lpb(0)) * (P_to_Map(2) - lpa(2)))
+              + ((P_to_Map(1) - lpa(1)) * (P_to_Map(2) - lpb(2)) - (P_to_Map(1) - lpb(1)) * (P_to_Map(2) - lpa(2)))
+                * ((P_to_Map(1) - lpa(1)) * (P_to_Map(2) - lpb(2)) - (P_to_Map(1) - lpb(1)) * (P_to_Map(2) - lpa(2))));
+      T ld2 = a012 / T(l12);
+      T _weight = T(1) - T(0.9) * ceres::abs(ld2) / ceres::sqrt(
+              ceres::sqrt( P_to_Map(0) * P_to_Map(0) +
+                           P_to_Map(1) * P_to_Map(1) +
+                           P_to_Map(2) * P_to_Map(2) ));
+      residual[0] = T(sqrt_information(0)) * _weight * ld2;
+
+      return true;
+    }
+
+    static ceres::CostFunction *Create(const Eigen::Vector3d& curr_point_,
+                                       const Eigen::Vector3d& last_point_a_,
+                                       const Eigen::Vector3d& last_point_b_,
+                                       const Eigen::Matrix4d& Tbl,
+                                       Eigen::Matrix<double, 1, 1>  sqrt_information_) {
+      return (new ceres::AutoDiffCostFunction<Cost_NavState_IMU_Line, 1, 6>(
+              new Cost_NavState_IMU_Line(curr_point_, last_point_a_, last_point_b_, Tbl, std::move(sqrt_information_))));
+    }
+
+    Eigen::Vector3d point;
+    Eigen::Vector3d vtx1;
+    Eigen::Vector3d vtx2;
+    double l12;
+    Eigen::Quaterniond qbl, qlb;
+    Eigen::Vector3d Pbl, Plb;
+    Eigen::Matrix<double, 1, 1> sqrt_information;
+};
+
+/** \brief Ceres Cost Funtion between PointCloud Flat Feature and Map Cloud
+ */
+struct Cost_NavState_IMU_Plan
+{
+    Cost_NavState_IMU_Plan(Eigen::Vector3d  _p, double _pa, double _pb, double _pc, double _pd,
+
+                           const Eigen::Matrix4d& Tbl, Eigen::Matrix<double, 1, 1>  sqrt_information_):
+            point(std::move(_p)), pa(_pa), pb(_pb), pc(_pc), pd(_pd), sqrt_information(std::move(sqrt_information_)){
+      Eigen::Matrix3d m3d = Tbl.topLeftCorner(3,3);
+      qbl = Eigen::Quaterniond(m3d).normalized();
+      qlb = qbl.conjugate();
+      Pbl = Tbl.topRightCorner(3,1);
+      Plb = -(qlb * Pbl);
+    }
+
+    template <typename T>
+    bool operator()(const T *PRi, T *residual) const {
+      Eigen::Matrix<T, 3, 1> cp{T(point.x()), T(point.y()), T(point.z())};
+
+      Eigen::Map<const Eigen::Matrix<T, 6, 1>> pri_wb(PRi);
+      Eigen::Quaternion<T> q_wb = Sophus::SO3<T>::exp(pri_wb.template segment<3>(3)).unit_quaternion();
+      Eigen::Matrix<T, 3, 1> t_wb = pri_wb.template segment<3>(0);
+      Eigen::Quaternion<T> q_wl = q_wb * qbl.cast<T>();
+      Eigen::Matrix<T, 3, 1> t_wl = q_wb * Pbl.cast<T>() + t_wb;
+      Eigen::Matrix<T, 3, 1> P_to_Map = q_wl * cp + t_wl;
+
+      T pd2 = T(pa) * P_to_Map(0) + T(pb) * P_to_Map(1) + T(pc) * P_to_Map(2) + T(pd);
+      T _weight = T(1) - T(0.9) * ceres::abs(pd2) /ceres::sqrt(
+              ceres::sqrt( P_to_Map(0) * P_to_Map(0) +
+                           P_to_Map(1) * P_to_Map(1) +
+                           P_to_Map(2) * P_to_Map(2) ));
+      residual[0] = T(sqrt_information(0)) * _weight * pd2;
+
+      return true;
+    }
+
+    static ceres::CostFunction *Create(const Eigen::Vector3d& curr_point_,
+                                       const double& pa_,
+                                       const double& pb_,
+                                       const double& pc_,
+                                       const double& pd_,
+                                       const Eigen::Matrix4d& Tbl,
+                                       Eigen::Matrix<double, 1, 1>  sqrt_information_) {
+      return (new ceres::AutoDiffCostFunction<Cost_NavState_IMU_Plan, 1, 6>(
+              new Cost_NavState_IMU_Plan(curr_point_, pa_, pb_, pc_, pd_, Tbl, std::move(sqrt_information_))));
+    }
+
+    double pa, pb, pc, pd;  //平面方程
+    Eigen::Vector3d point;  //当前点
+    Eigen::Quaterniond qbl, qlb;    //雷达与imu之间的变换与逆变换
+    Eigen::Vector3d Pbl, Plb;
+    Eigen::Matrix<double, 1, 1> sqrt_information;
+};
+
+
+/** \brief Ceres Cost Funtion between PointCloud Flat Feature and Map Cloud
+ */
+struct Cost_NavState_IMU_Plan_Vec
+{
+    Cost_NavState_IMU_Plan_Vec(Eigen::Vector3d  _p, 
+							   Eigen::Vector3d  _p_proj,
+							   const Eigen::Matrix4d& Tbl,
+							   Eigen::Matrix<double, 3, 3> _sqrt_information):
+                               point(std::move(_p)),
+							   point_proj(std::move(_p_proj)), 
+							   sqrt_information(std::move(_sqrt_information)){
+      Eigen::Matrix3d m3d = Tbl.topLeftCorner(3,3);
+      qbl = Eigen::Quaterniond(m3d).normalized();
+      qlb = qbl.conjugate();
+      Pbl = Tbl.topRightCorner(3,1);
+      Plb = -(qlb * Pbl);
+    }
+
+    template <typename T>
+    bool operator()(const T *PRi, T *residual) const {
+      Eigen::Matrix<T, 3, 1> cp{T(point.x()), T(point.y()), T(point.z())};
+	  Eigen::Matrix<T, 3, 1> cp_proj{T(point_proj.x()), T(point_proj.y()), T(point_proj.z())};
+
+      Eigen::Map<const Eigen::Matrix<T, 6, 1>> pri_wb(PRi);
+      Eigen::Quaternion<T> q_wb = Sophus::SO3<T>::exp(pri_wb.template segment<3>(3)).unit_quaternion();
+      Eigen::Matrix<T, 3, 1> t_wb = pri_wb.template segment<3>(0);
+      Eigen::Quaternion<T> q_wl = q_wb * qbl.cast<T>();
+      Eigen::Matrix<T, 3, 1> t_wl = q_wb * Pbl.cast<T>() + t_wb;
+      Eigen::Matrix<T, 3, 1> P_to_Map = q_wl * cp + t_wl;
+
+	  Eigen::Map<Eigen::Matrix<T, 3, 1> > eResiduals(residual);
+      eResiduals = P_to_Map - cp_proj;
+	  T _weight = T(1) - T(0.9) * (P_to_Map - cp_proj).norm() /ceres::sqrt(
+              ceres::sqrt( P_to_Map(0) * P_to_Map(0) +
+                           P_to_Map(1) * P_to_Map(1) +
+                           P_to_Map(2) * P_to_Map(2) ));
+	  eResiduals *= _weight;
+	  eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());
+
+      return true;
+    }
+
+    static ceres::CostFunction *Create(const Eigen::Vector3d& curr_point_,
+                                       const Eigen::Vector3d&  p_proj_,
+                                       const Eigen::Matrix4d& Tbl,
+									   const Eigen::Matrix<double, 3, 3> sqrt_information_) {
+      return (new ceres::AutoDiffCostFunction<Cost_NavState_IMU_Plan_Vec, 3, 6>(
+              new Cost_NavState_IMU_Plan_Vec(curr_point_, p_proj_, Tbl, sqrt_information_)));
+    }
+
+    Eigen::Vector3d point;
+	Eigen::Vector3d point_proj;
+    Eigen::Quaterniond qbl, qlb;
+    Eigen::Vector3d Pbl, Plb;
+	Eigen::Matrix<double, 3, 3> sqrt_information;
+};
+
+
+struct Cost_NonFeature_ICP
+{
+    Cost_NonFeature_ICP(Eigen::Vector3d  _p, double _pa, double _pb, double _pc, double _pd,
+                        const Eigen::Matrix4d& Tbl, Eigen::Matrix<double, 1, 1>  sqrt_information_):
+            			point(std::move(_p)), pa(_pa), pb(_pb), pc(_pc), pd(_pd), sqrt_information(std::move(sqrt_information_)){
+      Eigen::Matrix3d m3d = Tbl.topLeftCorner(3,3);
+      qbl = Eigen::Quaterniond(m3d).normalized();
+      qlb = qbl.conjugate();
+      Pbl = Tbl.topRightCorner(3,1);
+      Plb = -(qlb * Pbl);
+    }
+
+    template <typename T>
+    bool operator()(const T *PRi, T *residual) const {
+      Eigen::Matrix<T, 3, 1> cp{T(point.x()), T(point.y()), T(point.z())};
+
+      Eigen::Map<const Eigen::Matrix<T, 6, 1>> pri_wb(PRi);
+      Eigen::Quaternion<T> q_wb = Sophus::SO3<T>::exp(pri_wb.template segment<3>(3)).unit_quaternion();
+      Eigen::Matrix<T, 3, 1> t_wb = pri_wb.template segment<3>(0);
+      Eigen::Quaternion<T> q_wl = q_wb * qbl.cast<T>();
+      Eigen::Matrix<T, 3, 1> t_wl = q_wb * Pbl.cast<T>() + t_wb;
+      Eigen::Matrix<T, 3, 1> P_to_Map = q_wl * cp + t_wl;
+
+      T pd2 = T(pa) * P_to_Map(0) + T(pb) * P_to_Map(1) + T(pc) * P_to_Map(2) + T(pd);
+      T _weight = T(1) - T(0.9) * ceres::abs(pd2) /ceres::sqrt(
+              ceres::sqrt( P_to_Map(0) * P_to_Map(0) +
+                           P_to_Map(1) * P_to_Map(1) +
+                           P_to_Map(2) * P_to_Map(2) ));
+      residual[0] = T(sqrt_information(0)) * _weight * pd2;
+
+      return true;
+    }
+
+    static ceres::CostFunction *Create(const Eigen::Vector3d& curr_point_,
+                                       const double& pa_,
+                                       const double& pb_,
+                                       const double& pc_,
+                                       const double& pd_,
+                                       const Eigen::Matrix4d& Tbl,
+                                       Eigen::Matrix<double, 1, 1>  sqrt_information_) {
+      return (new ceres::AutoDiffCostFunction<Cost_NonFeature_ICP, 1, 6>(
+              new Cost_NonFeature_ICP(curr_point_, pa_, pb_, pc_, pd_, Tbl, std::move(sqrt_information_))));
+    }
+
+    double pa, pb, pc, pd;
+    Eigen::Vector3d point;
+    Eigen::Quaterniond qbl, qlb;
+    Eigen::Vector3d Pbl, Plb;
+    Eigen::Matrix<double, 1, 1> sqrt_information;
+};
+
+/** \brief Ceres Cost Funtion for Initial Gravity Direction
+ */
+struct Cost_Initial_G
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	Cost_Initial_G(Eigen::Vector3d acc_): acc(acc_){}
+
+	template <typename T>
+	bool operator()( const T *q, T *residual) const {
+		Eigen::Matrix<T, 3, 1> acc_T = acc.cast<T>();
+		Eigen::Quaternion<T> q_wg{q[0], q[1], q[2], q[3]};
+		Eigen::Matrix<T, 3, 1> g_I{T(0), T(0), T(-9.805)};
+		Eigen::Matrix<T, 3, 1> resi = q_wg * g_I - acc_T;
+		residual[0] = resi[0];
+		residual[1] = resi[1];
+		residual[2] = resi[2];
+
+		return true;
+	}
+
+	static ceres::CostFunction *Create(Eigen::Vector3d acc_) {
+		return (new ceres::AutoDiffCostFunction<Cost_Initial_G, 3, 4>(
+						new Cost_Initial_G(acc_)));
+	}
+
+	Eigen::Vector3d acc;
+};
+
+/** \brief Ceres Cost Funtion of IMU Factor in LIO Initialization
+ */
+struct Cost_Initialization_IMU
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	Cost_Initialization_IMU(IMUIntegrator& measure_,
+									Eigen::Vector3d ri_,
+									Eigen::Vector3d rj_,
+									Eigen::Vector3d dp_,
+									Eigen::Matrix<double, 9, 9>  sqrt_information_):
+									imu_measure(measure_),
+									ri(ri_),
+									rj(rj_),
+									dp(dp_),
+									sqrt_information(std::move(sqrt_information_)){}
+
+	template <typename T>
+	bool operator()(const T *rwg_, const T *vi_, const T *vj_, const T *ba_, const T *bg_, T *residual) const {
+		Eigen::Matrix<T, 3, 1> G_I{T(0), T(0), T(-9.805)};
+		
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> ba(ba_);
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> bg(bg_);
+		Eigen::Matrix<T, 3, 1> dbg = bg - imu_measure.GetBiasGyr().cast<T>();
+		Eigen::Matrix<T, 3, 1> dba = ba - imu_measure.GetBiasAcc().cast<T>();
+		
+		Sophus::SO3<T> SO3_Ri = Sophus::SO3<T>::exp(ri.cast<T>());
+		Sophus::SO3<T> SO3_Rj = Sophus::SO3<T>::exp(rj.cast<T>());
+
+		Eigen::Matrix<T, 3, 1> dP = dp.cast<T>();
+
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> rwg(rwg_);
+		Sophus::SO3<T> SO3_Rwg = Sophus::SO3<T>::exp(rwg);
+
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> vi(vi_);
+		Eigen::Matrix<T, 3, 1> Vi = vi;
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> vj(vj_);
+		Eigen::Matrix<T, 3, 1> Vj = vj;
+
+		Eigen::Map<Eigen::Matrix<T, 9, 1> > eResiduals(residual);
+		eResiduals = Eigen::Matrix<T, 9, 1>::Zero();
+
+		T dTij = T(imu_measure.GetDeltaTime());
+		T dT2 = dTij*dTij;
+		Eigen::Matrix<T, 3, 1> dPij = imu_measure.GetDeltaP().cast<T>();
+		Eigen::Matrix<T, 3, 1> dVij = imu_measure.GetDeltaV().cast<T>();
+		Sophus::SO3<T> dRij = Sophus::SO3<T>(imu_measure.GetDeltaQ().cast<T>());
+		Sophus::SO3<T> RiT = SO3_Ri.inverse();
+
+		Eigen::Matrix<T, 3, 1> rPij = RiT*(dP - Vi*dTij - SO3_Rwg*G_I*dT2*T(0.5)) -
+						(dPij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BG).cast<T>()*dbg +
+						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_P, IMUIntegrator::O_BA).cast<T>()*dba);
+
+		Sophus::SO3<T> dR_dbg = Sophus::SO3<T>::exp(
+						imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_R, IMUIntegrator::O_BG).cast<T>()*dbg);
+		Sophus::SO3<T> rRij = (dRij * dR_dbg).inverse() * RiT * SO3_Rj;
+		Eigen::Matrix<T, 3, 1> rPhiij = rRij.log();
+
+		Eigen::Matrix<T, 3, 1> rVij = RiT*(Vj - Vi - SO3_Rwg*G_I*dTij) -
+						(dVij + imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BG).cast<T>()*dbg +
+										imu_measure.GetJacobian().block<3,3>(IMUIntegrator::O_V, IMUIntegrator::O_BA).cast<T>()*dba);
+
+		eResiduals.template segment<3>(0) = rPij;
+		eResiduals.template segment<3>(3) = rPhiij;
+		eResiduals.template segment<3>(6) = rVij;
+
+		eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());
+
+		return true;
+	}
+
+	static ceres::CostFunction *Create(IMUIntegrator& measure_,
+										Eigen::Vector3d ri_,
+										Eigen::Vector3d rj_,
+										Eigen::Vector3d dp_,
+										Eigen::Matrix<double, 9, 9>  sqrt_information_) {
+		return (new ceres::AutoDiffCostFunction<Cost_Initialization_IMU, 9, 3, 3, 3, 3, 3>(
+						new Cost_Initialization_IMU(measure_,
+															ri_,
+															rj_,
+															dp_,
+															std::move(sqrt_information_))));
+	}
+
+	IMUIntegrator imu_measure;
+	Eigen::Vector3d ri;
+	Eigen::Vector3d rj;
+	Eigen::Vector3d dp;
+	Eigen::Matrix<double, 9, 9> sqrt_information;
+};
+
+/** \brief Ceres Cost Funtion of IMU Biases and Velocity Prior Factor in LIO Initialization
+ */
+struct Cost_Initialization_Prior_bv
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	Cost_Initialization_Prior_bv(Eigen::Vector3d prior_, 
+									Eigen::Matrix3d sqrt_information_):
+									prior(prior_),
+									sqrt_information(std::move(sqrt_information_)){}
+
+	template <typename T>
+	bool operator()(const T *bv_, T *residual) const {
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> bv(bv_);
+		Eigen::Matrix<T, 3, 1> Bv = bv;
+
+		Eigen::Matrix<T, 3, 1> prior_T(prior.cast<T>());
+		Eigen::Matrix<T, 3, 1> prior_Bv = prior_T;
+
+		Eigen::Map<Eigen::Matrix<T, 3, 1> > eResiduals(residual);
+		eResiduals = Eigen::Matrix<T, 3, 1>::Zero();
+
+		eResiduals = Bv - prior_Bv;
+
+		eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());
+
+		return true;
+	}
+
+	static ceres::CostFunction *Create(Eigen::Vector3d prior_, Eigen::Matrix3d sqrt_information_) {
+		return (new ceres::AutoDiffCostFunction<Cost_Initialization_Prior_bv, 3, 3>(
+						new Cost_Initialization_Prior_bv(prior_, std::move(sqrt_information_))));
+	}
+
+	Eigen::Vector3d prior;
+	Eigen::Matrix3d sqrt_information;
+};
+
+/** \brief Ceres Cost Funtion of Rwg Prior Factor in LIO Initialization
+ */
+struct Cost_Initialization_Prior_R
+{
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	Cost_Initialization_Prior_R(Eigen::Vector3d prior_, 
+								Eigen::Matrix3d sqrt_information_):
+								prior(prior_),
+								sqrt_information(std::move(sqrt_information_)){}
+
+	template <typename T>
+	bool operator()( const T *r_wg_, T *residual) const {
+		Eigen::Map<const Eigen::Matrix<T, 3, 1>> r_wg(r_wg_);
+		Eigen::Matrix<T, 3, 1> R_wg = r_wg;
+		Sophus::SO3<T> SO3_R_wg = Sophus::SO3<T>::exp(R_wg);
+
+		Eigen::Matrix<T, 3, 1> prior_T(prior.cast<T>());
+		Sophus::SO3<T> prior_R_wg = Sophus::SO3<T>::exp(prior_T);
+
+		Sophus::SO3<T> d_R = SO3_R_wg.inverse() * prior_R_wg;
+		Eigen::Matrix<T, 3, 1> d_Phi = d_R.log();
+
+		Eigen::Map<Eigen::Matrix<T, 3, 1> > eResiduals(residual);
+		eResiduals = Eigen::Matrix<T, 3, 1>::Zero();
+
+		eResiduals = d_Phi;
+
+		eResiduals.applyOnTheLeft(sqrt_information.template cast<T>());
+
+		return true;
+	}
+
+	static ceres::CostFunction *Create(Eigen::Vector3d prior_, Eigen::Matrix3d sqrt_information_) {
+		return (new ceres::AutoDiffCostFunction<Cost_Initialization_Prior_R, 3, 3>(
+						new Cost_Initialization_Prior_R(prior_, std::move(sqrt_information_))));
+	}
+
+	Eigen::Vector3d prior;
+	Eigen::Matrix3d sqrt_information;
+};
+
+#endif //LIO_LIVOX_CERESFUNC_H

lio/src/lio/mapper.cpp

@@ -0,0 +1,664 @@
+//
+// Created by zx on 22-11-1.
+//
+
+#include "mapper.h"
+
+
+Mapper::Mapper(int wndsize,float filter_coner,float filter_surf,
+               int ivox_nearby_type,int max_ivox_node,float resolution)
+{
+    odomcallback_= nullptr;
+    cloudMappedcallback_= nullptr;
+    WINDOWSIZE=wndsize;
+    exit_=false;
+
+    estimator = new Estimator(filter_parameter_corner, filter_parameter_surf,
+            ivox_nearby_type,max_ivox_node,resolution);
+    lidarFrameList.reset(new std::list<Estimator::LidarFrame>);
+    thread_ = std::thread(&Mapper::process, this);
+
+}
+Mapper::~Mapper()
+{
+    exit_=true;
+    thread_.join();
+    if(estimator){
+        delete estimator;
+        estimator= nullptr;
+    }
+}
+
+
+void Mapper::completed()
+{
+    exit_=true;
+    thread_.join();
+    estimator->print();
+
+}
+
+void Mapper::InitRT(const Eigen::Matrix3d& R,const Eigen::Vector3d& t)
+{
+    exTlb.topLeftCorner(3,3) = R;
+    exTlb.topRightCorner(3,1) = t;
+    exRlb = R;
+    exRbl = R.transpose();
+    exPlb = t;
+    exPbl = -1.0 * exRbl * exPlb;
+}
+
+void Mapper::SetOdomCallback(odomCallback callback)
+{
+    odomcallback_=callback;
+}
+
+void Mapper::SetCloudMappedCallback(CloudMappedCallback callback)
+{
+    cloudMappedcallback_=callback;
+}
+
+void Mapper::SetLocalMapCloudCallback(CloudMappedCallback callback)
+{
+    cloudLocalMappcallback_=callback;
+}
+
+void Mapper::AddImu(const ImuData& imu)
+{
+    std::unique_lock<std::mutex> lock(_mutexIMUQueue);
+    _imuMsgQueue.push(imu);
+}
+void Mapper::AddCloud(const TimeCloud& cloud)
+{
+    std::unique_lock<std::mutex> lock(_mutexLidarQueue);
+    _lidarMsgQueue.push(cloud);
+}
+
+
+bool Mapper::fetchImuMsgs(double startTime, double endTime, std::vector<ImuData> &vimuMsg){
+    std::unique_lock<std::mutex> lock(_mutexIMUQueue);
+    double current_time = 0;
+    vimuMsg.clear();
+    while(true)
+    {
+        if(_imuMsgQueue.empty())
+            break;
+        if(_imuMsgQueue.back().timebase<endTime ||
+                _imuMsgQueue.front().timebase>=endTime)
+            break;
+        ImuData& tmpimumsg = _imuMsgQueue.front();
+        double time = tmpimumsg.timebase;
+        if(time<=endTime && time>startTime){
+            vimuMsg.push_back(tmpimumsg);
+            current_time = time;
+            _imuMsgQueue.pop();
+            if(time == endTime) break;
+        } else{
+            if(time<=startTime){
+                _imuMsgQueue.pop();
+            } else{
+                double dt_1 = endTime - current_time;
+                double dt_2 = time - endTime;
+                if(dt_1 < 0 || dt_2 < 0 || dt_1 + dt_2<=0)
+                    printf("ERROR  fetchImuMsgs  \n");
+                double w1 = dt_2 / (dt_1 + dt_2);
+                double w2 = dt_1 / (dt_1 + dt_2);
+                //sensor_msgs::ImuPtr theLastIMU(new sensor_msgs::Imu);
+                ImuData theLastIMU;
+                theLastIMU.linear_acceleration.x = w1 * vimuMsg.back().linear_acceleration.x + w2 * tmpimumsg.linear_acceleration.x;
+                theLastIMU.linear_acceleration.y = w1 * vimuMsg.back().linear_acceleration.y + w2 * tmpimumsg.linear_acceleration.y;
+                theLastIMU.linear_acceleration.z = w1 * vimuMsg.back().linear_acceleration.z + w2 * tmpimumsg.linear_acceleration.z;
+                theLastIMU.angular_velocity.x = w1 * vimuMsg.back().angular_velocity.x + w2 * tmpimumsg.angular_velocity.x;
+                theLastIMU.angular_velocity.y = w1 * vimuMsg.back().angular_velocity.y + w2 * tmpimumsg.angular_velocity.y;
+                theLastIMU.angular_velocity.z = w1 * vimuMsg.back().angular_velocity.z + w2 * tmpimumsg.angular_velocity.z;
+                theLastIMU.timebase=endTime;
+                vimuMsg.emplace_back(theLastIMU);
+                break;
+            }
+        }
+    }
+    return !vimuMsg.empty();
+}
+
+/** \brief Remove Lidar Distortion
+  * \param[in] cloud: lidar cloud need to be undistorted
+  * \param[in] dRlc: delta rotation
+  * \param[in] dtlc: delta displacement
+  */
+void Mapper::RemoveLidarDistortion(pcl::PointCloud<PointType>::Ptr& cloud,
+                           const Eigen::Matrix3d& dRlc, const Eigen::Vector3d& dtlc){
+    int PointsNum = cloud->points.size();
+    for (int i = 0; i < PointsNum; i++) {
+        Eigen::Vector3d startP;
+        float s = cloud->points[i].normal_x;
+        Eigen::Quaterniond qlc = Eigen::Quaterniond(dRlc).normalized();
+        Eigen::Quaterniond delta_qlc = Eigen::Quaterniond::Identity().slerp(s, qlc).normalized();
+        const Eigen::Vector3d delta_Plc = s * dtlc;
+        startP = delta_qlc * Eigen::Vector3d(cloud->points[i].x,cloud->points[i].y,cloud->points[i].z) + delta_Plc;
+        Eigen::Vector3d _po = dRlc.transpose() * (startP - dtlc);
+
+        cloud->points[i].x = _po(0);
+        cloud->points[i].y = _po(1);
+        cloud->points[i].z = _po(2);
+        cloud->points[i].normal_x = 1.0;
+    }
+}
+
+
+bool Mapper::TryMAPInitialization()
+{
+//雷达数据达到三帧
+    Eigen::Vector3d average_acc = -lidarFrameList->begin()->imuIntegrator.GetAverageAcc();
+    double info_g = std::fabs(9.805 - average_acc.norm());
+    average_acc = average_acc * 9.805 / average_acc.norm();
+
+    // calculate the initial gravity direction
+    double para_quat[4];
+    para_quat[0] = 1;
+    para_quat[1] = 0;
+    para_quat[2] = 0;
+    para_quat[3] = 0;
+
+    //优化imu初始坐标系与世界坐标系的旋转变换
+    ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization();
+    ceres::Problem problem_quat;
+
+    problem_quat.AddParameterBlock(para_quat, 4, quatParam);
+
+    //初始状态雷达未动， 初始加速度应该=g， imu坐标系与雷达坐标系之间的变换*初始加速度=g，建立误差模型
+    problem_quat.AddResidualBlock(Cost_Initial_G::Create(average_acc),
+                                  nullptr,
+                                  para_quat);
+
+    ceres::Solver::Options options_quat;
+    ceres::Solver::Summary summary_quat;
+    ceres::Solve(options_quat, &problem_quat, &summary_quat);
+
+    Eigen::Quaterniond q_wg(para_quat[0], para_quat[1], para_quat[2], para_quat[3]);
+
+
+    //build prior factor of LIO initialization
+    Eigen::Vector3d prior_r = Eigen::Vector3d::Zero();
+    Eigen::Vector3d prior_ba = Eigen::Vector3d::Zero();
+    Eigen::Vector3d prior_bg = Eigen::Vector3d::Zero();
+    std::vector<Eigen::Vector3d> prior_v;
+    int v_size = lidarFrameList->size();        //前三帧雷达数据
+    for (int i = 0; i < v_size; i++)
+    {
+        prior_v.push_back(Eigen::Vector3d::Zero());
+    }
+    //对初始旋转四元数做对数变换，得到旋转向量
+    Sophus::SO3d SO3_R_wg(q_wg.toRotationMatrix());
+    prior_r = SO3_R_wg.log();
+
+    for (int i = 1; i < v_size; i++)
+    {
+        auto iter = lidarFrameList->begin();
+        auto iter_next = lidarFrameList->begin();
+        std::advance(iter, i - 1);
+        std::advance(iter_next, i);
+
+        Eigen::Vector3d velo_imu = (iter_next->P - iter->P + iter_next->Q * exPlb - iter->Q * exPlb) / (iter_next->timeStamp - iter->timeStamp);
+        prior_v[i] = velo_imu;
+    }
+    prior_v[0] = prior_v[1];
+
+    //3帧数据的3轴速度
+    double para_v[v_size][3];
+    double para_r[3];
+    double para_ba[3];
+    double para_bg[3];
+
+    for (int i = 0; i < 3; i++)
+    {
+        para_r[i] = 0;
+        para_ba[i] = 0;
+        para_bg[i] = 0;
+    }
+
+    for (int i = 0; i < v_size; i++)
+    {
+        for (int j = 0; j < 3; j++)
+        {
+            para_v[i][j] = prior_v[i][j];
+        }
+    }
+
+    Eigen::Matrix<double, 3, 3> sqrt_information_r = 2000.0 * Eigen::Matrix<double, 3, 3>::Identity();
+    Eigen::Matrix<double, 3, 3> sqrt_information_ba = 1000.0 * Eigen::Matrix<double, 3, 3>::Identity();
+    Eigen::Matrix<double, 3, 3> sqrt_information_bg = 4000.0 * Eigen::Matrix<double, 3, 3>::Identity();
+    Eigen::Matrix<double, 3, 3> sqrt_information_v = 4000.0 * Eigen::Matrix<double, 3, 3>::Identity();
+
+    ceres::Problem::Options problem_options;
+    ceres::Problem problem(problem_options);
+    problem.AddParameterBlock(para_r, 3);
+    problem.AddParameterBlock(para_ba, 3);
+    problem.AddParameterBlock(para_bg, 3);
+    for (int i = 0; i < v_size; i++)
+    {
+        problem.AddParameterBlock(para_v[i], 3);
+    }
+
+    // add CostFunction
+    problem.AddResidualBlock(Cost_Initialization_Prior_R::Create(prior_r, sqrt_information_r),
+                             nullptr,
+                             para_r);
+
+    problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_ba, sqrt_information_ba),
+                             nullptr,
+                             para_ba);
+    problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_bg, sqrt_information_bg),
+                             nullptr,
+                             para_bg);
+
+    for (int i = 0; i < v_size; i++)
+    {
+        problem.AddResidualBlock(Cost_Initialization_Prior_bv::Create(prior_v[i], sqrt_information_v),
+                                 nullptr,
+                                 para_v[i]);
+    }
+
+    for (int i = 1; i < v_size; i++)
+    {
+        auto iter = lidarFrameList->begin();
+        auto iter_next = lidarFrameList->begin();
+        std::advance(iter, i - 1);
+        std::advance(iter_next, i);
+
+        Eigen::Vector3d pi = iter->P + iter->Q * exPlb;
+        Sophus::SO3d SO3_Ri(iter->Q * exRlb);
+        Eigen::Vector3d ri = SO3_Ri.log();
+        Eigen::Vector3d pj = iter_next->P + iter_next->Q * exPlb;
+        Sophus::SO3d SO3_Rj(iter_next->Q * exRlb);
+        Eigen::Vector3d rj = SO3_Rj.log();
+
+        problem.AddResidualBlock(Cost_Initialization_IMU::Create(iter_next->imuIntegrator,
+                                                                 ri,
+                                                                 rj,
+                                                                 pj - pi,
+                                                                 Eigen::LLT<Eigen::Matrix<double, 9, 9>>
+                                                                         (iter_next->imuIntegrator.GetCovariance().block<
+                                                                                 9,
+                                                                                 9>(0, 0).inverse())
+                                                                         .matrixL().transpose()),
+                                 nullptr,
+                                 para_r,
+                                 para_v[i - 1],
+                                 para_v[i],
+                                 para_ba,
+                                 para_bg);
+    }
+
+    ceres::Solver::Options options;
+    options.minimizer_progress_to_stdout = false;
+    options.linear_solver_type = ceres::DENSE_QR;
+    options.num_threads = 6;
+    ceres::Solver::Summary summary;
+    ceres::Solve(options, &problem, &summary);
+
+    Eigen::Vector3d r_wg(para_r[0], para_r[1], para_r[2]);
+    GravityVector = Sophus::SO3d::exp(r_wg) * Eigen::Vector3d(0, 0, -9.805);
+
+    Eigen::Vector3d ba_vec(para_ba[0], para_ba[1], para_ba[2]);
+    Eigen::Vector3d bg_vec(para_bg[0], para_bg[1], para_bg[2]);
+
+    if (ba_vec.norm() > 0.5 || bg_vec.norm() > 0.5)
+    {
+        printf("Too Large Biases! Initialization Failed!\n");
+        return false;
+    }
+
+    for (int i = 0; i < v_size; i++)
+    {
+        auto iter = lidarFrameList->begin();
+        std::advance(iter, i);
+        iter->ba = ba_vec;
+        iter->bg = bg_vec;
+        Eigen::Vector3d bv_vec(para_v[i][0], para_v[i][1], para_v[i][2]);
+        if ((bv_vec - prior_v[i]).norm() > 2.0)
+        {
+            printf("Too Large Velocity! Initialization Failed!\n");
+            std::cout << "delta v norm: " << (bv_vec - prior_v[i]).norm() << std::endl;
+            return false;
+        }
+        iter->V = bv_vec;
+    }
+
+    for (size_t i = 0; i < v_size - 1; i++)
+    {
+        auto laser_trans_i = lidarFrameList->begin();
+        auto laser_trans_j = lidarFrameList->begin();
+        std::advance(laser_trans_i, i);
+        std::advance(laser_trans_j, i + 1);
+        laser_trans_j->imuIntegrator.PreIntegration(laser_trans_i->timeStamp, laser_trans_i->bg, laser_trans_i->ba);
+    }
+
+
+    // //if IMU success initialized，保留2帧最近的数据。
+    WINDOWSIZE = Estimator::SLIDEWINDOWSIZE;
+    while (lidarFrameList->size() > WINDOWSIZE)
+    {
+        lidarFrameList->pop_front();
+    }
+    Eigen::Vector3d Pwl = lidarFrameList->back().P;
+    Eigen::Quaterniond Qwl = lidarFrameList->back().Q;
+    lidarFrameList->back().P = Pwl + Qwl * exPlb;
+    lidarFrameList->back().Q = Qwl * exRlb;
+
+    // std::cout << "\n=============================\n| Initialization Successful |"<<"\n=============================\n" << std::endl;
+
+    return true;
+}
+
+
+pcl::PointCloud<PointType>::Ptr Mapper::GetMapCorner()
+{
+    return estimator->m_global_map.laserCloudCornerFromLocal;
+}
+pcl::PointCloud<PointType>::Ptr Mapper::GetMapSurf()
+{
+    return estimator->m_global_map.laserCloudSurfFromLocal;
+}
+pcl::PointCloud<PointType>::Ptr Mapper::GetMapNonf()
+{
+    return estimator->m_global_map.laserCloudNonFeatureFromLocal;
+}
+
+pcl::PointCloud<PointType>::Ptr Mapper::GetMapFull()
+{
+    return estimator->m_global_map.full_cloud();
+}
+
+void Mapper::SetInitPose(Eigen::Matrix4d transform)
+{
+    transformAftMapped=transform;
+}
+
+/** \brief Mapping main thread
+  */
+void Mapper::process()
+{
+    double time_last_lidar = -1;
+    double time_curr_lidar = -1;
+    Eigen::Matrix3d delta_Rl = Eigen::Matrix3d::Identity();
+    Eigen::Vector3d delta_tl = Eigen::Vector3d::Zero();
+    Eigen::Matrix3d delta_Rb = Eigen::Matrix3d::Identity();
+    Eigen::Vector3d delta_tb = Eigen::Vector3d::Zero();
+    std::vector<ImuData> vimuMsg;
+
+
+
+    while (exit_==false)
+    {
+
+        auto start = std::chrono::system_clock::now();
+        newfullCloud = false;
+        pcl::PointCloud<PointType>::Ptr cloudFull(new pcl::PointCloud<PointType>());
+        std::unique_lock<std::mutex> lock_lidar(_mutexLidarQueue);
+        if (!_lidarMsgQueue.empty())
+        {
+            // get new lidar msg
+            time_curr_lidar = _lidarMsgQueue.front().timebase;
+            cloudFull=_lidarMsgQueue.front().cloud;
+            _lidarMsgQueue.pop();
+            newfullCloud = true;
+        }
+        lock_lidar.unlock();
+
+        if (newfullCloud == false)
+        {
+            continue;
+        }
+
+        if (IMU_Mode > 0 && time_last_lidar > 0)
+        {
+            // get IMU msg int the Specified time interval
+            //第二帧雷达数据以后，进入这里,雷达已经初始化，第一帧雷达imu预积分器使用默认值
+            vimuMsg.clear();
+            int countFail = 0;
+            //将上一帧雷达数据时间点到当前时间点的imu数据从queue中取出
+            while (!fetchImuMsgs(time_last_lidar, time_curr_lidar, vimuMsg))
+            {
+                countFail++;
+                if (countFail > 100)
+                {
+                    break;
+                }
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            }
+
+        }
+        // this lidar frame init
+        Estimator::LidarFrame lidarFrame;
+        lidarFrame.laserCloud = cloudFull;
+        lidarFrame.timeStamp = time_curr_lidar;
+
+        boost::shared_ptr<std::list<Estimator::LidarFrame>> lidar_list;
+        //计算当前雷达数据的imu里程计
+        if (!vimuMsg.empty())
+        {
+            if (!LidarIMUInited)  //imu未初始化（前20帧数据）
+            {
+                // if get IMU msg successfully, use gyro integration to update delta_Rl
+                lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
+                lidarFrame.imuIntegrator.GyroIntegration(time_last_lidar);
+                //imu坐在世界坐标系下的旋转量
+                delta_Rb = lidarFrame.imuIntegrator.GetDeltaQ().toRotationMatrix();
+                //
+                delta_Rl = exTlb.topLeftCorner(3, 3) * delta_Rb * exTlb.topLeftCorner(3, 3).transpose();
+
+                // predict current lidar pose
+                lidarFrame.P = transformAftMapped.topLeftCorner(3, 3) * delta_tb
+                        + transformAftMapped.topRightCorner(3, 1);
+                Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3, 3) * delta_Rb;
+                lidarFrame.Q = m3d;
+
+                lidar_list.reset(new std::list<Estimator::LidarFrame>);
+                lidar_list->push_back(lidarFrame);
+            }
+            else   //imu已经初始化，此时WINDOWSIZE=2，lidarFrameList->size=2
+            {
+                // if get IMU msg successfully, use pre-integration to update delta lidar pose
+                //此时的imuIntegrator为默认imuIntegrator，使用上一帧雷达时刻的imu ba和bg预积分
+                lidarFrame.imuIntegrator.PushIMUMsg(vimuMsg);
+                lidarFrame.imuIntegrator.PreIntegration(lidarFrameList->back().timeStamp,
+                                                        lidarFrameList->back().bg,
+                                                        lidarFrameList->back().ba);
+
+                //上一时刻的雷达状态，（位姿、速度）
+                const Eigen::Vector3d &Pwbpre = lidarFrameList->back().P;
+                const Eigen::Quaterniond &Qwbpre = lidarFrameList->back().Q;
+                const Eigen::Vector3d &Vwbpre = lidarFrameList->back().V;
+
+                //获取当前帧到上一帧的状态差（dp，dq，dv）
+                const Eigen::Quaterniond &dQ = lidarFrame.imuIntegrator.GetDeltaQ();
+                const Eigen::Vector3d &dP = lidarFrame.imuIntegrator.GetDeltaP();
+                const Eigen::Vector3d &dV = lidarFrame.imuIntegrator.GetDeltaV();
+                double dt = lidarFrame.imuIntegrator.GetDeltaTime();
+
+                //更新当前帧的状态，赋值当前帧的ba和bg为上一帧的ba、bg。
+                lidarFrame.Q = Qwbpre * dQ;
+                lidarFrame.P = Pwbpre + Vwbpre * dt + 0.5 * GravityVector * dt * dt + Qwbpre * (dP);
+                lidarFrame.V = Vwbpre + GravityVector * dt + Qwbpre * (dV);
+                lidarFrame.bg = lidarFrameList->back().bg;
+                lidarFrame.ba = lidarFrameList->back().ba;
+
+                //上一雷达时刻imu位姿
+                Eigen::Quaterniond Qwlpre = Qwbpre * Eigen::Quaterniond(exRbl);
+                Eigen::Vector3d Pwlpre = Qwbpre * exPbl + Pwbpre;
+
+                //当前雷达时刻的imu位姿
+                Eigen::Quaterniond Qwl = lidarFrame.Q * Eigen::Quaterniond(exRbl);
+                Eigen::Vector3d Pwl = lidarFrame.Q * exPbl + lidarFrame.P;
+
+                //两时刻imu位姿差
+                delta_Rl = Qwlpre.conjugate() * Qwl;
+                delta_tl = Qwlpre.conjugate() * (Pwl - Pwlpre);
+                delta_Rb = dQ.toRotationMatrix();
+                delta_tb = dP;
+
+                lidarFrameList->push_back(lidarFrame);
+                lidarFrameList->pop_front();
+                lidar_list = lidarFrameList;
+            }
+        }
+        else   //imu队列无数据(第一帧雷达数据先到）
+        {
+            //imu无数据，但已经初始化过， 表示imu出现中断， break，退出程序
+            if (LidarIMUInited)
+                break;
+            else
+            {   //第一帧数据
+                // predict current lidar pose
+                lidarFrame.P = transformAftMapped.topLeftCorner(3, 3) * delta_tb
+                        + transformAftMapped.topRightCorner(3, 1);
+                Eigen::Matrix3d m3d = transformAftMapped.topLeftCorner(3, 3) * delta_Rb;
+                lidarFrame.Q = m3d;
+
+                lidar_list.reset(new std::list<Estimator::LidarFrame>);
+                lidar_list->push_back(lidarFrame);
+            }
+        }
+
+        // remove lidar distortion
+        //根据imu里程计（当前帧与上一帧的差），消除点云运动畸变
+
+
+        RemoveLidarDistortion(cloudFull, delta_Rl, delta_tl);
+
+        // optimize current lidar pose with IMU,
+        //printf("window size :%d  lidar list size %d---\n",WINDOWSIZE,lidar_list->size());
+
+        estimator->EstimateLidarPose(*lidar_list, exTlb, GravityVector);
+
+        pcl::PointCloud<PointType>::Ptr laserCloudCornerMap(new pcl::PointCloud<PointType>());
+        pcl::PointCloud<PointType>::Ptr laserCloudSurfMap(new pcl::PointCloud<PointType>());
+
+
+        Eigen::Matrix4d transformTobeMapped = Eigen::Matrix4d::Identity();
+        transformTobeMapped.topLeftCorner(3, 3) = lidar_list->front().Q * exRbl;
+        transformTobeMapped.topRightCorner(3, 1) = lidar_list->front().Q * exPbl + lidar_list->front().P;
+
+        // update delta transformation
+        delta_Rb = transformAftMapped.topLeftCorner(3, 3).transpose() * lidar_list->front().Q.toRotationMatrix();
+        delta_tb = transformAftMapped.topLeftCorner(3,
+                                                    3).transpose() * (lidar_list->front().P - transformAftMapped.topRightCorner(
+                3,
+                1));
+        Eigen::Matrix3d Rwlpre = transformAftMapped.topLeftCorner(3, 3) * exRbl;
+        Eigen::Vector3d Pwlpre = transformAftMapped.topLeftCorner(3, 3) * exPbl + transformAftMapped.topRightCorner(
+                3,
+                1);
+        delta_Rl = Rwlpre.transpose() * transformTobeMapped.topLeftCorner(3, 3);
+        delta_tl = Rwlpre.transpose() * (transformTobeMapped.topRightCorner(3, 1) - Pwlpre);
+        transformAftMapped.topLeftCorner(3, 3) = lidar_list->front().Q.toRotationMatrix();
+        transformAftMapped.topRightCorner(3, 1) = lidar_list->front().P;
+
+        // publish odometry rostopic
+        if (odomcallback_!= nullptr)
+        {
+            odomcallback_(transformTobeMapped,lidar_list->front().timeStamp);
+        }
+
+        // publish lidar points
+        if(cloudMappedcallback_!= nullptr)
+        {
+            int laserCloudFullResNum = lidar_list->front().laserCloud->points.size();
+            pcl::PointCloud<PointType>::Ptr laserCloudAfterEstimate(new pcl::PointCloud<PointType>());
+            laserCloudAfterEstimate->reserve(laserCloudFullResNum);
+            for (int i = 0; i < laserCloudFullResNum; i++)
+            {
+                PointType temp_point;
+                LocalMapIvox::pointAssociateToMap(&lidar_list->front().laserCloud->points[i],
+                                                 &temp_point,
+                                                 transformTobeMapped);
+                laserCloudAfterEstimate->push_back(temp_point);
+            }
+            cloudMappedcallback_(laserCloudAfterEstimate,lidar_list->front().timeStamp);
+        }
+
+        if( cloudLocalMappcallback_!= nullptr)
+        {
+
+            pcl::PointCloud<PointType> all;
+
+            all+=estimator->local_corner_cloud_;
+            all+=estimator->local_surf_cloud_;
+            all+=estimator->local_nonf_cloud_;
+
+            cloudLocalMappcallback_(all.makeShared(),lidar_list->front().timeStamp);
+        }
+
+        // if tightly coupled IMU message, start IMU initialization
+        if (IMU_Mode > 1 && !LidarIMUInited)
+        {
+            // update lidar frame pose
+            lidarFrame.P = transformTobeMapped.topRightCorner(3, 1);
+            Eigen::Matrix3d m3d = transformTobeMapped.topLeftCorner(3, 3);
+            lidarFrame.Q = m3d;
+
+            // static int pushCount = 0;
+            if (pushCount == 0)
+            {
+                lidarFrameList->push_back(lidarFrame);
+                lidarFrameList->back().imuIntegrator.Reset();
+                if (lidarFrameList->size() > WINDOWSIZE)
+                    lidarFrameList->pop_front();
+            }
+            else
+            {
+                lidarFrameList->back().laserCloud = lidarFrame.laserCloud;
+                lidarFrameList->back().imuIntegrator.PushIMUMsg(vimuMsg);
+                lidarFrameList->back().timeStamp = lidarFrame.timeStamp;
+                lidarFrameList->back().P = lidarFrame.P;
+                lidarFrameList->back().Q = lidarFrame.Q;
+            }
+            pushCount++;
+            if (pushCount >= 3)
+            {
+                pushCount = 0;
+                if (lidarFrameList->size() > 1)
+                {
+                    auto iterRight = std::prev(lidarFrameList->end());
+                    auto iterLeft = std::prev(std::prev(lidarFrameList->end()));
+                    iterRight->imuIntegrator.PreIntegration(iterLeft->timeStamp, iterLeft->bg, iterLeft->ba);
+                }
+                //记录前2/3 windowsize的数据时间点， 此时间点之前的数据不参与imu初始化，初始化完成后置WINDOWSIE=2
+                if (lidarFrameList->size() == int(WINDOWSIZE / 1.5))
+                {
+                    startTime = lidarFrameList->back().timeStamp;
+                }
+
+                if (!LidarIMUInited && lidarFrameList->size() == WINDOWSIZE && lidarFrameList->front().timeStamp >= startTime)
+                {
+                    std::cout << "**************Start MAP Initialization!!!******************" << std::endl;
+                    if (TryMAPInitialization())
+                    {
+                        LidarIMUInited = true;
+                        pushCount = 0;
+                        startTime = 0;
+                    }
+                    std::cout << "**************Finish MAP Initialization!!!******************" << std::endl;
+                }
+
+            }
+        }
+        time_last_lidar = time_curr_lidar;
+        auto end = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        double tm=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+
+
+        printf("local map size(%d,%d,%d)  time:%.5fs  cloud queue size :%d\n",
+               estimator->local_corner_cloud_.size(),
+        estimator->local_surf_cloud_.size(),
+        estimator->local_nonf_cloud_.size(),
+        tm,_lidarMsgQueue.size());
+
+    }
+}
+
+void Mapper::LoadMap(std::string file)
+{
+    estimator->m_global_map.load_map(file);
+}

lio/src/lio/mapper.h

@@ -0,0 +1,83 @@
+//
+// Created by zx on 22-11-1.
+//
+
+#ifndef SRC_LIO_LIVOX_SRC_LIO_MAPPER_H_
+#define SRC_LIO_LIVOX_SRC_LIO_MAPPER_H_
+
+#include "Estimator.h"
+
+#include "typedef.h"
+
+
+class Mapper
+{
+ public:
+    Mapper(int wndsize,float filter_coner,float filter_surf,
+           int ivox_nearby_type,int max_ivox_node,float resolution);
+    ~Mapper();
+    void InitRT(const Eigen::Matrix3d& R,const Eigen::Vector3d& T);
+    void AddImu(const ImuData& imu);
+    void AddCloud(const TimeCloud& cloud);
+
+
+    void SetOdomCallback(odomCallback callback);
+    void SetCloudMappedCallback(CloudMappedCallback callback);
+
+    void SetLocalMapCloudCallback(CloudMappedCallback callback);
+
+    bool fetchImuMsgs(double startTime, double endTime, std::vector<ImuData> &vimuMsg);
+
+    void RemoveLidarDistortion(pcl::PointCloud<PointType>::Ptr& cloud,
+                               const Eigen::Matrix3d& dRlc, const Eigen::Vector3d& dtlc);
+
+    bool TryMAPInitialization();
+
+    void process();
+    void completed();
+    void LoadMap(std::string file);
+    pcl::PointCloud<PointType>::Ptr GetMapFull();
+    pcl::PointCloud<PointType>::Ptr GetMapCorner();
+    pcl::PointCloud<PointType>::Ptr GetMapSurf();
+    pcl::PointCloud<PointType>::Ptr GetMapNonf();
+    void SetInitPose(Eigen::Matrix4d transform);
+
+
+ public:
+    int WINDOWSIZE;
+ protected:
+    bool exit_;
+    std::thread thread_;
+
+    odomCallback odomcallback_;
+    CloudMappedCallback cloudMappedcallback_;
+    CloudMappedCallback cloudLocalMappcallback_;
+
+
+    bool LidarIMUInited = false;
+    boost::shared_ptr<std::list<Estimator::LidarFrame>> lidarFrameList;
+    Estimator* estimator;
+
+    bool newfullCloud = false;
+
+    Eigen::Matrix4d transformAftMapped = Eigen::Matrix4d::Identity();
+
+    std::mutex _mutexLidarQueue;
+
+    std::queue<TimeCloud> _lidarMsgQueue;
+    std::mutex _mutexIMUQueue;
+    std::queue<ImuData> _imuMsgQueue;
+    Eigen::Matrix4d exTlb;      //雷达到imu变换
+    Eigen::Matrix3d exRlb, exRbl;
+    Eigen::Vector3d exPlb, exPbl;
+    Eigen::Vector3d GravityVector;
+    float filter_parameter_corner = 0.2;
+    float filter_parameter_surf = 0.4;
+    int IMU_Mode = 2;
+    int pushCount = 0;
+    double startTime = 0;
+
+};
+
+
+#endif //SRC_LIO_LIVOX_SRC_LIO_MAPPER_H_

lio/src/pathcreator.cpp

@@ -0,0 +1,94 @@
+#include "pathcreator.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <stdint.h>
+#include <stdio.h>
+
+PathCreator::PathCreator()
+{
+
+}
+
+PathCreator::~PathCreator()
+{
+
+}
+
+std::string PathCreator::GetCurPath()
+{
+    return m_current_path;
+}
+bool PathCreator::Mkdir(std::string dirName)
+{
+    uint32_t beginCmpPath = 0;
+    uint32_t endCmpPath = 0;
+    std::string fullPath = "";
+
+        if('/' != dirName[0])
+        {
+            fullPath = getcwd(nullptr, 0);
+            beginCmpPath = fullPath.size();
+            fullPath = fullPath + "/" + dirName;
+        }
+        else
+        {
+            //Absolute path
+            fullPath = dirName;
+            beginCmpPath = 1;
+        }
+        if (fullPath[fullPath.size() - 1] != '/')
+        {
+            fullPath += "/";
+        }
+        endCmpPath = fullPath.size();
+
+        //create dirs;
+        for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+        {
+            if('/' == fullPath[i])
+            {
+                std::string curPath = fullPath.substr(0, i);
+                if(access(curPath.c_str(), F_OK) != 0)
+                {
+                    if(mkdir(curPath.c_str(), /*S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH*/0777) == -1)
+                    {
+                        printf("mkdir(%s) failed\n", curPath.c_str());
+                        return false;
+                    }
+                }
+            }
+        }
+        m_current_path=fullPath;
+        return true;
+
+}
+
+bool PathCreator::CreateDatePath(std::string root, bool add_time)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    if (add_time)
+    {
+        sprintf(buf, "%s/%d%02d%02d-%02d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday,
+                t->tm_hour,
+                t->tm_min,
+                t->tm_sec);
+    }
+    else
+    {
+        sprintf(buf, "%s/%d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday);
+    }
+    return Mkdir(buf);
+}

lio/src/pathcreator.h

@@ -0,0 +1,17 @@
+#ifndef PATHCREATOR_H
+#define PATHCREATOR_H
+#include <string>
+
+class PathCreator
+{
+public:
+    PathCreator();
+    ~PathCreator();
+    std::string GetCurPath();
+    bool Mkdir(std::string dir);
+    bool CreateDatePath(std::string root, bool add_time = true);
+protected:
+    std::string m_current_path;
+};
+
+#endif // PATHCREATOR_H

lio/src/segment/LidarFeatureExtractor.h

@@ -0,0 +1,101 @@
+#ifndef LIO_LIVOX_LIDARFEATUREEXTRACTOR_H
+#define LIO_LIVOX_LIDARFEATUREEXTRACTOR_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"
+#include "typedef.h"
+class LidarFeatureExtractor{
+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(pcl::PointCloud<PointType>::Ptr& laserCloud,
+                                     pcl::PointCloud<PointType>::Ptr& laserConerFeature,
+                                     pcl::PointCloud<PointType>::Ptr& laserSurfFeature,
+                                     pcl::PointCloud<PointType>::Ptr& laserNonFeature,
+                                     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(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

lio/src/segment/pointsCorrect.cpp

@@ -0,0 +1,326 @@
+#include "pointsCorrect.hpp"
+
+float gnd_pos[6];
+int frame_count = 0;
+int frame_lenth_threshold = 5;//5 frames update
+
+int GetNeiborPCA_cor(SNeiborPCA_cor &npca, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::KdTreeFLANN<pcl::PointXYZ> kdtree, pcl::PointXYZ searchPoint, float fSearchRadius)
+{
+    std::vector<float> k_dis;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr subCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    if(kdtree.radiusSearch(searchPoint,fSearchRadius,npca.neibors,k_dis)>5)
+    {
+        subCloud->width=npca.neibors.size();
+        subCloud->height=1;
+        subCloud->points.resize(subCloud->width*subCloud->height);
+
+        for (int pid=0;pid<subCloud->points.size();pid++)//搜索半径内的地面点云 sy
+        {
+            subCloud->points[pid].x=cloud->points[npca.neibors[pid]].x;
+            subCloud->points[pid].y=cloud->points[npca.neibors[pid]].y;
+            subCloud->points[pid].z=cloud->points[npca.neibors[pid]].z;
+        }
+        //利用PCA主元分析法获得点云的三个主方向，获取质心，计算协方差，获得协方差矩阵，求取协方差矩阵的特征值和特长向量，特征向量即为主方向。 sy
+        Eigen::Vector4f pcaCentroid;
+    	pcl::compute3DCentroid(*subCloud, pcaCentroid);
+	    Eigen::Matrix3f covariance;
+	    pcl::computeCovarianceMatrixNormalized(*subCloud, pcaCentroid, covariance);
+	    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
+	    npca.eigenVectorsPCA = eigen_solver.eigenvectors();
+	    npca.eigenValuesPCA = eigen_solver.eigenvalues();
+        float vsum=npca.eigenValuesPCA(0)+npca.eigenValuesPCA(1)+npca.eigenValuesPCA(2);
+        npca.eigenValuesPCA(0)=npca.eigenValuesPCA(0)/(vsum+0.000001);//单位化 sy
+        npca.eigenValuesPCA(1)=npca.eigenValuesPCA(1)/(vsum+0.000001);
+        npca.eigenValuesPCA(2)=npca.eigenValuesPCA(2)/(vsum+0.000001);
+    }
+    else
+    {
+        npca.neibors.clear();
+    }
+    //std::cout << "in PCA2\n";
+    return npca.neibors.size();
+}
+
+int FilterGndForPos_cor(float* outPoints,float*inPoints,int inNum)
+{
+    int outNum=0;
+    float dx=2;
+    float dy=2;
+    int x_len = 20;
+    int y_len = 10;
+    int nx=2*x_len/dx; //80
+    int ny=2*y_len/dy; //10
+    float offx=-20,offy=-10;
+    float THR=0.4;
+    
+
+    float *imgMinZ=(float*)calloc(nx*ny,sizeof(float));
+    float *imgMaxZ=(float*)calloc(nx*ny,sizeof(float));
+    float *imgSumZ=(float*)calloc(nx*ny,sizeof(float));
+    float *imgMeanZ=(float*)calloc(nx*ny,sizeof(float));
+    int *imgNumZ=(int*)calloc(nx*ny,sizeof(int));
+    int *idtemp = (int*)calloc(inNum,sizeof(int));
+    for(int ii=0;ii<nx*ny;ii++)
+    {
+        imgMinZ[ii]=10;
+        imgMaxZ[ii]=-10;
+        imgMeanZ[ii] = -10;
+        imgSumZ[ii]=0;
+        imgNumZ[ii]=0;
+    }
+
+    for(int pid=0;pid<inNum;pid++)
+    {
+        idtemp[pid] = -1;
+        if((inPoints[pid*4] > -x_len) && (inPoints[pid*4]<x_len)&&(inPoints[pid*4+1]>-y_len)&&(inPoints[pid*4+1]<y_len))
+        {
+            int idx=(inPoints[pid*4]-offx)/dx;
+            int idy=(inPoints[pid*4+1]-offy)/dy;
+            idtemp[pid] = idx+idy*nx;
+            if (idtemp[pid] >= nx*ny)
+                continue;
+            imgSumZ[idx+idy*nx] += inPoints[pid*4+2];
+            imgNumZ[idx+idy*nx] +=1;
+            if(inPoints[pid*4+2]<imgMinZ[idx+idy*nx])
+            {
+                imgMinZ[idx+idy*nx]=inPoints[pid*4+2];
+            }
+            if(inPoints[pid*4+2]>imgMaxZ[idx+idy*nx]){
+                imgMaxZ[idx+idy*nx]=inPoints[pid*4+2];
+            }
+        }
+    }
+    for(int pid=0;pid<inNum;pid++)
+    {
+        if (outNum >= 60000)
+            break;
+        if(idtemp[pid] > 0 && idtemp[pid] < nx*ny)
+        {
+            imgMeanZ[idtemp[pid]] = float(imgSumZ[idtemp[pid]]/(imgNumZ[idtemp[pid]] + 0.0001));
+            //最高点与均值高度差小于阈值；点数大于3；均值高度小于1 
+            if((imgMaxZ[idtemp[pid]] - imgMeanZ[idtemp[pid]]) < THR && imgNumZ[idtemp[pid]] > 3 && imgMeanZ[idtemp[pid]] < 2)
+            {// imgMeanZ[idtemp[pid]]<0&&
+                outPoints[outNum*4]=inPoints[pid*4];
+                outPoints[outNum*4+1]=inPoints[pid*4+1];
+                outPoints[outNum*4+2]=inPoints[pid*4+2];
+                outPoints[outNum*4+3]=inPoints[pid*4+3];
+                outNum++;
+            }
+        }
+    }
+
+    free(imgMinZ);
+    free(imgMaxZ);
+    free(imgSumZ);
+    free(imgMeanZ);
+    free(imgNumZ);
+    free(idtemp);
+    return outNum;
+}
+
+int CalGndPos_cor(float *gnd, float *fPoints,int pointNum,float fSearchRadius)
+{
+    // 初始化gnd
+    for(int ii=0;ii<6;ii++)
+    {
+        gnd[ii]=0;
+    }
+    // 转换点云到pcl的格式
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    //去除异常点
+    if (pointNum <= 3)
+    {
+        return 0;
+    }
+    cloud->width=pointNum;
+    cloud->height=1;
+    cloud->points.resize(cloud->width*cloud->height);
+
+    for (int pid=0;pid<cloud->points.size();pid++)
+    {
+        cloud->points[pid].x=fPoints[pid*4];
+        cloud->points[pid].y=fPoints[pid*4+1];
+        cloud->points[pid].z=fPoints[pid*4+2];
+    }
+    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
+    kdtree.setInputCloud (cloud);
+    int nNum=0;
+    unsigned char* pLabel = (unsigned char*)calloc(pointNum,sizeof(unsigned char));
+    for(int pid=0;pid<pointNum;pid++)
+    {
+        if ((nNum<1000)&&(pLabel[pid]==0))
+        {
+            SNeiborPCA_cor npca;
+            pcl::PointXYZ searchPoint;
+            searchPoint.x=cloud->points[pid].x;
+            searchPoint.y=cloud->points[pid].y;
+            searchPoint.z=cloud->points[pid].z;
+
+            if(GetNeiborPCA_cor(npca,cloud,kdtree,searchPoint,fSearchRadius)>0)
+            {
+                for(int ii=0;ii<npca.neibors.size();ii++)
+                {
+                    pLabel[npca.neibors[ii]]=1;
+                }
+
+                if(npca.eigenValuesPCA[1]/(npca.eigenValuesPCA[0] + 0.00001)>5000){ //指的是主方向与次方向差异较大。即这一小块接近平面 sy
+
+                        if(npca.eigenVectorsPCA(2,0)>0) //垂直向上？
+                        {
+                            gnd[0]+=npca.eigenVectorsPCA(0,0);
+                            gnd[1]+=npca.eigenVectorsPCA(1,0);
+                            gnd[2]+=npca.eigenVectorsPCA(2,0);
+
+                            gnd[3]+=searchPoint.x;
+                            gnd[4]+=searchPoint.y;
+                            gnd[5]+=searchPoint.z;
+                        }
+                        else
+                        {
+                            gnd[0]+=-npca.eigenVectorsPCA(0,0);
+                            gnd[1]+=-npca.eigenVectorsPCA(1,0);
+                            gnd[2]+=-npca.eigenVectorsPCA(2,0);
+
+                            gnd[3]+=searchPoint.x;
+                            gnd[4]+=searchPoint.y;
+                            gnd[5]+=searchPoint.z;
+                        }
+                        nNum++;
+
+                }
+            }
+        }
+    }
+    free(pLabel);
+    if(nNum>0)
+    {
+        for(int ii=0;ii<6;ii++)
+        {
+            gnd[ii]/=nNum; //平均法向量 & 地面点云的中心
+        }
+        if(abs(gnd[0])<0.1){
+            gnd[0]=gnd[0]*(1-abs(gnd[0]))*4.5;
+        }
+        else if(abs(gnd[0])<0.2){
+            gnd[0]=gnd[0]*(1-abs(gnd[0]))*3.2;
+        }
+        else{
+            gnd[0]=gnd[0]*(1-abs(gnd[0]))*2.8;
+        }
+        gnd[1] = gnd[1]*2.3;
+        
+    }
+    return nNum;
+}
+
+int GetRTMatrix_cor(float *RTM, float *v0, float *v1)
+{
+    // 归一化
+    float nv0=sqrt(v0[0]*v0[0]+v0[1]*v0[1]+v0[2]*v0[2]);
+    v0[0]/=(nv0+0.000001);
+    v0[1]/=(nv0+0.000001);
+    v0[2]/=(nv0+0.000001);
+
+    float nv1=sqrt(v1[0]*v1[0]+v1[1]*v1[1]+v1[2]*v1[2]);
+    v1[0]/=(nv1+0.000001);
+    v1[1]/=(nv1+0.000001);
+    v1[2]/=(nv1+0.000001);
+
+    // 叉乘
+    float v2[3];
+    v2[0]=v0[1]*v1[2]-v0[2]*v1[1];
+    v2[1]=v0[2]*v1[0]-v0[0]*v1[2];
+    v2[2]=v0[0]*v1[1]-v0[1]*v1[0];
+
+    // 正余弦
+    float cosAng=0,sinAng=0;
+    cosAng=v0[0]*v1[0]+v0[1]*v1[1]+v0[2]*v1[2];
+    sinAng=sqrt(1-cosAng*cosAng);
+
+    // 计算旋转矩阵
+    RTM[0]=v2[0]*v2[0]*(1-cosAng)+cosAng;
+    RTM[4]=v2[1]*v2[1]*(1-cosAng)+cosAng;
+    RTM[8]=v2[2]*v2[2]*(1-cosAng)+cosAng;
+
+    RTM[1]=RTM[3]=v2[0]*v2[1]*(1-cosAng);
+    RTM[2]=RTM[6]=v2[0]*v2[2]*(1-cosAng);
+    RTM[5]=RTM[7]=v2[1]*v2[2]*(1-cosAng);
+
+    RTM[1]+=(v2[2])*sinAng;
+    RTM[2]+=(-v2[1])*sinAng;
+    RTM[3]+=(-v2[2])*sinAng;
+
+    RTM[5]+=(v2[0])*sinAng;
+    RTM[6]+=(v2[1])*sinAng;
+    RTM[7]+=(-v2[0])*sinAng;
+
+    return 0;
+}
+
+int CorrectPoints_cor(float *fPoints,int pointNum,float *gndPos)
+{
+    float RTM[9];
+    float gndHeight=0;
+    float znorm[3]={0,0,1};
+    float tmp[3];
+
+    GetRTMatrix_cor(RTM,gndPos,znorm);
+
+    gndHeight = RTM[2]*gndPos[3]+RTM[5]*gndPos[4]+RTM[8]*gndPos[5];
+
+    for(int pid=0;pid<pointNum;pid++)
+    {
+        tmp[0]=RTM[0]*fPoints[pid*4]+RTM[3]*fPoints[pid*4+1]+RTM[6]*fPoints[pid*4+2];
+        tmp[1]=RTM[1]*fPoints[pid*4]+RTM[4]*fPoints[pid*4+1]+RTM[7]*fPoints[pid*4+2];
+        tmp[2]=RTM[2]*fPoints[pid*4]+RTM[5]*fPoints[pid*4+1]+RTM[8]*fPoints[pid*4+2]-gndHeight;
+
+        fPoints[pid*4]=tmp[0];
+        fPoints[pid*4+1]=tmp[1];
+        fPoints[pid*4+2]=tmp[2];
+    }
+    return 0;
+}
+
+
+int GetGndPos(float *pos, float *fPoints,int pointNum){
+    float *fPoints3=(float*)calloc(60000*4,sizeof(float));//地面点
+    int pnum3 = FilterGndForPos_cor(fPoints3,fPoints,pointNum);
+    float tmpPos[6];
+    if (pnum3 < 3)
+    {
+        std::cout << "too few ground points!\n";
+    }
+    int gndnum = CalGndPos_cor(tmpPos,fPoints3,pnum3,1.0);//用法向量判断，获取到法向量 & 地面搜索点，放到tmppos
+    if(gnd_pos[5]==0){
+        memcpy(gnd_pos,tmpPos,sizeof(tmpPos));
+    }
+    else{
+
+        if(frame_count<frame_lenth_threshold&&tmpPos[5]!=0){
+            if(gndnum>0&&abs(gnd_pos[0]-tmpPos[0])<0.1&&abs(gnd_pos[1]-tmpPos[1])<0.1){//更新法向量            
+                for(int i = 0;i<6;i++){
+                    gnd_pos[i] = (gnd_pos[i]+tmpPos[i])*0.5;
+                }
+                frame_count = 0;
+            }
+            else{
+                frame_count++;
+            }
+        }
+        else if(tmpPos[5]!=0){
+            memcpy(gnd_pos,tmpPos,sizeof(tmpPos));
+            frame_count = 0;
+        }
+    }
+   
+    memcpy(pos,gnd_pos,sizeof(float)*6);
+
+    free(fPoints3);
+    
+    return 0;
+}
+
+
+

lio/src/segment/pointsCorrect.hpp

@@ -0,0 +1,33 @@
+#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

lio/src/segment/segment.hpp

@@ -0,0 +1,162 @@
+#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

lio/src/utils/TimerRecord.h

@@ -0,0 +1,72 @@
+//
+// Created by zx on 22-11-3.
+//
+
+#ifndef SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_H_
+#define SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_H_
+
+#include <map>
+#include <string>
+#include <vector>
+#include <chrono>
+#include <numeric>
+#include <iostream>
+
+struct FuncRecord
+{
+ public:
+    FuncRecord()=default;
+    FuncRecord(std::string name,double time)
+    {
+        name_=name;
+        timesqueue_.emplace_back(time);
+    }
+
+    std::string name_;
+    std::vector<double> timesqueue_;
+};
+
+class TimerRecord
+{
+ public:
+
+ public:
+    template <class F>
+    static void Execute(F func,const std::string& func_name)
+    {
+        auto t1 = std::chrono::high_resolution_clock::now();
+        func();
+        auto t2 = std::chrono::high_resolution_clock::now();
+        auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1).count() * 1000;
+        if (records_.find(func_name) != records_.end()) {
+            records_[func_name].timesqueue_.emplace_back(time_used);
+        } else {
+            records_.insert({func_name, FuncRecord(func_name, time_used)});
+        }
+    }
+
+    static void PrintAll()
+    {
+        std::cout <<">> ===== Printing run time ====="<<std::endl;
+        for (const auto& r : records_) {
+            auto v = std::minmax_element(r.second.timesqueue_.begin(), r.second.timesqueue_.end());
+
+            std::cout<< "> [ " << r.first << " ] average time usage: "
+                     << std::accumulate(r.second.timesqueue_.begin(), r.second.timesqueue_.end(), 0.0) /
+                             double(r.second.timesqueue_.size())
+                     << " ms , called times: " << r.second.timesqueue_.size()<<
+                     "   max:"<<*v.second<<"ms , min:"<<*v.first<<"ms"<<std::endl;
+        }
+        std::cout << ">>> ===== Printing run time end ====="<<std::endl;
+    }
+
+
+
+ protected:
+    static std::map<std::string,FuncRecord> records_;
+};
+std::map<std::string,FuncRecord> TimerRecord::records_=std::map<std::string,FuncRecord>();
+
+
+
+#endif //SRC_LIO_LIVOX_SRC_UTILS_TIMERRECORD_H_

livox/common/FastCRC/FastCRC.h

@@ -0,0 +1,80 @@
+/* FastCRC library code is placed under the MIT license
+ * Copyright (c) 2014,2015 Frank Bosing
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+// Teensy 3.0, Teensy 3.1:
+// See K20P64M72SF1RM.pdf (Kinetis), Pages 638 - 641 for documentation of CRC
+// Device See KINETIS_4N30D.pdf for Errata (Errata ID 2776)
+//
+// So, ALL HW-calculations are done as 32 bit.
+//
+//
+//
+// Thanks to:
+// - Catalogue of parametrised CRC algorithms, CRC RevEng
+// http://reveng.sourceforge.net/crc-catalogue/
+//
+// - Danjel McGougan (CRC-Table-Generator)
+//
+
+//
+// modify from FastCRC library @ 2018/11/20
+//
+
+#ifndef FASTCRC_FASTCRC_H_
+#define FASTCRC_FASTCRC_H_
+
+#include <stdint.h>
+
+// ================= 16-BIT CRC ===================
+
+class FastCRC16 {
+ public:
+  FastCRC16(uint16_t seed);
+
+  // change function name from mcrf4xx_upd to mcrf4xx
+  uint16_t mcrf4xx_calc(
+      const uint8_t *data,
+      const uint16_t datalen);  // Equivalent to _crc_ccitt_update() in
+                                // crc16.h from avr_libc
+
+ private:
+  uint16_t seed_;
+};
+
+// ================= 32-BIT CRC ===================
+
+class FastCRC32 {
+ public:
+  FastCRC32(uint32_t seed);
+
+  // change function name from crc32_upd to crc32
+  uint32_t crc32_calc(
+      const uint8_t *data,
+      uint16_t len);  // Call for subsequent calculations with previous seed
+
+ private:
+  uint32_t seed_;
+};
+
+#endif  // FASTCRC_FASTCRC_H_

livox/common/FastCRC/FastCRC_tables.hpp

@@ -0,0 +1,201 @@
+/* FastCRC library code is placed under the MIT license
+ * Copyright (c) 2014,2015 Frank Bosing
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+/*
+ Tables generated with universal_crc by Danjel McGougan
+*/
+
+//
+// modify from FastCRC library @ 2018/11/20
+//
+
+#ifndef FASTCRC_FASTCRC_TABLES_H_
+#define FASTCRC_FASTCRC_TABLES_H_
+
+#include <stdint.h>
+
+// crc16 table
+const uint16_t crc_table_mcrf4xx[1024] = {
+    0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
+    0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
+    0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
+    0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+    0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
+    0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
+    0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
+    0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+    0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
+    0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
+    0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
+    0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+    0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
+    0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
+    0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
+    0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+    0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
+    0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
+    0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
+    0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+    0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
+    0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
+    0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
+    0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+    0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
+    0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
+    0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
+    0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+    0x3de3, 0x2c6a, 0x1ef1, 0x0f78, 0x0000, 0x19d8, 0x33b0, 0x2a68, 0x6760,
+    0x7eb8, 0x54d0, 0x4d08, 0xcec0, 0xd718, 0xfd70, 0xe4a8, 0xa9a0, 0xb078,
+    0x9a10, 0x83c8, 0x9591, 0x8c49, 0xa621, 0xbff9, 0xf2f1, 0xeb29, 0xc141,
+    0xd899, 0x5b51, 0x4289, 0x68e1, 0x7139, 0x3c31, 0x25e9, 0x0f81, 0x1659,
+    0x2333, 0x3aeb, 0x1083, 0x095b, 0x4453, 0x5d8b, 0x77e3, 0x6e3b, 0xedf3,
+    0xf42b, 0xde43, 0xc79b, 0x8a93, 0x934b, 0xb923, 0xa0fb, 0xb6a2, 0xaf7a,
+    0x8512, 0x9cca, 0xd1c2, 0xc81a, 0xe272, 0xfbaa, 0x7862, 0x61ba, 0x4bd2,
+    0x520a, 0x1f02, 0x06da, 0x2cb2, 0x356a, 0x4666, 0x5fbe, 0x75d6, 0x6c0e,
+    0x2106, 0x38de, 0x12b6, 0x0b6e, 0x88a6, 0x917e, 0xbb16, 0xa2ce, 0xefc6,
+    0xf61e, 0xdc76, 0xc5ae, 0xd3f7, 0xca2f, 0xe047, 0xf99f, 0xb497, 0xad4f,
+    0x8727, 0x9eff, 0x1d37, 0x04ef, 0x2e87, 0x375f, 0x7a57, 0x638f, 0x49e7,
+    0x503f, 0x6555, 0x7c8d, 0x56e5, 0x4f3d, 0x0235, 0x1bed, 0x3185, 0x285d,
+    0xab95, 0xb24d, 0x9825, 0x81fd, 0xccf5, 0xd52d, 0xff45, 0xe69d, 0xf0c4,
+    0xe91c, 0xc374, 0xdaac, 0x97a4, 0x8e7c, 0xa414, 0xbdcc, 0x3e04, 0x27dc,
+    0x0db4, 0x146c, 0x5964, 0x40bc, 0x6ad4, 0x730c, 0x8ccc, 0x9514, 0xbf7c,
+    0xa6a4, 0xebac, 0xf274, 0xd81c, 0xc1c4, 0x420c, 0x5bd4, 0x71bc, 0x6864,
+    0x256c, 0x3cb4, 0x16dc, 0x0f04, 0x195d, 0x0085, 0x2aed, 0x3335, 0x7e3d,
+    0x67e5, 0x4d8d, 0x5455, 0xd79d, 0xce45, 0xe42d, 0xfdf5, 0xb0fd, 0xa925,
+    0x834d, 0x9a95, 0xafff, 0xb627, 0x9c4f, 0x8597, 0xc89f, 0xd147, 0xfb2f,
+    0xe2f7, 0x613f, 0x78e7, 0x528f, 0x4b57, 0x065f, 0x1f87, 0x35ef, 0x2c37,
+    0x3a6e, 0x23b6, 0x09de, 0x1006, 0x5d0e, 0x44d6, 0x6ebe, 0x7766, 0xf4ae,
+    0xed76, 0xc71e, 0xdec6, 0x93ce, 0x8a16, 0xa07e, 0xb9a6, 0xcaaa, 0xd372,
+    0xf91a, 0xe0c2, 0xadca, 0xb412, 0x9e7a, 0x87a2, 0x046a, 0x1db2, 0x37da,
+    0x2e02, 0x630a, 0x7ad2, 0x50ba, 0x4962, 0x5f3b, 0x46e3, 0x6c8b, 0x7553,
+    0x385b, 0x2183, 0x0beb, 0x1233, 0x91fb, 0x8823, 0xa24b, 0xbb93, 0xf69b,
+    0xef43, 0xc52b, 0xdcf3, 0xe999, 0xf041, 0xda29, 0xc3f1, 0x8ef9, 0x9721,
+    0xbd49, 0xa491, 0x2759, 0x3e81, 0x14e9, 0x0d31, 0x4039, 0x59e1, 0x7389,
+    0x6a51, 0x7c08, 0x65d0, 0x4fb8, 0x5660, 0x1b68, 0x02b0, 0x28d8, 0x3100,
+    0xb2c8, 0xab10, 0x8178, 0x98a0, 0xd5a8, 0xcc70, 0xe618, 0xffc0, 0x0000,
+    0x5adc, 0xb5b8, 0xef64, 0x6361, 0x39bd, 0xd6d9, 0x8c05, 0xc6c2, 0x9c1e,
+    0x737a, 0x29a6, 0xa5a3, 0xff7f, 0x101b, 0x4ac7, 0x8595, 0xdf49, 0x302d,
+    0x6af1, 0xe6f4, 0xbc28, 0x534c, 0x0990, 0x4357, 0x198b, 0xf6ef, 0xac33,
+    0x2036, 0x7aea, 0x958e, 0xcf52, 0x033b, 0x59e7, 0xb683, 0xec5f, 0x605a,
+    0x3a86, 0xd5e2, 0x8f3e, 0xc5f9, 0x9f25, 0x7041, 0x2a9d, 0xa698, 0xfc44,
+    0x1320, 0x49fc, 0x86ae, 0xdc72, 0x3316, 0x69ca, 0xe5cf, 0xbf13, 0x5077,
+    0x0aab, 0x406c, 0x1ab0, 0xf5d4, 0xaf08, 0x230d, 0x79d1, 0x96b5, 0xcc69,
+    0x0676, 0x5caa, 0xb3ce, 0xe912, 0x6517, 0x3fcb, 0xd0af, 0x8a73, 0xc0b4,
+    0x9a68, 0x750c, 0x2fd0, 0xa3d5, 0xf909, 0x166d, 0x4cb1, 0x83e3, 0xd93f,
+    0x365b, 0x6c87, 0xe082, 0xba5e, 0x553a, 0x0fe6, 0x4521, 0x1ffd, 0xf099,
+    0xaa45, 0x2640, 0x7c9c, 0x93f8, 0xc924, 0x054d, 0x5f91, 0xb0f5, 0xea29,
+    0x662c, 0x3cf0, 0xd394, 0x8948, 0xc38f, 0x9953, 0x7637, 0x2ceb, 0xa0ee,
+    0xfa32, 0x1556, 0x4f8a, 0x80d8, 0xda04, 0x3560, 0x6fbc, 0xe3b9, 0xb965,
+    0x5601, 0x0cdd, 0x461a, 0x1cc6, 0xf3a2, 0xa97e, 0x257b, 0x7fa7, 0x90c3,
+    0xca1f, 0x0cec, 0x5630, 0xb954, 0xe388, 0x6f8d, 0x3551, 0xda35, 0x80e9,
+    0xca2e, 0x90f2, 0x7f96, 0x254a, 0xa94f, 0xf393, 0x1cf7, 0x462b, 0x8979,
+    0xd3a5, 0x3cc1, 0x661d, 0xea18, 0xb0c4, 0x5fa0, 0x057c, 0x4fbb, 0x1567,
+    0xfa03, 0xa0df, 0x2cda, 0x7606, 0x9962, 0xc3be, 0x0fd7, 0x550b, 0xba6f,
+    0xe0b3, 0x6cb6, 0x366a, 0xd90e, 0x83d2, 0xc915, 0x93c9, 0x7cad, 0x2671,
+    0xaa74, 0xf0a8, 0x1fcc, 0x4510, 0x8a42, 0xd09e, 0x3ffa, 0x6526, 0xe923,
+    0xb3ff, 0x5c9b, 0x0647, 0x4c80, 0x165c, 0xf938, 0xa3e4, 0x2fe1, 0x753d,
+    0x9a59, 0xc085, 0x0a9a, 0x5046, 0xbf22, 0xe5fe, 0x69fb, 0x3327, 0xdc43,
+    0x869f, 0xcc58, 0x9684, 0x79e0, 0x233c, 0xaf39, 0xf5e5, 0x1a81, 0x405d,
+    0x8f0f, 0xd5d3, 0x3ab7, 0x606b, 0xec6e, 0xb6b2, 0x59d6, 0x030a, 0x49cd,
+    0x1311, 0xfc75, 0xa6a9, 0x2aac, 0x7070, 0x9f14, 0xc5c8, 0x09a1, 0x537d,
+    0xbc19, 0xe6c5, 0x6ac0, 0x301c, 0xdf78, 0x85a4, 0xcf63, 0x95bf, 0x7adb,
+    0x2007, 0xac02, 0xf6de, 0x19ba, 0x4366, 0x8c34, 0xd6e8, 0x398c, 0x6350,
+    0xef55, 0xb589, 0x5aed, 0x0031, 0x4af6, 0x102a, 0xff4e, 0xa592, 0x2997,
+    0x734b, 0x9c2f, 0xc6f3, 0x0000, 0x1cbb, 0x3976, 0x25cd, 0x72ec, 0x6e57,
+    0x4b9a, 0x5721, 0xe5d8, 0xf963, 0xdcae, 0xc015, 0x9734, 0x8b8f, 0xae42,
+    0xb2f9, 0xc3a1, 0xdf1a, 0xfad7, 0xe66c, 0xb14d, 0xadf6, 0x883b, 0x9480,
+    0x2679, 0x3ac2, 0x1f0f, 0x03b4, 0x5495, 0x482e, 0x6de3, 0x7158, 0x8f53,
+    0x93e8, 0xb625, 0xaa9e, 0xfdbf, 0xe104, 0xc4c9, 0xd872, 0x6a8b, 0x7630,
+    0x53fd, 0x4f46, 0x1867, 0x04dc, 0x2111, 0x3daa, 0x4cf2, 0x5049, 0x7584,
+    0x693f, 0x3e1e, 0x22a5, 0x0768, 0x1bd3, 0xa92a, 0xb591, 0x905c, 0x8ce7,
+    0xdbc6, 0xc77d, 0xe2b0, 0xfe0b, 0x16b7, 0x0a0c, 0x2fc1, 0x337a, 0x645b,
+    0x78e0, 0x5d2d, 0x4196, 0xf36f, 0xefd4, 0xca19, 0xd6a2, 0x8183, 0x9d38,
+    0xb8f5, 0xa44e, 0xd516, 0xc9ad, 0xec60, 0xf0db, 0xa7fa, 0xbb41, 0x9e8c,
+    0x8237, 0x30ce, 0x2c75, 0x09b8, 0x1503, 0x4222, 0x5e99, 0x7b54, 0x67ef,
+    0x99e4, 0x855f, 0xa092, 0xbc29, 0xeb08, 0xf7b3, 0xd27e, 0xcec5, 0x7c3c,
+    0x6087, 0x454a, 0x59f1, 0x0ed0, 0x126b, 0x37a6, 0x2b1d, 0x5a45, 0x46fe,
+    0x6333, 0x7f88, 0x28a9, 0x3412, 0x11df, 0x0d64, 0xbf9d, 0xa326, 0x86eb,
+    0x9a50, 0xcd71, 0xd1ca, 0xf407, 0xe8bc, 0x2d6e, 0x31d5, 0x1418, 0x08a3,
+    0x5f82, 0x4339, 0x66f4, 0x7a4f, 0xc8b6, 0xd40d, 0xf1c0, 0xed7b, 0xba5a,
+    0xa6e1, 0x832c, 0x9f97, 0xeecf, 0xf274, 0xd7b9, 0xcb02, 0x9c23, 0x8098,
+    0xa555, 0xb9ee, 0x0b17, 0x17ac, 0x3261, 0x2eda, 0x79fb, 0x6540, 0x408d,
+    0x5c36, 0xa23d, 0xbe86, 0x9b4b, 0x87f0, 0xd0d1, 0xcc6a, 0xe9a7, 0xf51c,
+    0x47e5, 0x5b5e, 0x7e93, 0x6228, 0x3509, 0x29b2, 0x0c7f, 0x10c4, 0x619c,
+    0x7d27, 0x58ea, 0x4451, 0x1370, 0x0fcb, 0x2a06, 0x36bd, 0x8444, 0x98ff,
+    0xbd32, 0xa189, 0xf6a8, 0xea13, 0xcfde, 0xd365, 0x3bd9, 0x2762, 0x02af,
+    0x1e14, 0x4935, 0x558e, 0x7043, 0x6cf8, 0xde01, 0xc2ba, 0xe777, 0xfbcc,
+    0xaced, 0xb056, 0x959b, 0x8920, 0xf878, 0xe4c3, 0xc10e, 0xddb5, 0x8a94,
+    0x962f, 0xb3e2, 0xaf59, 0x1da0, 0x011b, 0x24d6, 0x386d, 0x6f4c, 0x73f7,
+    0x563a, 0x4a81, 0xb48a, 0xa831, 0x8dfc, 0x9147, 0xc666, 0xdadd, 0xff10,
+    0xe3ab, 0x5152, 0x4de9, 0x6824, 0x749f, 0x23be, 0x3f05, 0x1ac8, 0x0673,
+    0x772b, 0x6b90, 0x4e5d, 0x52e6, 0x05c7, 0x197c, 0x3cb1, 0x200a, 0x92f3,
+    0x8e48, 0xab85, 0xb73e, 0xe01f, 0xfca4, 0xd969, 0xc5d2};
+
+// crc32 table
+const uint32_t crc_table_crc32[256] = {
+    0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
+    0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+    0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
+    0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+    0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
+    0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+    0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c,
+    0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+    0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
+    0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+    0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106,
+    0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+    0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
+    0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+    0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
+    0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+    0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
+    0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+    0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
+    0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+    0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
+    0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+    0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
+    0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+    0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
+    0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+    0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
+    0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+    0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
+    0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+    0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
+    0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+    0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
+    0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+    0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
+    0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+    0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
+    0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+    0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
+    0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+    0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
+    0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+    0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d};
+
+#endif

livox/common/FastCRC/FastCRCsw.cpp

@@ -0,0 +1,143 @@
+/* FastCRC library code is placed under the MIT license
+ * Copyright (c) 2014,2015,2016 Frank Bosing
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+//
+// Thanks to:
+// - Catalogue of parametrised CRC algorithms, CRC RevEng
+// http://reveng.sourceforge.net/crc-catalogue/
+//
+// - Danjel McGougan (CRC-Table-Generator)
+//
+
+//
+// modify from FastCRC library @ 2018/11/20
+//
+
+#include "FastCRC.h"
+#include "FastCRC_tables.hpp"
+
+// ================= 16-BIT CRC ===================
+/** Constructor
+ */
+FastCRC16::FastCRC16(uint16_t seed) { seed_ = seed; }
+
+#define crc_n4(crc, data, table)                                               \
+  crc ^= data;                                                                 \
+  crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
+        (table[((data >> 16) & 0xff) + 0x100]) ^ (table[data >> 24]);
+
+/** MCRF4XX
+ * equivalent to _crc_ccitt_update() in crc16.h from avr_libc
+ * @param data Pointer to Data
+ * @param datalen Length of Data
+ * @return CRC value
+ */
+
+uint16_t FastCRC16::mcrf4xx_calc(const uint8_t *data, uint16_t len) {
+  uint16_t crc = seed_;
+
+  while (((uintptr_t)data & 3) && len) {
+    crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
+    len--;
+  }
+
+  while (len >= 16) {
+    len -= 16;
+    crc_n4(crc, ((uint32_t *)data)[0], crc_table_mcrf4xx);
+    crc_n4(crc, ((uint32_t *)data)[1], crc_table_mcrf4xx);
+    crc_n4(crc, ((uint32_t *)data)[2], crc_table_mcrf4xx);
+    crc_n4(crc, ((uint32_t *)data)[3], crc_table_mcrf4xx);
+    data += 16;
+  }
+
+  while (len--) {
+    crc = (crc >> 8) ^ crc_table_mcrf4xx[(crc & 0xff) ^ *data++];
+  }
+
+  // seed = crc;
+  return crc;
+}
+
+// ================= 32-BIT CRC ===================
+/** Constructor
+ */
+FastCRC32::FastCRC32(uint32_t seed) { seed_ = seed; }
+
+#define crc_n4d(crc, data, table)                                              \
+  crc ^= data;                                                                 \
+  crc = (table[(crc & 0xff) + 0x300]) ^ (table[((crc >> 8) & 0xff) + 0x200]) ^ \
+        (table[((crc >> 16) & 0xff) + 0x100]) ^ (table[(crc >> 24) & 0xff]);
+
+#define crcsm_n4d(crc, data, table)       \
+  crc ^= data;                            \
+  crc = (crc >> 8) ^ (table[crc & 0xff]); \
+  crc = (crc >> 8) ^ (table[crc & 0xff]); \
+  crc = (crc >> 8) ^ (table[crc & 0xff]); \
+  crc = (crc >> 8) ^ (table[crc & 0xff]);
+
+/** CRC32
+ * Alias CRC-32/ADCCP, PKZIP, Ethernet, 802.3
+ * @param data Pointer to Data
+ * @param datalen Length of Data
+ * @return CRC value
+ */
+#if CRC_BIGTABLES
+#define CRC_TABLE_CRC32 crc_table_crc32_big
+#else
+#define CRC_TABLE_CRC32 crc_table_crc32
+#endif
+
+uint32_t FastCRC32::crc32_calc(const uint8_t *data, uint16_t len) {
+  uint32_t crc = seed_ ^ 0xffffffff;
+
+  while (((uintptr_t)data & 3) && len) {
+    crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
+    len--;
+  }
+
+  while (len >= 16) {
+    len -= 16;
+#if CRC_BIGTABLES
+    crc_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
+    crc_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
+    crc_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
+    crc_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
+#else
+    crcsm_n4d(crc, ((uint32_t *)data)[0], CRC_TABLE_CRC32);
+    crcsm_n4d(crc, ((uint32_t *)data)[1], CRC_TABLE_CRC32);
+    crcsm_n4d(crc, ((uint32_t *)data)[2], CRC_TABLE_CRC32);
+    crcsm_n4d(crc, ((uint32_t *)data)[3], CRC_TABLE_CRC32);
+#endif
+    data += 16;
+  }
+
+  while (len--) {
+    crc = (crc >> 8) ^ CRC_TABLE_CRC32[(crc & 0xff) ^ *data++];
+  }
+
+  // seed = crc;
+  crc ^= 0xffffffff;
+
+  return crc;
+}

livox/common/FastCRC/LICENSE.md

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2016 Frank
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

livox/common/FastCRC/README.md

@@ -0,0 +1,56 @@
+FastCRC
+=======
+
+Fast CRC Arduino library
+Up to 30 times faster than crc16.h (_avr_libc)
+
+ - uses the on-chip hardware for Teensy 3.0 / 3.1 / 3.2 / 3.5 / 3.6
+ - uses fast table-algorithms for other chips
+
+List of supported CRC calculations:
+-
+7 BIT:
+
+CRC7
+ (poly=0x09 init=0x00 refin=false refout=false xorout=0x00 check=0x75)
+ MultiMediaCard interface
+
+
+8 BIT:
+
+SMBUS
+ (poly=0x07 init=0x00 refin=false refout=false xorout=0x00 check=0xf4)
+
+MAXIM
+ (poly=0x31 init=0x00 refin=true refout=true xorout=0x00  check=0xa1)
+
+
+16 BIT:
+
+KERMIT (Alias CRC-16/CCITT, CRC-16/CCITT-TRUE, CRC-CCITT)
+ (poly=0x1021 init=0x0000 refin=true refout=true xorout=0x0000 check=0x2189
+  Attention: sometimes you'll find byteswapped presentation of result in other implementations)
+
+CCITT-FALSE
+ (poly=0x1021 init=0xffff refin=false refout=false xorout=0x0000 check=0x29b1)
+
+MCRF4XX
+ (poly=0x1021 init=0xffff refin=true refout=true xorout=0x0000 check=0x6f91)
+
+MODBUS
+ (poly=0x8005 init=0xffff refin=true refout=true xorout=0x0000 check=0x4b37)
+
+XMODEM (Alias ZMODEM, CRC-16/ACORN)
+ (poly=0x1021 init=0x0000 refin=false refout=false xorout=0x0000 check=0x31c3)
+
+X25 (Alias CRC-16/IBM-SDLC, CRC-16/ISO-HDLC, CRC-B)
+ (poly=0x1021 init=0xffff refin=true refout=true xorout=0xffff check=0x906e)
+
+
+32 BIT:
+
+CRC32, CRC-32/ADCCP, PKZIP, ETHERNET, 802.3
+  (poly=0x04c11db7 init=0xffffffff refin=true refout=true xorout=0xffffffff check=0xcbf43926)
+
+CKSUM, CRC-32/POSIX
+  (poly=0x04c11db7 init=0x00000000 refin=false refout=false xorout=0xffffffff check=0x765e7680)

livox/common/comm/comm_device.h

@@ -0,0 +1,71 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef COMM_COMM_DEVICE_H_
+#define COMM_COMM_DEVICE_H_
+
+#include <stdint.h>
+
+namespace livox_ros {
+
+const uint32_t kDevNameLengthMax = 256;
+
+/** Communication device type define */
+enum CommDeviceType {
+  kCommDevUart,
+  kCommDevUsb,
+  kCommDevCan,
+  kCommDevEthernet,
+  kCommDevUndef
+};
+
+/** Communication device uart config */
+struct CommDevUartConfig {
+  uint8_t baudrate;
+  uint8_t parity;
+};
+
+/** Communication device usb config */
+struct CommDevUsbConfig {
+  void *data;
+};
+
+/** Communication device can config */
+struct CommDevCanConfig {
+  void *data;
+};
+
+/** Communication device config */
+typedef struct {
+  uint8_t type;
+  char name[kDevNameLengthMax];
+  union {
+    CommDevUartConfig uart;
+    CommDevUsbConfig usb;
+    CommDevCanConfig can;
+  } config;
+} CommDevConfig;
+
+}  // namespace livox_ros
+#endif  // COMM_COMM_DEVICE_H_

livox/common/comm/comm_protocol.cpp

@@ -0,0 +1,224 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "comm_protocol.h"
+#include <stdio.h>
+#include <string.h>
+#include <iostream>
+
+namespace livox_ros {
+
+CommProtocol::CommProtocol(ProtocolConfig &config) : config_(config) {
+  ResetParser();
+  ResetCache();
+
+  offset_to_read_index_ = 0;
+  packet_length_ = 0;
+
+  if (kGps == config.type) {
+    is_length_known = false;
+    protocol_ = new GpsProtocol();
+  } else {
+    is_length_known = true;
+    protocol_ = nullptr;
+  }
+}
+
+CommProtocol::~CommProtocol() {
+  if (protocol_) {
+    delete protocol_;
+  }
+}
+
+uint8_t *CommProtocol::FetchCacheFreeSpace(uint32_t *o_len) {
+  UpdateCache();
+
+  if (cache_.wr_idx < cache_.size) {
+    *o_len = cache_.size - cache_.wr_idx;
+    return &cache_.buf[cache_.wr_idx];
+  } else {
+    *o_len = 0;
+    return nullptr;
+  }
+}
+
+int32_t CommProtocol::UpdateCacheWrIdx(uint32_t used_size) {
+  if ((cache_.wr_idx + used_size) < cache_.size) {
+    cache_.wr_idx += used_size;
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+uint32_t CommProtocol::GetCacheTailSize() {
+  if (cache_.wr_idx < cache_.size) {
+    return cache_.size - cache_.wr_idx;
+  } else {
+    return 0;
+  }
+}
+
+uint32_t CommProtocol::GetValidDataSize() {
+  if (cache_.wr_idx > cache_.rd_idx) {
+    return cache_.wr_idx - cache_.rd_idx;
+  } else {
+    return 0;
+  }
+}
+
+void CommProtocol::UpdateCache(void) {
+  if (GetCacheTailSize() <
+      kMoveCacheLimit) { /* move unused data to cache head */
+    uint32_t valid_data_size = GetValidDataSize();
+
+    if (valid_data_size) {
+      memmove(cache_.buf, &cache_.buf[cache_.rd_idx], valid_data_size);
+      cache_.rd_idx = 0;
+      cache_.wr_idx = valid_data_size;
+    } else if (cache_.rd_idx) {
+      cache_.rd_idx = 0;
+      cache_.wr_idx = 0;
+    }
+  }
+}
+
+int32_t CommProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+                           const CommPacket &i_packet) {
+  return protocol_->Pack(o_buf, o_buf_size, o_len, i_packet);
+}
+
+void CommProtocol::ResetParser() { fsm_parse_step_ = kSearchPacketPreamble; }
+
+int32_t CommProtocol::ParseCommStream(CommPacket *o_pack) {
+  int32_t ret = kParseFail;
+  while ((GetValidDataSize() > protocol_->GetPreambleLen()) &&
+         (GetValidDataSize() > offset_to_read_index_)) {
+    switch (fsm_parse_step_) {
+      case kSearchPacketPreamble: {
+        FsmSearchPacketPreamble();
+        break;
+      }
+      case kFindPacketLength: {
+        FsmFindPacketLength();
+        break;
+      }
+      case kGetPacketData: {
+        ret = FsmGetPacketData(o_pack);
+        break;
+      }
+      default: { FsmParserStateTransfer(kSearchPacketPreamble); }
+    }
+
+    /* Must exit when in the below case */
+    if ((ret == kParseSuccess) || (ret == kParseNeedMoreData)) break;
+  }
+
+  return ret;
+}
+
+uint16_t CommProtocol::GetAndUpdateSeqNum() {
+  /* add lock here for thread safe */
+  uint16_t seq = seq_num_;
+  seq_num_++;
+
+  return seq;
+}
+
+int32_t CommProtocol::FsmSearchPacketPreamble() {
+  do {
+    if (!protocol_->CheckPreamble(GetCacheReadPos())) {
+      if (!is_length_known) {
+        offset_to_read_index_ = 0;
+        packet_length_ = 0;
+        FsmParserStateTransfer(kFindPacketLength);
+        break;
+      } else {
+        packet_length_ = protocol_->GetPacketLen(GetCacheReadPos());
+        if ((packet_length_ < cache_.size) &&
+            (packet_length_ >
+             protocol_
+                 ->GetPacketWrapperLen())) { /* check the legality of length */
+          FsmParserStateTransfer(kGetPacketData);
+          break;
+        }
+      }
+    }
+    // printf("|%2x",  cache_.buf[cache_.rd_idx]);
+    ++cache_.rd_idx; /* skip one byte */
+  } while (0);
+
+  return 0;
+}
+
+int32_t CommProtocol::FsmFindPacketLength() {
+  uint32_t valid_data_size = GetValidDataSize();
+  uint32_t ret = protocol_->FindPacketLen(GetCacheReadPos(), valid_data_size);
+  if (ret == kFindLengthSuccess) {
+    packet_length_ = protocol_->GetPacketLen(GetCacheReadPos());
+    FsmParserStateTransfer(kGetPacketData);
+    offset_to_read_index_ = 0;
+  } else if (ret == kFindLengthContinue) { /* continue to find next time */
+    offset_to_read_index_ = valid_data_size;
+  } else { /* find length error */
+    offset_to_read_index_ = 0;
+    cache_.rd_idx += valid_data_size;
+    FsmParserStateTransfer(kSearchPacketPreamble);
+    printf("Continue to find length error\n");
+  }
+
+  return 0;
+}
+
+int32_t CommProtocol::FsmGetPacketData(CommPacket *o_pack) {
+  int32_t ret = kParseFail;
+  do {
+    if (GetValidDataSize() < packet_length_) {
+      ret = kParseNeedMoreData;
+      break;
+    }
+
+    if (protocol_->CheckPacket(GetCacheReadPos())) {
+      cache_.rd_idx += protocol_->GetPreambleLen();
+      FsmParserStateTransfer(kSearchPacketPreamble);
+      printf("Check packet error\n");
+      break;
+    }
+
+    if (protocol_->ParsePacket(GetCacheReadPos(), packet_length_, o_pack)) {
+      cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos());
+      FsmParserStateTransfer(kSearchPacketPreamble);
+      printf("Parse packet error\n");
+      break;
+    }
+
+    cache_.rd_idx += protocol_->GetPacketLen(GetCacheReadPos());
+    FsmParserStateTransfer(kSearchPacketPreamble);
+    return kParseSuccess;
+  } while (0);
+
+  return ret;
+}
+
+}  // namespace livox_ros

livox/common/comm/comm_protocol.h

@@ -0,0 +1,105 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef COMM_COMM_PROTOCOL_H_
+#define COMM_COMM_PROTOCOL_H_
+
+#include <stdint.h>
+#include "gps_protocol.h"
+#include "protocol.h"
+#include "sdk_protocol.h"
+
+namespace livox_ros {
+const uint32_t kCacheSize = 8192;
+const uint32_t kMoveCacheLimit = 1536;
+
+enum FsmParseState {
+  kSearchPacketPreamble = 0,
+  kFindPacketLength = 1,
+  kGetPacketData = 2,
+  kParseStepUndef
+};
+
+/** Communication data cache define */
+typedef struct {
+  uint8_t buf[kCacheSize];
+  uint32_t rd_idx;
+  uint32_t wr_idx;
+  uint32_t size;
+} CommCache;
+
+class CommProtocol {
+ public:
+  CommProtocol(ProtocolConfig &config);
+  ~CommProtocol();
+
+  int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+               const CommPacket &i_packet);
+
+  int32_t ParseCommStream(CommPacket *o_pack);
+
+  uint8_t *FetchCacheFreeSpace(uint32_t *o_len);
+
+  int32_t UpdateCacheWrIdx(uint32_t used_size);
+
+  uint16_t GetAndUpdateSeqNum();
+
+  void ResetParser();
+
+ private:
+  uint32_t GetCacheTailSize();
+  uint32_t GetValidDataSize();
+  void UpdateCache(void);
+  uint8_t *GetCacheReadPos() { return &cache_.buf[cache_.rd_idx]; }
+  void ResetCache() {
+    cache_.wr_idx = 0;
+    cache_.rd_idx = 0;
+    cache_.size = kCacheSize;
+  }
+
+  ProtocolConfig config_;
+  Protocol *protocol_;
+  CommCache cache_;
+  uint16_t seq_num_;
+
+  bool is_length_known;
+  bool IsLengthKnown() { return is_length_known; }
+
+  volatile uint32_t offset_to_read_index_;
+  uint32_t packet_length_;
+  volatile uint32_t fsm_parse_step_;
+  int32_t FsmSearchPacketPreamble();
+  int32_t FsmFindPacketLength();
+  int32_t FsmGetPacketData(CommPacket *o_pack);
+  void FsmParserStateTransfer(uint32_t new_state) {
+    if (new_state < kParseStepUndef) {
+      fsm_parse_step_ = new_state;
+    } else {
+      fsm_parse_step_ = kSearchPacketPreamble;
+    }
+  }
+};
+
+}  // namespace livox_ros
+#endif  // COMM_COMM_PROTOCOL_H_

livox/common/comm/gps_protocol.cpp

@@ -0,0 +1,125 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "gps_protocol.h"
+
+#include <ctype.h>
+#include <stdio.h>
+#include <string.h>
+
+namespace livox_ros {
+
+const uint8_t kGpsProtocolSof = '$';
+const uint8_t kGpsProtocolEof = '*';
+const uint32_t kPacketLengthLmit = 200;
+const uint32_t kPreambleLen = 1;
+const uint32_t kWrapperLen = 4; /** '$' + '*' + '2 checksum byte' */
+
+GpsProtocol::GpsProtocol() { found_length_ = 0; }
+
+int32_t GpsProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+                          const CommPacket &i_packet) {
+  // GpsPacket* gps_packet = (GpsPacket*)o_buf;
+  return 0;
+}
+
+int32_t GpsProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
+                                 CommPacket *o_packet) {
+  // GpsPacket *gps_packet = (GpsPacket *)i_buf;
+
+  if (i_len < GetPacketWrapperLen()) {
+    return -1;  // packet length error
+  }
+  memset((void *)o_packet, 0, sizeof(CommPacket));
+  o_packet->protocol = kGps;
+  o_packet->data = (uint8_t *)i_buf;
+  o_packet->data_len = i_len;
+
+  return 0;
+}
+
+uint32_t GpsProtocol::GetPreambleLen() { return kPreambleLen; /** '$' */ }
+
+uint32_t GpsProtocol::GetPacketWrapperLen() {
+  return kWrapperLen; /** '$' + '*' + '2 checksum bytes' */
+}
+
+uint32_t GpsProtocol::FindPacketLen(const uint8_t *buf, uint32_t buf_length) {
+  uint32_t i = 0;
+  for (; (i < buf_length) && (i < kPacketLengthLmit); i++) {
+    if ((buf[i] == kGpsProtocolEof) && (buf[0] == kGpsProtocolSof)) {
+      found_length_ = i + 1 + 2; /* num = index + 1 + two bytes checksum */
+      return kFindLengthSuccess;
+    }
+  }
+  if (i < kPacketLengthLmit) {
+    return kFindLengthContinue;
+  } else {
+    return kFindLengthError;
+  }
+}
+
+uint32_t GpsProtocol::GetPacketLen(const uint8_t *buf) { return found_length_; }
+
+int32_t GpsProtocol::CheckPreamble(const uint8_t *buf) {
+  GpsPreamble *preamble = (GpsPreamble *)buf;
+
+  if (preamble->sof == kGpsProtocolSof) {
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+int32_t GpsProtocol::CheckPacket(const uint8_t *buf) {
+  uint8_t checksum =
+      CalcGpsPacketChecksum(&buf[1], found_length_ - kWrapperLen);
+  uint8_t raw_checksum = AscciiToHex(&buf[found_length_ - 2]);
+  if (checksum == raw_checksum) {
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+uint8_t GpsProtocol::CalcGpsPacketChecksum(const uint8_t *buf,
+                                           uint32_t length) {
+  uint8_t result = buf[0];
+  for (uint32_t i = 1; i < length; i++) {
+    result ^= buf[i];
+  }
+  return result;
+}
+
+uint8_t AscciiToHex(const uint8_t *TwoChar) {
+  uint8_t h = toupper(TwoChar[0]) - 0x30;
+  if (h > 9) h -= 7;
+
+  uint8_t l = toupper(TwoChar[1]) - 0x30;
+  if (l > 9) l -= 7;
+
+  return h * 16 + l;
+}
+
+}  // namespace livox_ros

livox/common/comm/gps_protocol.h

@@ -0,0 +1,79 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_GPS_PROTOCOL_H_
+#define LIVOX_GPS_PROTOCOL_H_
+
+#include <stdint.h>
+#include "protocol.h"
+
+namespace livox_ros {
+
+#pragma pack(1)
+
+typedef struct {
+  uint8_t sof;
+  uint8_t cmd_str[1];
+} GpsPreamble;
+
+typedef struct {
+  uint8_t sof;
+  uint8_t data[1];
+} GpsPacket;
+
+#pragma pack()
+
+uint8_t AscciiToHex(const uint8_t *TwoChar);
+
+class GpsProtocol : public Protocol {
+ public:
+  GpsProtocol();
+  ~GpsProtocol() = default;
+
+  int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
+                      CommPacket *o_packet) override;
+
+  int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+               const CommPacket &i_packet) override;
+
+  uint32_t GetPreambleLen() override;
+
+  uint32_t GetPacketWrapperLen() override;
+
+  uint32_t FindPacketLen(const uint8_t *buf, uint32_t buf_length) override;
+
+  uint32_t GetPacketLen(const uint8_t *buf) override;
+
+  int32_t CheckPreamble(const uint8_t *buf) override;
+
+  int32_t CheckPacket(const uint8_t *buf) override;
+
+ private:
+  uint32_t found_length_;
+
+  uint8_t CalcGpsPacketChecksum(const uint8_t *buf, uint32_t length);
+};
+
+}  // namespace livox_ros
+#endif  // LIVOX_GPS_PROTOCOL_H_

livox/common/comm/protocol.h

@@ -0,0 +1,106 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef COMM_PROTOCOL_H_
+#define COMM_PROTOCOL_H_
+
+#include <stdint.h>
+
+namespace livox_ros {
+typedef struct CommPacket CommPacket;
+
+typedef int (*RequestPackCb)(CommPacket *packet);
+
+typedef enum { kRequestPack, kAckPack, kMsgPack } PacketType;
+
+typedef enum { kLidarSdk, kRsvd1, kGps, kProtocolUndef } ProtocolType;
+
+typedef enum { kNoNeed, kNeedAck, kDelayAck } NeedAckType;
+
+typedef enum { kParseSuccess, kParseFail, kParseNeedMoreData } ParseResult;
+
+typedef enum {
+  kFindLengthSuccess,
+  kFindLengthContinue,
+  kFindLengthError
+} FindLengthResult;
+
+typedef struct CommPacket {
+  uint8_t packet_type;
+  uint8_t protocol;
+  uint8_t protocol_version;
+  uint8_t cmd_set;
+  uint32_t cmd_code;
+  uint32_t sender;
+  uint32_t sub_sender;
+  uint32_t receiver;
+  uint32_t sub_receiver;
+  uint32_t seq_num;
+  uint8_t *data;
+  uint16_t data_len;
+  uint32_t padding;
+} CommPacket;
+
+/** SDK Protocol info config */
+typedef struct {
+  uint16_t seed16;
+  uint16_t seed32;
+} SdkProtocolConfig;
+
+/** NAME-0183 Protocol info config for gps */
+typedef struct { void *data; } GpsProtocolConfig;
+
+typedef struct {
+  uint8_t type;
+  union {
+    SdkProtocolConfig sdk;
+    GpsProtocolConfig gps;
+  } config;
+} ProtocolConfig;
+
+class Protocol {
+ public:
+  virtual ~Protocol() = default;
+
+  virtual int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
+                              CommPacket *o_packet) = 0;
+
+  virtual int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+                       const CommPacket &i_packet) = 0;
+
+  virtual uint32_t GetPreambleLen() = 0;
+
+  virtual uint32_t GetPacketWrapperLen() = 0;
+
+  virtual uint32_t FindPacketLen(const uint8_t *buf, uint32_t buf_length) = 0;
+
+  virtual uint32_t GetPacketLen(const uint8_t *buf) = 0;
+
+  virtual int32_t CheckPreamble(const uint8_t *buf) = 0;
+
+  virtual int32_t CheckPacket(const uint8_t *buf) = 0;
+};
+
+}  // namespace livox_ros
+#endif  // COMM_PROTOCOL_H_

livox/common/comm/sdk_protocol.cpp

@@ -0,0 +1,134 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "sdk_protocol.h"
+
+#include <stdio.h>
+#include <string.h>
+
+namespace livox_ros {
+const uint8_t kSdkProtocolSof = 0xAA;
+const uint32_t kSdkPacketCrcSize = 4;          // crc32
+const uint32_t kSdkPacketPreambleCrcSize = 2;  // crc16
+
+SdkProtocol::SdkProtocol(uint16_t seed16, uint32_t seed32)
+    : crc16_(seed16), crc32_(seed32) {}
+
+int32_t SdkProtocol::Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+                          const CommPacket &i_packet) {
+  SdkPacket *sdk_packet = (SdkPacket *)o_buf;
+
+  if (kLidarSdk != i_packet.protocol) {
+    return -1;
+  }
+
+  sdk_packet->sof = kSdkProtocolSof;
+  sdk_packet->length = i_packet.data_len + GetPacketWrapperLen();
+
+  if (sdk_packet->length > o_buf_size) {
+    return -1;
+  }
+
+  sdk_packet->version = kSdkVer0;
+  sdk_packet->packet_type = i_packet.packet_type;
+  sdk_packet->seq_num = i_packet.seq_num & 0xFFFF;
+  sdk_packet->preamble_crc =
+      crc16_.mcrf4xx_calc(o_buf, GetPreambleLen() - kSdkPacketPreambleCrcSize);
+  sdk_packet->cmd_set = i_packet.cmd_set;
+  sdk_packet->cmd_id = i_packet.cmd_code;
+
+  memcpy(sdk_packet->data, i_packet.data, i_packet.data_len);
+
+  uint32_t crc =
+      crc32_.crc32_calc(o_buf, sdk_packet->length - kSdkPacketCrcSize);
+  o_buf[sdk_packet->length - 4] = crc & 0xFF;
+  o_buf[sdk_packet->length - 3] = (crc >> 8) & 0xFF;
+  o_buf[sdk_packet->length - 2] = (crc >> 16) & 0xFF;
+  o_buf[sdk_packet->length - 1] = (crc >> 24) & 0xFF;
+
+  *o_len = sdk_packet->length;
+
+  return 0;
+}
+
+int32_t SdkProtocol::ParsePacket(const uint8_t *i_buf, uint32_t i_len,
+                                 CommPacket *o_packet) {
+  SdkPacket *sdk_packet = (SdkPacket *)i_buf;
+
+  if (i_len < GetPacketWrapperLen()) {
+    return -1;  // packet lenth error
+  }
+
+  memset((void *)o_packet, 0, sizeof(CommPacket));
+
+  o_packet->packet_type = sdk_packet->packet_type;
+  o_packet->protocol = kLidarSdk;
+  o_packet->protocol_version = sdk_packet->version;
+
+  o_packet->seq_num = sdk_packet->seq_num;
+  o_packet->cmd_set = sdk_packet->cmd_set;
+  o_packet->cmd_code = sdk_packet->cmd_id;
+  o_packet->data_len = sdk_packet->length - GetPacketWrapperLen();
+  o_packet->data = sdk_packet->data;
+
+  return 0;
+}
+
+uint32_t SdkProtocol::GetPreambleLen() { return sizeof(SdkPreamble); }
+
+uint32_t SdkProtocol::GetPacketWrapperLen() {
+  return sizeof(SdkPacket) - 1 + kSdkPacketCrcSize;
+}
+
+uint32_t SdkProtocol::GetPacketLen(const uint8_t *buf) {
+  SdkPreamble *preamble = (SdkPreamble *)buf;
+  return preamble->length;
+}
+
+int32_t SdkProtocol::CheckPreamble(const uint8_t *buf) {
+  SdkPreamble *preamble = (SdkPreamble *)buf;
+
+  if ((preamble->sof == kSdkProtocolSof) &&
+      (crc16_.mcrf4xx_calc(buf, GetPreambleLen()) == 0)) {
+    return 0;
+  } else {
+    return -1;
+  }
+}
+
+int32_t SdkProtocol::CheckPacket(const uint8_t *buf) {
+  SdkPacket *sdk_packet = (SdkPacket *)buf;
+
+  uint32_t crc = ((uint32_t)(buf[sdk_packet->length - 4])) |
+                 (((uint32_t)(buf[sdk_packet->length - 3])) << 8) |
+                 (((uint32_t)(buf[sdk_packet->length - 2])) << 16) |
+                 (((uint32_t)(buf[sdk_packet->length - 1])) << 24);
+
+  if (crc32_.crc32_calc(buf, sdk_packet->length - kSdkPacketCrcSize) == crc) {
+    return 0;
+  } else {
+    return -1;
+  }
+}
+}  // namespace livox_ros

livox/common/comm/sdk_protocol.h

@@ -0,0 +1,86 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#ifndef LIVOX_SDK_PROTOCOL_H_
+#define LIVOX_SDK_PROTOCOL_H_
+
+#include <stdint.h>
+#include "FastCRC/FastCRC.h"
+#include "protocol.h"
+
+namespace livox_ros {
+typedef enum { kSdkVerNone, kSdkVer0, kSdkVer1 } SdkVersion;
+
+#pragma pack(1)
+
+typedef struct {
+  uint8_t sof;
+  uint8_t version;
+  uint16_t length;
+  uint8_t packet_type;
+  uint16_t seq_num;
+  uint16_t preamble_crc;
+} SdkPreamble;
+
+typedef struct {
+  uint8_t sof;
+  uint8_t version;
+  uint16_t length;
+  uint8_t packet_type;
+  uint16_t seq_num;
+  uint16_t preamble_crc;
+  uint8_t cmd_set;
+  uint8_t cmd_id;
+  uint8_t data[1];
+} SdkPacket;
+
+#pragma pack()
+
+class SdkProtocol : public Protocol {
+ public:
+  SdkProtocol(uint16_t seed16, uint32_t seed32);
+  ~SdkProtocol() = default;
+
+  int32_t ParsePacket(const uint8_t *i_buf, uint32_t i_len,
+                      CommPacket *o_packet) override;
+
+  int32_t Pack(uint8_t *o_buf, uint32_t o_buf_size, uint32_t *o_len,
+               const CommPacket &i_packet) override;
+
+  uint32_t GetPreambleLen() override;
+
+  uint32_t GetPacketWrapperLen() override;
+
+  uint32_t GetPacketLen(const uint8_t *buf) override;
+
+  int32_t CheckPreamble(const uint8_t *buf) override;
+
+  int32_t CheckPacket(const uint8_t *buf) override;
+
+ private:
+  FastCRC16 crc16_;
+  FastCRC32 crc32_;
+};
+}  // namespace livox_ros
+#endif  // LIVOX_SDK_PROTOCOL_H_

livox/common/rapidjson/allocators.h

@@ -0,0 +1,308 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ALLOCATORS_H_
+#define RAPIDJSON_ALLOCATORS_H_
+
+#include "rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Allocator
+
+/*! \class rapidjson::Allocator
+    \brief Concept for allocating, resizing and freeing memory block.
+
+    Note that Malloc() and Realloc() are non-static but Free() is static.
+
+    So if an allocator need to support Free(), it needs to put its pointer in
+    the header of memory block.
+
+\code
+concept Allocator {
+    static const bool kNeedFree;    //!< Whether this allocator needs to call
+Free().
+
+    // Allocate a memory block.
+    // \param size of the memory block in bytes.
+    // \returns pointer to the memory block.
+    void* Malloc(size_t size);
+
+    // Resize a memory block.
+    // \param originalPtr The pointer to current memory block. Null pointer is
+permitted.
+    // \param originalSize The current size in bytes. (Design issue: since some
+allocator may not book-keep this, explicitly pass to it can save memory.)
+    // \param newSize the new size in bytes.
+    void* Realloc(void* originalPtr, size_t originalSize, size_t newSize);
+
+    // Free a memory block.
+    // \param pointer to the memory block. Null pointer is permitted.
+    static void Free(void *ptr);
+};
+\endcode
+*/
+
+/*! \def RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-defined kDefaultChunkCapacity definition.
+
+    User can define this as any \c size that is a power of 2.
+*/
+
+#ifndef RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY
+#define RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY (64 * 1024)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// CrtAllocator
+
+//! C-runtime library allocator.
+/*! This class is just wrapper for standard C library memory routines.
+    \note implements Allocator concept
+*/
+class CrtAllocator {
+ public:
+  static const bool kNeedFree = true;
+  void *Malloc(size_t size) {
+    if (size)  //  behavior of malloc(0) is implementation defined.
+      return std::malloc(size);
+    else
+      return NULL;  // standardize to returning NULL.
+  }
+  void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
+    (void)originalSize;
+    if (newSize == 0) {
+      std::free(originalPtr);
+      return NULL;
+    }
+    return std::realloc(originalPtr, newSize);
+  }
+  static void Free(void *ptr) { std::free(ptr); }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// MemoryPoolAllocator
+
+//! Default memory allocator used by the parser and DOM.
+/*! This allocator allocate memory blocks from pre-allocated memory chunks.
+
+    It does not free memory blocks. And Realloc() only allocate new memory.
+
+    The memory chunks are allocated by BaseAllocator, which is CrtAllocator by
+   default.
+
+    User may also supply a buffer as the first chunk.
+
+    If the user-buffer is full then additional chunks are allocated by
+   BaseAllocator.
+
+    The user-buffer is not deallocated by this allocator.
+
+    \tparam BaseAllocator the allocator type for allocating memory chunks.
+   Default is CrtAllocator. \note implements Allocator concept
+*/
+template <typename BaseAllocator = CrtAllocator>
+class MemoryPoolAllocator {
+ public:
+  static const bool kNeedFree =
+      false;  //!< Tell users that no need to call Free() with this allocator.
+              //!< (concept Allocator)
+
+  //! Constructor with chunkSize.
+  /*! \param chunkSize The size of memory chunk. The default is
+     kDefaultChunkSize. \param baseAllocator The allocator for allocating memory
+     chunks.
+  */
+  MemoryPoolAllocator(size_t chunkSize = kDefaultChunkCapacity,
+                      BaseAllocator *baseAllocator = 0)
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(0),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {}
+
+  //! Constructor with user-supplied buffer.
+  /*! The user buffer will be used firstly. When it is full, memory pool
+     allocates new chunk with chunk size.
+
+      The user buffer will not be deallocated when this allocator is destructed.
+
+      \param buffer User supplied buffer.
+      \param size Size of the buffer in bytes. It must at least larger than
+     sizeof(ChunkHeader). \param chunkSize The size of memory chunk. The default
+     is kDefaultChunkSize. \param baseAllocator The allocator for allocating
+     memory chunks.
+  */
+  MemoryPoolAllocator(void *buffer, size_t size,
+                      size_t chunkSize = kDefaultChunkCapacity,
+                      BaseAllocator *baseAllocator = 0)
+      : chunkHead_(0),
+        chunk_capacity_(chunkSize),
+        userBuffer_(buffer),
+        baseAllocator_(baseAllocator),
+        ownBaseAllocator_(0) {
+    RAPIDJSON_ASSERT(buffer != 0);
+    RAPIDJSON_ASSERT(size > sizeof(ChunkHeader));
+    chunkHead_ = reinterpret_cast<ChunkHeader *>(buffer);
+    chunkHead_->capacity = size - sizeof(ChunkHeader);
+    chunkHead_->size = 0;
+    chunkHead_->next = 0;
+  }
+
+  //! Destructor.
+  /*! This deallocates all memory chunks, excluding the user-supplied buffer.
+   */
+  ~MemoryPoolAllocator() {
+    Clear();
+    RAPIDJSON_DELETE(ownBaseAllocator_);
+  }
+
+  //! Deallocates all memory chunks, excluding the user-supplied buffer.
+  void Clear() {
+    while (chunkHead_ && chunkHead_ != userBuffer_) {
+      ChunkHeader *next = chunkHead_->next;
+      baseAllocator_->Free(chunkHead_);
+      chunkHead_ = next;
+    }
+    if (chunkHead_ && chunkHead_ == userBuffer_)
+      chunkHead_->size = 0;  // Clear user buffer
+  }
+
+  //! Computes the total capacity of allocated memory chunks.
+  /*! \return total capacity in bytes.
+   */
+  size_t Capacity() const {
+    size_t capacity = 0;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next)
+      capacity += c->capacity;
+    return capacity;
+  }
+
+  //! Computes the memory blocks allocated.
+  /*! \return total used bytes.
+   */
+  size_t Size() const {
+    size_t size = 0;
+    for (ChunkHeader *c = chunkHead_; c != 0; c = c->next) size += c->size;
+    return size;
+  }
+
+  //! Allocates a memory block. (concept Allocator)
+  void *Malloc(size_t size) {
+    if (!size) return NULL;
+
+    size = RAPIDJSON_ALIGN(size);
+    if (chunkHead_ == 0 || chunkHead_->size + size > chunkHead_->capacity)
+      if (!AddChunk(chunk_capacity_ > size ? chunk_capacity_ : size))
+        return NULL;
+
+    void *buffer = reinterpret_cast<char *>(chunkHead_) +
+                   RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size;
+    chunkHead_->size += size;
+    return buffer;
+  }
+
+  //! Resizes a memory block (concept Allocator)
+  void *Realloc(void *originalPtr, size_t originalSize, size_t newSize) {
+    if (originalPtr == 0) return Malloc(newSize);
+
+    if (newSize == 0) return NULL;
+
+    originalSize = RAPIDJSON_ALIGN(originalSize);
+    newSize = RAPIDJSON_ALIGN(newSize);
+
+    // Do not shrink if new size is smaller than original
+    if (originalSize >= newSize) return originalPtr;
+
+    // Simply expand it if it is the last allocation and there is sufficient
+    // space
+    if (originalPtr ==
+        reinterpret_cast<char *>(chunkHead_) +
+            RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + chunkHead_->size -
+            originalSize) {
+      size_t increment = static_cast<size_t>(newSize - originalSize);
+      if (chunkHead_->size + increment <= chunkHead_->capacity) {
+        chunkHead_->size += increment;
+        return originalPtr;
+      }
+    }
+
+    // Realloc process: allocate and copy memory, do not free original buffer.
+    if (void *newBuffer = Malloc(newSize)) {
+      if (originalSize) std::memcpy(newBuffer, originalPtr, originalSize);
+      return newBuffer;
+    } else
+      return NULL;
+  }
+
+  //! Frees a memory block (concept Allocator)
+  static void Free(void *ptr) { (void)ptr; }  // Do nothing
+
+ private:
+  //! Copy constructor is not permitted.
+  MemoryPoolAllocator(const MemoryPoolAllocator &rhs) /* = delete */;
+  //! Copy assignment operator is not permitted.
+  MemoryPoolAllocator &operator=(const MemoryPoolAllocator &rhs) /* = delete */;
+
+  //! Creates a new chunk.
+  /*! \param capacity Capacity of the chunk in bytes.
+      \return true if success.
+  */
+  bool AddChunk(size_t capacity) {
+    if (!baseAllocator_)
+      ownBaseAllocator_ = baseAllocator_ = RAPIDJSON_NEW(BaseAllocator)();
+    if (ChunkHeader *chunk =
+            reinterpret_cast<ChunkHeader *>(baseAllocator_->Malloc(
+                RAPIDJSON_ALIGN(sizeof(ChunkHeader)) + capacity))) {
+      chunk->capacity = capacity;
+      chunk->size = 0;
+      chunk->next = chunkHead_;
+      chunkHead_ = chunk;
+      return true;
+    } else
+      return false;
+  }
+
+  static const int kDefaultChunkCapacity =
+      RAPIDJSON_ALLOCATOR_DEFAULT_CHUNK_CAPACITY;  //!< Default chunk capacity.
+
+  //! Chunk header for perpending to each chunk.
+  /*! Chunks are stored as a singly linked list.
+   */
+  struct ChunkHeader {
+    size_t capacity;  //!< Capacity of the chunk in bytes (excluding the header
+                      //!< itself).
+    size_t size;      //!< Current size of allocated memory in bytes.
+    ChunkHeader *next;  //!< Next chunk in the linked list.
+  };
+
+  ChunkHeader *chunkHead_;  //!< Head of the chunk linked-list. Only the head
+                            //!< chunk serves allocation.
+  size_t chunk_capacity_;   //!< The minimum capacity of chunk when they are
+                            //!< allocated.
+  void *userBuffer_;        //!< User supplied buffer.
+  BaseAllocator
+      *baseAllocator_;  //!< base allocator for allocating memory chunks.
+  BaseAllocator *ownBaseAllocator_;  //!< base allocator created by this object.
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ENCODINGS_H_

livox/common/rapidjson/cursorstreamwrapper.h

@@ -0,0 +1,81 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_CURSORSTREAMWRAPPER_H_
+#define RAPIDJSON_CURSORSTREAMWRAPPER_H_
+
+#include "stream.h"
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Cursor stream wrapper for counting line and column number if error exists.
+/*!
+    \tparam InputStream     Any stream that implements Stream Concept
+*/
+template <typename InputStream, typename Encoding = UTF8<>>
+class CursorStreamWrapper : public GenericStreamWrapper<InputStream, Encoding> {
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  CursorStreamWrapper(InputStream &is)
+      : GenericStreamWrapper<InputStream, Encoding>(is), line_(1), col_(0) {}
+
+  // counting line and column number
+  Ch Take() {
+    Ch ch = this->is_.Take();
+    if (ch == '\n') {
+      line_++;
+      col_ = 0;
+    } else {
+      col_++;
+    }
+    return ch;
+  }
+
+  //! Get the error line number, if error exists.
+  size_t GetLine() const { return line_; }
+  //! Get the error column number, if error exists.
+  size_t GetColumn() const { return col_; }
+
+ private:
+  size_t line_;  //!< Current Line
+  size_t col_;   //!< Current Column
+};
+
+#if defined(_MSC_VER) && _MSC_VER <= 1800
+RAPIDJSON_DIAG_POP
+#endif
+
+#if defined(__GNUC__)
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_CURSORSTREAMWRAPPER_H_

livox/common/rapidjson/encodedstream.h

@@ -0,0 +1,407 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ENCODEDSTREAM_H_
+#define RAPIDJSON_ENCODEDSTREAM_H_
+
+#include "memorystream.h"
+#include "stream.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Input byte stream wrapper with a statically bound encoding.
+/*!
+    \tparam Encoding The interpretation of encoding of the stream. Either UTF8,
+   UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam InputByteStream Type of input
+   byte stream. For example, FileReadStream.
+*/
+template <typename Encoding, typename InputByteStream>
+class EncodedInputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  EncodedInputStream(InputByteStream &is) : is_(is) {
+    current_ = Encoding::TakeBOM(is_);
+  }
+
+  Ch Peek() const { return current_; }
+  Ch Take() {
+    Ch c = current_;
+    current_ = Encoding::Take(is_);
+    return c;
+  }
+  size_t Tell() const { return is_.Tell(); }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  EncodedInputStream(const EncodedInputStream &);
+  EncodedInputStream &operator=(const EncodedInputStream &);
+
+  InputByteStream &is_;
+  Ch current_;
+};
+
+//! Specialized for UTF8 MemoryStream.
+template <>
+class EncodedInputStream<UTF8<>, MemoryStream> {
+ public:
+  typedef UTF8<>::Ch Ch;
+
+  EncodedInputStream(MemoryStream &is) : is_(is) {
+    if (static_cast<unsigned char>(is_.Peek()) == 0xEFu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBBu) is_.Take();
+    if (static_cast<unsigned char>(is_.Peek()) == 0xBFu) is_.Take();
+  }
+  Ch Peek() const { return is_.Peek(); }
+  Ch Take() { return is_.Take(); }
+  size_t Tell() const { return is_.Tell(); }
+
+  // Not implemented
+  void Put(Ch) {}
+  void Flush() {}
+  Ch *PutBegin() { return 0; }
+  size_t PutEnd(Ch *) { return 0; }
+
+  MemoryStream &is_;
+
+ private:
+  EncodedInputStream(const EncodedInputStream &);
+  EncodedInputStream &operator=(const EncodedInputStream &);
+};
+
+//! Output byte stream wrapper with statically bound encoding.
+/*!
+    \tparam Encoding The interpretation of encoding of the stream. Either UTF8,
+   UTF16LE, UTF16BE, UTF32LE, UTF32BE. \tparam OutputByteStream Type of input
+   byte stream. For example, FileWriteStream.
+*/
+template <typename Encoding, typename OutputByteStream>
+class EncodedOutputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+
+ public:
+  typedef typename Encoding::Ch Ch;
+
+  EncodedOutputStream(OutputByteStream &os, bool putBOM = true) : os_(os) {
+    if (putBOM) Encoding::PutBOM(os_);
+  }
+
+  void Put(Ch c) { Encoding::Put(os_, c); }
+  void Flush() { os_.Flush(); }
+
+  // Not implemented
+  Ch Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  EncodedOutputStream(const EncodedOutputStream &);
+  EncodedOutputStream &operator=(const EncodedOutputStream &);
+
+  OutputByteStream &os_;
+};
+
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
+  UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
+
+//! Input stream wrapper with dynamically bound encoding and automatic encoding
+//! detection.
+/*!
+    \tparam CharType Type of character for reading.
+    \tparam InputByteStream type of input byte stream to be wrapped.
+*/
+template <typename CharType, typename InputByteStream>
+class AutoUTFInputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+
+ public:
+  typedef CharType Ch;
+
+  //! Constructor.
+  /*!
+      \param is input stream to be wrapped.
+      \param type UTF encoding type if it is not detected from the stream.
+  */
+  AutoUTFInputStream(InputByteStream &is, UTFType type = kUTF8)
+      : is_(&is), type_(type), hasBOM_(false) {
+    RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
+    DetectType();
+    static const TakeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Take)};
+    takeFunc_ = f[type_];
+    current_ = takeFunc_(*is_);
+  }
+
+  UTFType GetType() const { return type_; }
+  bool HasBOM() const { return hasBOM_; }
+
+  Ch Peek() const { return current_; }
+  Ch Take() {
+    Ch c = current_;
+    current_ = takeFunc_(*is_);
+    return c;
+  }
+  size_t Tell() const { return is_->Tell(); }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  AutoUTFInputStream(const AutoUTFInputStream &);
+  AutoUTFInputStream &operator=(const AutoUTFInputStream &);
+
+  // Detect encoding type with BOM or RFC 4627
+  void DetectType() {
+    // BOM (Byte Order Mark):
+    // 00 00 FE FF  UTF-32BE
+    // FF FE 00 00  UTF-32LE
+    // FE FF        UTF-16BE
+    // FF FE        UTF-16LE
+    // EF BB BF     UTF-8
+
+    const unsigned char *c =
+        reinterpret_cast<const unsigned char *>(is_->Peek4());
+    if (!c) return;
+
+    unsigned bom =
+        static_cast<unsigned>(c[0] | (c[1] << 8) | (c[2] << 16) | (c[3] << 24));
+    hasBOM_ = false;
+    if (bom == 0xFFFE0000) {
+      type_ = kUTF32BE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    } else if (bom == 0x0000FEFF) {
+      type_ = kUTF32LE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFF) == 0xFFFE) {
+      type_ = kUTF16BE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFF) == 0xFEFF) {
+      type_ = kUTF16LE;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+    } else if ((bom & 0xFFFFFF) == 0xBFBBEF) {
+      type_ = kUTF8;
+      hasBOM_ = true;
+      is_->Take();
+      is_->Take();
+      is_->Take();
+    }
+
+    // RFC 4627: Section 3
+    // "Since the first two characters of a JSON text will always be ASCII
+    // characters [RFC0020], it is possible to determine whether an octet
+    // stream is UTF-8, UTF-16 (BE or LE), or UTF-32 (BE or LE) by looking
+    // at the pattern of nulls in the first four octets."
+    // 00 00 00 xx  UTF-32BE
+    // 00 xx 00 xx  UTF-16BE
+    // xx 00 00 00  UTF-32LE
+    // xx 00 xx 00  UTF-16LE
+    // xx xx xx xx  UTF-8
+
+    if (!hasBOM_) {
+      int pattern =
+          (c[0] ? 1 : 0) | (c[1] ? 2 : 0) | (c[2] ? 4 : 0) | (c[3] ? 8 : 0);
+      switch (pattern) {
+        case 0x08:
+          type_ = kUTF32BE;
+          break;
+        case 0x0A:
+          type_ = kUTF16BE;
+          break;
+        case 0x01:
+          type_ = kUTF32LE;
+          break;
+        case 0x05:
+          type_ = kUTF16LE;
+          break;
+        case 0x0F:
+          type_ = kUTF8;
+          break;
+        default:
+          break;  // Use type defined by user.
+      }
+    }
+
+    // Runtime check whether the size of character type is sufficient. It only
+    // perform checks with assertion.
+    if (type_ == kUTF16LE || type_ == kUTF16BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
+    if (type_ == kUTF32LE || type_ == kUTF32BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
+  }
+
+  typedef Ch (*TakeFunc)(InputByteStream &is);
+  InputByteStream *is_;
+  UTFType type_;
+  Ch current_;
+  TakeFunc takeFunc_;
+  bool hasBOM_;
+};
+
+//! Output stream wrapper with dynamically bound encoding and automatic encoding
+//! detection.
+/*!
+    \tparam CharType Type of character for writing.
+    \tparam OutputByteStream type of output byte stream to be wrapped.
+*/
+template <typename CharType, typename OutputByteStream>
+class AutoUTFOutputStream {
+  RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+
+ public:
+  typedef CharType Ch;
+
+  //! Constructor.
+  /*!
+      \param os output stream to be wrapped.
+      \param type UTF encoding type.
+      \param putBOM Whether to write BOM at the beginning of the stream.
+  */
+  AutoUTFOutputStream(OutputByteStream &os, UTFType type, bool putBOM)
+      : os_(&os), type_(type) {
+    RAPIDJSON_ASSERT(type >= kUTF8 && type <= kUTF32BE);
+
+    // Runtime check whether the size of character type is sufficient. It only
+    // perform checks with assertion.
+    if (type_ == kUTF16LE || type_ == kUTF16BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 2);
+    if (type_ == kUTF32LE || type_ == kUTF32BE)
+      RAPIDJSON_ASSERT(sizeof(Ch) >= 4);
+
+    static const PutFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Put)};
+    putFunc_ = f[type_];
+
+    if (putBOM) PutBOM();
+  }
+
+  UTFType GetType() const { return type_; }
+
+  void Put(Ch c) { putFunc_(*os_, c); }
+  void Flush() { os_->Flush(); }
+
+  // Not implemented
+  Ch Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  AutoUTFOutputStream(const AutoUTFOutputStream &);
+  AutoUTFOutputStream &operator=(const AutoUTFOutputStream &);
+
+  void PutBOM() {
+    typedef void (*PutBOMFunc)(OutputByteStream &);
+    static const PutBOMFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(PutBOM)};
+    f[type_](*os_);
+  }
+
+  typedef void (*PutFunc)(OutputByteStream &, Ch);
+
+  OutputByteStream *os_;
+  UTFType type_;
+  PutFunc putFunc_;
+};
+
+#undef RAPIDJSON_ENCODINGS_FUNC
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_

livox/common/rapidjson/encodings.h

@@ -0,0 +1,816 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ENCODINGS_H_
+#define RAPIDJSON_ENCODINGS_H_
+
+#include "rapidjson.h"
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(
+    4244)  // conversion from 'type1' to 'type2', possible loss of data
+RAPIDJSON_DIAG_OFF(4702)  // unreachable code
+#elif defined(__GNUC__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+RAPIDJSON_DIAG_OFF(overflow)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// Encoding
+
+/*! \class rapidjson::Encoding
+    \brief Concept for encoding of Unicode characters.
+
+\code
+concept Encoding {
+    typename Ch;    //! Type of character. A "character" is actually a code unit
+in unicode's definition.
+
+    enum { supportUnicode = 1 }; // or 0 if not supporting unicode
+
+    //! \brief Encode a Unicode codepoint to an output stream.
+    //! \param os Output stream.
+    //! \param codepoint An unicode codepoint, ranging from 0x0 to 0x10FFFF
+inclusively. template<typename OutputStream> static void Encode(OutputStream&
+os, unsigned codepoint);
+
+    //! \brief Decode a Unicode codepoint from an input stream.
+    //! \param is Input stream.
+    //! \param codepoint Output of the unicode codepoint.
+    //! \return true if a valid codepoint can be decoded from the stream.
+    template <typename InputStream>
+    static bool Decode(InputStream& is, unsigned* codepoint);
+
+    //! \brief Validate one Unicode codepoint from an encoded stream.
+    //! \param is Input stream to obtain codepoint.
+    //! \param os Output for copying one codepoint.
+    //! \return true if it is valid.
+    //! \note This function just validating and copying the codepoint without
+actually decode it. template <typename InputStream, typename OutputStream>
+    static bool Validate(InputStream& is, OutputStream& os);
+
+    // The following functions are deal with byte streams.
+
+    //! Take a character from input byte stream, skip BOM if exist.
+    template <typename InputByteStream>
+    static CharType TakeBOM(InputByteStream& is);
+
+    //! Take a character from input byte stream.
+    template <typename InputByteStream>
+    static Ch Take(InputByteStream& is);
+
+    //! Put BOM to output byte stream.
+    template <typename OutputByteStream>
+    static void PutBOM(OutputByteStream& os);
+
+    //! Put a character to output byte stream.
+    template <typename OutputByteStream>
+    static void Put(OutputByteStream& os, Ch c);
+};
+\endcode
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF8
+
+//! UTF-8 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-8
+    http://tools.ietf.org/html/rfc3629
+    \tparam CharType Code unit for storing 8-bit UTF-8 data. Default is char.
+    \note implements Encoding concept
+*/
+template <typename CharType = char>
+struct UTF8 {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    if (codepoint <= 0x7F)
+      os.Put(static_cast<Ch>(codepoint & 0xFF));
+    else if (codepoint <= 0x7FF) {
+      os.Put(static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
+    } else if (codepoint <= 0xFFFF) {
+      os.Put(static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      os.Put(static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      os.Put(static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    }
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    if (codepoint <= 0x7F)
+      PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
+    else if (codepoint <= 0x7FF) {
+      PutUnsafe(os, static_cast<Ch>(0xC0 | ((codepoint >> 6) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint & 0x3F))));
+    } else if (codepoint <= 0xFFFF) {
+      PutUnsafe(os, static_cast<Ch>(0xE0 | ((codepoint >> 12) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      PutUnsafe(os, static_cast<Ch>(0xF0 | ((codepoint >> 18) & 0xFF)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 12) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | ((codepoint >> 6) & 0x3F)));
+      PutUnsafe(os, static_cast<Ch>(0x80 | (codepoint & 0x3F)));
+    }
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+#define RAPIDJSON_COPY() \
+  c = is.Take();         \
+  *codepoint = (*codepoint << 6) | (static_cast<unsigned char>(c) & 0x3Fu)
+#define RAPIDJSON_TRANS(mask) \
+  result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
+#define RAPIDJSON_TAIL() \
+  RAPIDJSON_COPY();      \
+  RAPIDJSON_TRANS(0x70)
+    typename InputStream::Ch c = is.Take();
+    if (!(c & 0x80)) {
+      *codepoint = static_cast<unsigned char>(c);
+      return true;
+    }
+
+    unsigned char type = GetRange(static_cast<unsigned char>(c));
+    if (type >= 32) {
+      *codepoint = 0;
+    } else {
+      *codepoint = (0xFFu >> type) & static_cast<unsigned char>(c);
+    }
+    bool result = true;
+    switch (type) {
+      case 2:
+        RAPIDJSON_TAIL();
+        return result;
+      case 3:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 4:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TAIL();
+        return result;
+      case 5:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 6:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 10:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TAIL();
+        return result;
+      case 11:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      default:
+        return false;
+    }
+#undef RAPIDJSON_COPY
+#undef RAPIDJSON_TRANS
+#undef RAPIDJSON_TAIL
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+#define RAPIDJSON_COPY() os.Put(c = is.Take())
+#define RAPIDJSON_TRANS(mask) \
+  result &= ((GetRange(static_cast<unsigned char>(c)) & mask) != 0)
+#define RAPIDJSON_TAIL() \
+  RAPIDJSON_COPY();      \
+  RAPIDJSON_TRANS(0x70)
+    Ch c;
+    RAPIDJSON_COPY();
+    if (!(c & 0x80)) return true;
+
+    bool result = true;
+    switch (GetRange(static_cast<unsigned char>(c))) {
+      case 2:
+        RAPIDJSON_TAIL();
+        return result;
+      case 3:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 4:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x50);
+        RAPIDJSON_TAIL();
+        return result;
+      case 5:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x10);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 6:
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      case 10:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x20);
+        RAPIDJSON_TAIL();
+        return result;
+      case 11:
+        RAPIDJSON_COPY();
+        RAPIDJSON_TRANS(0x60);
+        RAPIDJSON_TAIL();
+        RAPIDJSON_TAIL();
+        return result;
+      default:
+        return false;
+    }
+#undef RAPIDJSON_COPY
+#undef RAPIDJSON_TRANS
+#undef RAPIDJSON_TAIL
+  }
+
+  static unsigned char GetRange(unsigned char c) {
+    // Referring to DFA of http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
+    // With new mapping 1 -> 0x10, 7 -> 0x20, 9 -> 0x40, such that AND operation
+    // can test multiple types.
+    static const unsigned char type[] = {
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
+        0,    0,    0,    0,    0,    0,    0,    0,    0x10, 0x10, 0x10, 0x10,
+        0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10,
+        0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40,
+        0x40, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
+        8,    8,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,
+        2,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,    2,
+        2,    2,    2,    2,    2,    2,    2,    2,    10,   3,    3,    3,
+        3,    3,    3,    3,    3,    3,    3,    3,    3,    4,    3,    3,
+        11,   6,    6,    6,    5,    8,    8,    8,    8,    8,    8,    8,
+        8,    8,    8,    8,
+    };
+    return type[c];
+  }
+
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    typename InputByteStream::Ch c = Take(is);
+    if (static_cast<unsigned char>(c) != 0xEFu) return c;
+    c = is.Take();
+    if (static_cast<unsigned char>(c) != 0xBBu) return c;
+    c = is.Take();
+    if (static_cast<unsigned char>(c) != 0xBFu) return c;
+    c = is.Take();
+    return c;
+  }
+
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    return static_cast<Ch>(is.Take());
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xEFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xBBu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xBFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, Ch c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF16
+
+//! UTF-16 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-16
+    http://tools.ietf.org/html/rfc2781
+    \tparam CharType Type for storing 16-bit UTF-16 data. Default is wchar_t.
+   C++11 may use char16_t instead. \note implements Encoding concept
+
+    \note For in-memory access, no need to concern endianness. The code units
+   and code points are represented by CPU's endianness. For streaming, use
+   UTF16LE and UTF16BE, which handle endianness.
+*/
+template <typename CharType = wchar_t>
+struct UTF16 {
+  typedef CharType Ch;
+  RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 2);
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    if (codepoint <= 0xFFFF) {
+      RAPIDJSON_ASSERT(
+          codepoint < 0xD800 ||
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
+      os.Put(static_cast<typename OutputStream::Ch>(codepoint));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      unsigned v = codepoint - 0x10000;
+      os.Put(static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
+      os.Put(static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
+    }
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    if (codepoint <= 0xFFFF) {
+      RAPIDJSON_ASSERT(
+          codepoint < 0xD800 ||
+          codepoint > 0xDFFF);  // Code point itself cannot be surrogate pair
+      PutUnsafe(os, static_cast<typename OutputStream::Ch>(codepoint));
+    } else {
+      RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+      unsigned v = codepoint - 0x10000;
+      PutUnsafe(os, static_cast<typename OutputStream::Ch>((v >> 10) | 0xD800));
+      PutUnsafe(os,
+                static_cast<typename OutputStream::Ch>((v & 0x3FF) | 0xDC00));
+    }
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
+    typename InputStream::Ch c = is.Take();
+    if (c < 0xD800 || c > 0xDFFF) {
+      *codepoint = static_cast<unsigned>(c);
+      return true;
+    } else if (c <= 0xDBFF) {
+      *codepoint = (static_cast<unsigned>(c) & 0x3FF) << 10;
+      c = is.Take();
+      *codepoint |= (static_cast<unsigned>(c) & 0x3FF);
+      *codepoint += 0x10000;
+      return c >= 0xDC00 && c <= 0xDFFF;
+    }
+    return false;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 2);
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 2);
+    typename InputStream::Ch c;
+    os.Put(static_cast<typename OutputStream::Ch>(c = is.Take()));
+    if (c < 0xD800 || c > 0xDFFF)
+      return true;
+    else if (c <= 0xDBFF) {
+      os.Put(c = is.Take());
+      return c >= 0xDC00 && c <= 0xDFFF;
+    }
+    return false;
+  }
+};
+
+//! UTF-16 little endian encoding.
+template <typename CharType = wchar_t>
+struct UTF16LE : UTF16<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<uint8_t>(is.Take());
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
+                                                      0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(
+        (static_cast<unsigned>(c) >> 8) & 0xFFu));
+  }
+};
+
+//! UTF-16 big endian encoding.
+template <typename CharType = wchar_t>
+struct UTF16BE : UTF16<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint16_t>(c) == 0xFEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(
+        (static_cast<unsigned>(c) >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(static_cast<unsigned>(c) &
+                                                      0xFFu));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// UTF32
+
+//! UTF-32 encoding.
+/*! http://en.wikipedia.org/wiki/UTF-32
+    \tparam CharType Type for storing 32-bit UTF-32 data. Default is unsigned.
+   C++11 may use char32_t instead. \note implements Encoding concept
+
+    \note For in-memory access, no need to concern endianness. The code units
+   and code points are represented by CPU's endianness. For streaming, use
+   UTF32LE and UTF32BE, which handle endianness.
+*/
+template <typename CharType = unsigned>
+struct UTF32 {
+  typedef CharType Ch;
+  RAPIDJSON_STATIC_ASSERT(sizeof(Ch) >= 4);
+
+  enum { supportUnicode = 1 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
+    RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+    os.Put(codepoint);
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputStream::Ch) >= 4);
+    RAPIDJSON_ASSERT(codepoint <= 0x10FFFF);
+    PutUnsafe(os, codepoint);
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
+    Ch c = is.Take();
+    *codepoint = c;
+    return c <= 0x10FFFF;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputStream::Ch) >= 4);
+    Ch c;
+    os.Put(c = is.Take());
+    return c <= 0x10FFFF;
+  }
+};
+
+//! UTF-32 little endian enocoding.
+template <typename CharType = unsigned>
+struct UTF32LE : UTF32<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<uint8_t>(is.Take());
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
+  }
+};
+
+//! UTF-32 big endian encoding.
+template <typename CharType = unsigned>
+struct UTF32BE : UTF32<CharType> {
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    CharType c = Take(is);
+    return static_cast<uint32_t>(c) == 0x0000FEFFu ? Take(is) : c;
+  }
+
+  template <typename InputByteStream>
+  static CharType Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    unsigned c = static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 24;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 16;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take())) << 8;
+    c |= static_cast<unsigned>(static_cast<uint8_t>(is.Take()));
+    return static_cast<CharType>(c);
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0x00u));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFEu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(0xFFu));
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, CharType c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 24) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 16) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>((c >> 8) & 0xFFu));
+    os.Put(static_cast<typename OutputByteStream::Ch>(c & 0xFFu));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// ASCII
+
+//! ASCII encoding.
+/*! http://en.wikipedia.org/wiki/ASCII
+    \tparam CharType Code unit for storing 7-bit ASCII data. Default is char.
+    \note implements Encoding concept
+*/
+template <typename CharType = char>
+struct ASCII {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 0 };
+
+  template <typename OutputStream>
+  static void Encode(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_ASSERT(codepoint <= 0x7F);
+    os.Put(static_cast<Ch>(codepoint & 0xFF));
+  }
+
+  template <typename OutputStream>
+  static void EncodeUnsafe(OutputStream &os, unsigned codepoint) {
+    RAPIDJSON_ASSERT(codepoint <= 0x7F);
+    PutUnsafe(os, static_cast<Ch>(codepoint & 0xFF));
+  }
+
+  template <typename InputStream>
+  static bool Decode(InputStream &is, unsigned *codepoint) {
+    uint8_t c = static_cast<uint8_t>(is.Take());
+    *codepoint = c;
+    return c <= 0X7F;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static bool Validate(InputStream &is, OutputStream &os) {
+    uint8_t c = static_cast<uint8_t>(is.Take());
+    os.Put(static_cast<typename OutputStream::Ch>(c));
+    return c <= 0x7F;
+  }
+
+  template <typename InputByteStream>
+  static CharType TakeBOM(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    uint8_t c = static_cast<uint8_t>(Take(is));
+    return static_cast<Ch>(c);
+  }
+
+  template <typename InputByteStream>
+  static Ch Take(InputByteStream &is) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename InputByteStream::Ch) == 1);
+    return static_cast<Ch>(is.Take());
+  }
+
+  template <typename OutputByteStream>
+  static void PutBOM(OutputByteStream &os) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    (void)os;
+  }
+
+  template <typename OutputByteStream>
+  static void Put(OutputByteStream &os, Ch c) {
+    RAPIDJSON_STATIC_ASSERT(sizeof(typename OutputByteStream::Ch) == 1);
+    os.Put(static_cast<typename OutputByteStream::Ch>(c));
+  }
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// AutoUTF
+
+//! Runtime-specified UTF encoding type of a stream.
+enum UTFType {
+  kUTF8 = 0,     //!< UTF-8.
+  kUTF16LE = 1,  //!< UTF-16 little endian.
+  kUTF16BE = 2,  //!< UTF-16 big endian.
+  kUTF32LE = 3,  //!< UTF-32 little endian.
+  kUTF32BE = 4   //!< UTF-32 big endian.
+};
+
+//! Dynamically select encoding according to stream's runtime-specified UTF
+//! encoding type.
+/*! \note This class can be used with AutoUTFInputtStream and
+ * AutoUTFOutputStream, which provides GetType().
+ */
+template <typename CharType>
+struct AutoUTF {
+  typedef CharType Ch;
+
+  enum { supportUnicode = 1 };
+
+#define RAPIDJSON_ENCODINGS_FUNC(x) \
+  UTF8<Ch>::x, UTF16LE<Ch>::x, UTF16BE<Ch>::x, UTF32LE<Ch>::x, UTF32BE<Ch>::x
+
+  template <typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void Encode(OutputStream &os,
+                                           unsigned codepoint) {
+    typedef void (*EncodeFunc)(OutputStream &, unsigned);
+    static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Encode)};
+    (*f[os.GetType()])(os, codepoint);
+  }
+
+  template <typename OutputStream>
+  static RAPIDJSON_FORCEINLINE void EncodeUnsafe(OutputStream &os,
+                                                 unsigned codepoint) {
+    typedef void (*EncodeFunc)(OutputStream &, unsigned);
+    static const EncodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(EncodeUnsafe)};
+    (*f[os.GetType()])(os, codepoint);
+  }
+
+  template <typename InputStream>
+  static RAPIDJSON_FORCEINLINE bool Decode(InputStream &is,
+                                           unsigned *codepoint) {
+    typedef bool (*DecodeFunc)(InputStream &, unsigned *);
+    static const DecodeFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Decode)};
+    return (*f[is.GetType()])(is, codepoint);
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    typedef bool (*ValidateFunc)(InputStream &, OutputStream &);
+    static const ValidateFunc f[] = {RAPIDJSON_ENCODINGS_FUNC(Validate)};
+    return (*f[is.GetType()])(is, os);
+  }
+
+#undef RAPIDJSON_ENCODINGS_FUNC
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// Transcoder
+
+//! Encoding conversion.
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder {
+  //! Take one Unicode codepoint from source encoding, convert it to target
+  //! encoding and put it to the output stream.
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
+                                              OutputStream &os) {
+    unsigned codepoint;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
+    TargetEncoding::Encode(os, codepoint);
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
+                                                    OutputStream &os) {
+    unsigned codepoint;
+    if (!SourceEncoding::Decode(is, &codepoint)) return false;
+    TargetEncoding::EncodeUnsafe(os, codepoint);
+    return true;
+  }
+
+  //! Validate one Unicode codepoint from an encoded stream.
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    return Transcode(
+        is, os);  // Since source/target encoding is different, must transcode.
+  }
+};
+
+// Forward declaration.
+template <typename Stream>
+inline void PutUnsafe(Stream &stream, typename Stream::Ch c);
+
+//! Specialization of Transcoder with same source and target encoding.
+template <typename Encoding>
+struct Transcoder<Encoding, Encoding> {
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Transcode(InputStream &is,
+                                              OutputStream &os) {
+    os.Put(is.Take());  // Just copy one code unit. This semantic is different
+                        // from primary template class.
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool TranscodeUnsafe(InputStream &is,
+                                                    OutputStream &os) {
+    PutUnsafe(os, is.Take());  // Just copy one code unit. This semantic is
+                               // different from primary template class.
+    return true;
+  }
+
+  template <typename InputStream, typename OutputStream>
+  static RAPIDJSON_FORCEINLINE bool Validate(InputStream &is,
+                                             OutputStream &os) {
+    return Encoding::Validate(is, os);  // source/target encoding are the same
+  }
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__GNUC__) || (defined(_MSC_VER) && !defined(__clang__))
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ENCODINGS_H_

livox/common/rapidjson/error/en.h

@@ -0,0 +1,104 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ERROR_EN_H_
+#define RAPIDJSON_ERROR_EN_H_
+
+#include "error.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(switch - enum)
+RAPIDJSON_DIAG_OFF(covered - switch - default)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Maps error code of parsing into error message.
+/*!
+    \ingroup RAPIDJSON_ERRORS
+    \param parseErrorCode Error code obtained in parsing.
+    \return the error message.
+    \note User can make a copy of this function for localization.
+        Using switch-case is safer for future modification of error codes.
+*/
+inline const RAPIDJSON_ERROR_CHARTYPE* GetParseError_En(
+    ParseErrorCode parseErrorCode) {
+  switch (parseErrorCode) {
+    case kParseErrorNone:
+      return RAPIDJSON_ERROR_STRING("No error.");
+
+    case kParseErrorDocumentEmpty:
+      return RAPIDJSON_ERROR_STRING("The document is empty.");
+    case kParseErrorDocumentRootNotSingular:
+      return RAPIDJSON_ERROR_STRING(
+          "The document root must not be followed by other values.");
+
+    case kParseErrorValueInvalid:
+      return RAPIDJSON_ERROR_STRING("Invalid value.");
+
+    case kParseErrorObjectMissName:
+      return RAPIDJSON_ERROR_STRING("Missing a name for object member.");
+    case kParseErrorObjectMissColon:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a colon after a name of object member.");
+    case kParseErrorObjectMissCommaOrCurlyBracket:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a comma or '}' after an object member.");
+
+    case kParseErrorArrayMissCommaOrSquareBracket:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a comma or ']' after an array element.");
+
+    case kParseErrorStringUnicodeEscapeInvalidHex:
+      return RAPIDJSON_ERROR_STRING(
+          "Incorrect hex digit after \\u escape in string.");
+    case kParseErrorStringUnicodeSurrogateInvalid:
+      return RAPIDJSON_ERROR_STRING("The surrogate pair in string is invalid.");
+    case kParseErrorStringEscapeInvalid:
+      return RAPIDJSON_ERROR_STRING("Invalid escape character in string.");
+    case kParseErrorStringMissQuotationMark:
+      return RAPIDJSON_ERROR_STRING(
+          "Missing a closing quotation mark in string.");
+    case kParseErrorStringInvalidEncoding:
+      return RAPIDJSON_ERROR_STRING("Invalid encoding in string.");
+
+    case kParseErrorNumberTooBig:
+      return RAPIDJSON_ERROR_STRING("Number too big to be stored in double.");
+    case kParseErrorNumberMissFraction:
+      return RAPIDJSON_ERROR_STRING("Miss fraction part in number.");
+    case kParseErrorNumberMissExponent:
+      return RAPIDJSON_ERROR_STRING("Miss exponent in number.");
+
+    case kParseErrorTermination:
+      return RAPIDJSON_ERROR_STRING("Terminate parsing due to Handler error.");
+    case kParseErrorUnspecificSyntaxError:
+      return RAPIDJSON_ERROR_STRING("Unspecific syntax error.");
+
+    default:
+      return RAPIDJSON_ERROR_STRING("Unknown error.");
+  }
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ERROR_EN_H_

livox/common/rapidjson/error/error.h

@@ -0,0 +1,186 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ERROR_ERROR_H_
+#define RAPIDJSON_ERROR_ERROR_H_
+
+#include "../rapidjson.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+/*! \file error.h */
+
+/*! \defgroup RAPIDJSON_ERRORS RapidJSON error handling */
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ERROR_CHARTYPE
+
+//! Character type of error messages.
+/*! \ingroup RAPIDJSON_ERRORS
+    The default character type is \c char.
+    On Windows, user can define this macro as \c TCHAR for supporting both
+    unicode/non-unicode settings.
+*/
+#ifndef RAPIDJSON_ERROR_CHARTYPE
+#define RAPIDJSON_ERROR_CHARTYPE char
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ERROR_STRING
+
+//! Macro for converting string literial to \ref RAPIDJSON_ERROR_CHARTYPE[].
+/*! \ingroup RAPIDJSON_ERRORS
+    By default this conversion macro does nothing.
+    On Windows, user can define this macro as \c _T(x) for supporting both
+    unicode/non-unicode settings.
+*/
+#ifndef RAPIDJSON_ERROR_STRING
+#define RAPIDJSON_ERROR_STRING(x) x
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+///////////////////////////////////////////////////////////////////////////////
+// ParseErrorCode
+
+//! Error code of parsing.
+/*! \ingroup RAPIDJSON_ERRORS
+    \see GenericReader::Parse, GenericReader::GetParseErrorCode
+*/
+enum ParseErrorCode {
+  kParseErrorNone = 0,  //!< No error.
+
+  kParseErrorDocumentEmpty,            //!< The document is empty.
+  kParseErrorDocumentRootNotSingular,  //!< The document root must not follow by
+                                       //!< other values.
+
+  kParseErrorValueInvalid,  //!< Invalid value.
+
+  kParseErrorObjectMissName,   //!< Missing a name for object member.
+  kParseErrorObjectMissColon,  //!< Missing a colon after a name of object
+                               //!< member.
+  kParseErrorObjectMissCommaOrCurlyBracket,  //!< Missing a comma or '}' after
+                                             //!an
+                                             //!< object member.
+
+  kParseErrorArrayMissCommaOrSquareBracket,  //!< Missing a comma or ']' after
+                                             //!an
+                                             //!< array element.
+
+  kParseErrorStringUnicodeEscapeInvalidHex,  //!< Incorrect hex digit after \\u
+                                             //!< escape in string.
+  kParseErrorStringUnicodeSurrogateInvalid,  //!< The surrogate pair in string
+                                             //!is
+                                             //!< invalid.
+  kParseErrorStringEscapeInvalid,      //!< Invalid escape character in string.
+  kParseErrorStringMissQuotationMark,  //!< Missing a closing quotation mark in
+                                       //!< string.
+  kParseErrorStringInvalidEncoding,    //!< Invalid encoding in string.
+
+  kParseErrorNumberTooBig,        //!< Number too big to be stored in double.
+  kParseErrorNumberMissFraction,  //!< Miss fraction part in number.
+  kParseErrorNumberMissExponent,  //!< Miss exponent in number.
+
+  kParseErrorTermination,           //!< Parsing was terminated.
+  kParseErrorUnspecificSyntaxError  //!< Unspecific syntax error.
+};
+
+//! Result of parsing (wraps ParseErrorCode)
+/*!
+    \ingroup RAPIDJSON_ERRORS
+    \code
+        Document doc;
+        ParseResult ok = doc.Parse("[42]");
+        if (!ok) {
+            fprintf(stderr, "JSON parse error: %s (%u)",
+                    GetParseError_En(ok.Code()), ok.Offset());
+            exit(EXIT_FAILURE);
+        }
+    \endcode
+    \see GenericReader::Parse, GenericDocument::Parse
+*/
+struct ParseResult {
+  //!! Unspecified boolean type
+  typedef bool (ParseResult::*BooleanType)() const;
+
+ public:
+  //! Default constructor, no error.
+  ParseResult() : code_(kParseErrorNone), offset_(0) {}
+  //! Constructor to set an error.
+  ParseResult(ParseErrorCode code, size_t offset)
+      : code_(code), offset_(offset) {}
+
+  //! Get the error code.
+  ParseErrorCode Code() const { return code_; }
+  //! Get the error offset, if \ref IsError(), 0 otherwise.
+  size_t Offset() const { return offset_; }
+
+  //! Explicit conversion to \c bool, returns \c true, iff !\ref IsError().
+  operator BooleanType() const {
+    return !IsError() ? &ParseResult::IsError : NULL;
+  }
+  //! Whether the result is an error.
+  bool IsError() const { return code_ != kParseErrorNone; }
+
+  bool operator==(const ParseResult &that) const { return code_ == that.code_; }
+  bool operator==(ParseErrorCode code) const { return code_ == code; }
+  friend bool operator==(ParseErrorCode code, const ParseResult &err) {
+    return code == err.code_;
+  }
+
+  bool operator!=(const ParseResult &that) const { return !(*this == that); }
+  bool operator!=(ParseErrorCode code) const { return !(*this == code); }
+  friend bool operator!=(ParseErrorCode code, const ParseResult &err) {
+    return err != code;
+  }
+
+  //! Reset error code.
+  void Clear() { Set(kParseErrorNone); }
+  //! Update error code and offset.
+  void Set(ParseErrorCode code, size_t offset = 0) {
+    code_ = code;
+    offset_ = offset;
+  }
+
+ private:
+  ParseErrorCode code_;
+  size_t offset_;
+};
+
+//! Function pointer type of GetParseError().
+/*! \ingroup RAPIDJSON_ERRORS
+
+    This is the prototype for \c GetParseError_X(), where \c X is a locale.
+    User can dynamically change locale in runtime, e.g.:
+\code
+    GetParseErrorFunc GetParseError = GetParseError_En; // or whatever
+    const RAPIDJSON_ERROR_CHARTYPE* s =
+GetParseError(document.GetParseErrorCode()); \endcode
+*/
+typedef const RAPIDJSON_ERROR_CHARTYPE *(*GetParseErrorFunc)(ParseErrorCode);
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_ERROR_ERROR_H_

livox/common/rapidjson/filereadstream.h

@@ -0,0 +1,123 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_FILEREADSTREAM_H_
+#define RAPIDJSON_FILEREADSTREAM_H_
+
+#include <cstdio>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(missing - noreturn)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! File byte stream for input using fread().
+/*!
+    \note implements Stream concept
+*/
+class FileReadStream {
+ public:
+  typedef char Ch;  //!< Character type (byte).
+
+  //! Constructor.
+  /*!
+      \param fp File pointer opened for read.
+      \param buffer user-supplied buffer.
+      \param bufferSize size of buffer in bytes. Must >=4 bytes.
+  */
+  FileReadStream(std::FILE *fp, char *buffer, size_t bufferSize)
+      : fp_(fp),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    RAPIDJSON_ASSERT(fp_ != 0);
+    RAPIDJSON_ASSERT(bufferSize >= 4);
+    Read();
+  }
+
+  Ch Peek() const { return *current_; }
+  Ch Take() {
+    Ch c = *current_;
+    Read();
+    return c;
+  }
+  size_t Tell() const {
+    return count_ + static_cast<size_t>(current_ - buffer_);
+  }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const {
+    return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
+  }
+
+ private:
+  void Read() {
+    if (current_ < bufferLast_)
+      ++current_;
+    else if (!eof_) {
+      count_ += readCount_;
+      readCount_ = std::fread(buffer_, 1, bufferSize_, fp_);
+      bufferLast_ = buffer_ + readCount_ - 1;
+      current_ = buffer_;
+
+      if (readCount_ < bufferSize_) {
+        buffer_[readCount_] = '\0';
+        ++bufferLast_;
+        eof_ = true;
+      }
+    }
+  }
+
+  std::FILE *fp_;
+  Ch *buffer_;
+  size_t bufferSize_;
+  Ch *bufferLast_;
+  Ch *current_;
+  size_t readCount_;
+  size_t count_;  //!< Number of characters read
+  bool eof_;
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_

livox/common/rapidjson/filewritestream.h

@@ -0,0 +1,128 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_FILEWRITESTREAM_H_
+#define RAPIDJSON_FILEWRITESTREAM_H_
+
+#include <cstdio>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(unreachable - code)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of C file stream for output using fwrite().
+/*!
+    \note implements Stream concept
+*/
+class FileWriteStream {
+ public:
+  typedef char Ch;  //!< Character type. Only support char.
+
+  FileWriteStream(std::FILE *fp, char *buffer, size_t bufferSize)
+      : fp_(fp),
+        buffer_(buffer),
+        bufferEnd_(buffer + bufferSize),
+        current_(buffer_) {
+    RAPIDJSON_ASSERT(fp_ != 0);
+  }
+
+  void Put(char c) {
+    if (current_ >= bufferEnd_) Flush();
+
+    *current_++ = c;
+  }
+
+  void PutN(char c, size_t n) {
+    size_t avail = static_cast<size_t>(bufferEnd_ - current_);
+    while (n > avail) {
+      std::memset(current_, c, avail);
+      current_ += avail;
+      Flush();
+      n -= avail;
+      avail = static_cast<size_t>(bufferEnd_ - current_);
+    }
+
+    if (n > 0) {
+      std::memset(current_, c, n);
+      current_ += n;
+    }
+  }
+
+  void Flush() {
+    if (current_ != buffer_) {
+      size_t result =
+          std::fwrite(buffer_, 1, static_cast<size_t>(current_ - buffer_), fp_);
+      if (result < static_cast<size_t>(current_ - buffer_)) {
+        // failure deliberately ignored at this time
+        // added to avoid warn_unused_result build errors
+      }
+      current_ = buffer_;
+    }
+  }
+
+  // Not implemented
+  char Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(char *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  FileWriteStream(const FileWriteStream &);
+  FileWriteStream &operator=(const FileWriteStream &);
+
+  std::FILE *fp_;
+  char *buffer_;
+  char *bufferEnd_;
+  char *current_;
+};
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(FileWriteStream &stream, char c, size_t n) {
+  stream.PutN(c, n);
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_FILESTREAM_H_

livox/common/rapidjson/fwd.h

@@ -0,0 +1,170 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_FWD_H_
+#define RAPIDJSON_FWD_H_
+
+#include "rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+// encodings.h
+
+template <typename CharType>
+struct UTF8;
+template <typename CharType>
+struct UTF16;
+template <typename CharType>
+struct UTF16BE;
+template <typename CharType>
+struct UTF16LE;
+template <typename CharType>
+struct UTF32;
+template <typename CharType>
+struct UTF32BE;
+template <typename CharType>
+struct UTF32LE;
+template <typename CharType>
+struct ASCII;
+template <typename CharType>
+struct AutoUTF;
+
+template <typename SourceEncoding, typename TargetEncoding>
+struct Transcoder;
+
+// allocators.h
+
+class CrtAllocator;
+
+template <typename BaseAllocator>
+class MemoryPoolAllocator;
+
+// stream.h
+
+template <typename Encoding>
+struct GenericStringStream;
+
+typedef GenericStringStream<UTF8<char>> StringStream;
+
+template <typename Encoding>
+struct GenericInsituStringStream;
+
+typedef GenericInsituStringStream<UTF8<char>> InsituStringStream;
+
+// stringbuffer.h
+
+template <typename Encoding, typename Allocator>
+class GenericStringBuffer;
+
+typedef GenericStringBuffer<UTF8<char>, CrtAllocator> StringBuffer;
+
+// filereadstream.h
+
+class FileReadStream;
+
+// filewritestream.h
+
+class FileWriteStream;
+
+// memorybuffer.h
+
+template <typename Allocator>
+struct GenericMemoryBuffer;
+
+typedef GenericMemoryBuffer<CrtAllocator> MemoryBuffer;
+
+// memorystream.h
+
+struct MemoryStream;
+
+// reader.h
+
+template <typename Encoding, typename Derived>
+struct BaseReaderHandler;
+
+template <typename SourceEncoding, typename TargetEncoding,
+          typename StackAllocator>
+class GenericReader;
+
+typedef GenericReader<UTF8<char>, UTF8<char>, CrtAllocator> Reader;
+
+// writer.h
+
+template <typename OutputStream, typename SourceEncoding,
+          typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
+class Writer;
+
+// prettywriter.h
+
+template <typename OutputStream, typename SourceEncoding,
+          typename TargetEncoding, typename StackAllocator, unsigned writeFlags>
+class PrettyWriter;
+
+// document.h
+
+template <typename Encoding, typename Allocator>
+class GenericMember;
+
+template <bool Const, typename Encoding, typename Allocator>
+class GenericMemberIterator;
+
+template <typename CharType>
+struct GenericStringRef;
+
+template <typename Encoding, typename Allocator>
+class GenericValue;
+
+typedef GenericValue<UTF8<char>, MemoryPoolAllocator<CrtAllocator>> Value;
+
+template <typename Encoding, typename Allocator, typename StackAllocator>
+class GenericDocument;
+
+typedef GenericDocument<UTF8<char>, MemoryPoolAllocator<CrtAllocator>,
+                        CrtAllocator>
+    Document;
+
+// pointer.h
+
+template <typename ValueType, typename Allocator>
+class GenericPointer;
+
+typedef GenericPointer<Value, CrtAllocator> Pointer;
+
+// schema.h
+
+template <typename SchemaDocumentType>
+class IGenericRemoteSchemaDocumentProvider;
+
+template <typename ValueT, typename Allocator>
+class GenericSchemaDocument;
+
+typedef GenericSchemaDocument<Value, CrtAllocator> SchemaDocument;
+typedef IGenericRemoteSchemaDocumentProvider<SchemaDocument>
+    IRemoteSchemaDocumentProvider;
+
+template <typename SchemaDocumentType, typename OutputHandler,
+          typename StateAllocator>
+class GenericSchemaValidator;
+
+typedef GenericSchemaValidator<
+    SchemaDocument, BaseReaderHandler<UTF8<char>, void>, CrtAllocator>
+    SchemaValidator;
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_RAPIDJSONFWD_H_

livox/common/rapidjson/internal/biginteger.h

@@ -0,0 +1,295 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_BIGINTEGER_H_
+#define RAPIDJSON_BIGINTEGER_H_
+
+#include "../rapidjson.h"
+
+#if defined(_MSC_VER) && !__INTEL_COMPILER && defined(_M_AMD64)
+#include <intrin.h>  // for _umul128
+#pragma intrinsic(_umul128)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+class BigInteger {
+ public:
+  typedef uint64_t Type;
+
+  BigInteger(const BigInteger &rhs) : count_(rhs.count_) {
+    std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
+  }
+
+  explicit BigInteger(uint64_t u) : count_(1) { digits_[0] = u; }
+
+  BigInteger(const char *decimals, size_t length) : count_(1) {
+    RAPIDJSON_ASSERT(length > 0);
+    digits_[0] = 0;
+    size_t i = 0;
+    const size_t kMaxDigitPerIteration =
+        19;  // 2^64 = 18446744073709551616 > 10^19
+    while (length >= kMaxDigitPerIteration) {
+      AppendDecimal64(decimals + i, decimals + i + kMaxDigitPerIteration);
+      length -= kMaxDigitPerIteration;
+      i += kMaxDigitPerIteration;
+    }
+
+    if (length > 0) AppendDecimal64(decimals + i, decimals + i + length);
+  }
+
+  BigInteger &operator=(const BigInteger &rhs) {
+    if (this != &rhs) {
+      count_ = rhs.count_;
+      std::memcpy(digits_, rhs.digits_, count_ * sizeof(Type));
+    }
+    return *this;
+  }
+
+  BigInteger &operator=(uint64_t u) {
+    digits_[0] = u;
+    count_ = 1;
+    return *this;
+  }
+
+  BigInteger &operator+=(uint64_t u) {
+    Type backup = digits_[0];
+    digits_[0] += u;
+    for (size_t i = 0; i < count_ - 1; i++) {
+      if (digits_[i] >= backup) return *this;  // no carry
+      backup = digits_[i + 1];
+      digits_[i + 1] += 1;
+    }
+
+    // Last carry
+    if (digits_[count_ - 1] < backup) PushBack(1);
+
+    return *this;
+  }
+
+  BigInteger &operator*=(uint64_t u) {
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
+
+    uint64_t k = 0;
+    for (size_t i = 0; i < count_; i++) {
+      uint64_t hi;
+      digits_[i] = MulAdd64(digits_[i], u, k, &hi);
+      k = hi;
+    }
+
+    if (k > 0) PushBack(k);
+
+    return *this;
+  }
+
+  BigInteger &operator*=(uint32_t u) {
+    if (u == 0) return *this = 0;
+    if (u == 1) return *this;
+    if (*this == 1) return *this = u;
+
+    uint64_t k = 0;
+    for (size_t i = 0; i < count_; i++) {
+      const uint64_t c = digits_[i] >> 32;
+      const uint64_t d = digits_[i] & 0xFFFFFFFF;
+      const uint64_t uc = u * c;
+      const uint64_t ud = u * d;
+      const uint64_t p0 = ud + k;
+      const uint64_t p1 = uc + (p0 >> 32);
+      digits_[i] = (p0 & 0xFFFFFFFF) | (p1 << 32);
+      k = p1 >> 32;
+    }
+
+    if (k > 0) PushBack(k);
+
+    return *this;
+  }
+
+  BigInteger &operator<<=(size_t shift) {
+    if (IsZero() || shift == 0) return *this;
+
+    size_t offset = shift / kTypeBit;
+    size_t interShift = shift % kTypeBit;
+    RAPIDJSON_ASSERT(count_ + offset <= kCapacity);
+
+    if (interShift == 0) {
+      std::memmove(digits_ + offset, digits_, count_ * sizeof(Type));
+      count_ += offset;
+    } else {
+      digits_[count_] = 0;
+      for (size_t i = count_; i > 0; i--)
+        digits_[i + offset] = (digits_[i] << interShift) |
+                              (digits_[i - 1] >> (kTypeBit - interShift));
+      digits_[offset] = digits_[0] << interShift;
+      count_ += offset;
+      if (digits_[count_]) count_++;
+    }
+
+    std::memset(digits_, 0, offset * sizeof(Type));
+
+    return *this;
+  }
+
+  bool operator==(const BigInteger &rhs) const {
+    return count_ == rhs.count_ &&
+           std::memcmp(digits_, rhs.digits_, count_ * sizeof(Type)) == 0;
+  }
+
+  bool operator==(const Type rhs) const {
+    return count_ == 1 && digits_[0] == rhs;
+  }
+
+  BigInteger &MultiplyPow5(unsigned exp) {
+    static const uint32_t kPow5[12] = {
+        5,
+        5 * 5,
+        5 * 5 * 5,
+        5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5,
+        5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5 * 5};
+    if (exp == 0) return *this;
+    for (; exp >= 27; exp -= 27)
+      *this *= RAPIDJSON_UINT64_C2(0X6765C793, 0XFA10079D);  // 5^27
+    for (; exp >= 13; exp -= 13)
+      *this *= static_cast<uint32_t>(1220703125u);  // 5^13
+    if (exp > 0) *this *= kPow5[exp - 1];
+    return *this;
+  }
+
+  // Compute absolute difference of this and rhs.
+  // Assume this != rhs
+  bool Difference(const BigInteger &rhs, BigInteger *out) const {
+    int cmp = Compare(rhs);
+    RAPIDJSON_ASSERT(cmp != 0);
+    const BigInteger *a, *b;  // Makes a > b
+    bool ret;
+    if (cmp < 0) {
+      a = &rhs;
+      b = this;
+      ret = true;
+    } else {
+      a = this;
+      b = &rhs;
+      ret = false;
+    }
+
+    Type borrow = 0;
+    for (size_t i = 0; i < a->count_; i++) {
+      Type d = a->digits_[i] - borrow;
+      if (i < b->count_) d -= b->digits_[i];
+      borrow = (d > a->digits_[i]) ? 1 : 0;
+      out->digits_[i] = d;
+      if (d != 0) out->count_ = i + 1;
+    }
+
+    return ret;
+  }
+
+  int Compare(const BigInteger &rhs) const {
+    if (count_ != rhs.count_) return count_ < rhs.count_ ? -1 : 1;
+
+    for (size_t i = count_; i-- > 0;)
+      if (digits_[i] != rhs.digits_[i])
+        return digits_[i] < rhs.digits_[i] ? -1 : 1;
+
+    return 0;
+  }
+
+  size_t GetCount() const { return count_; }
+  Type GetDigit(size_t index) const {
+    RAPIDJSON_ASSERT(index < count_);
+    return digits_[index];
+  }
+  bool IsZero() const { return count_ == 1 && digits_[0] == 0; }
+
+ private:
+  void AppendDecimal64(const char *begin, const char *end) {
+    uint64_t u = ParseUint64(begin, end);
+    if (IsZero())
+      *this = u;
+    else {
+      unsigned exp = static_cast<unsigned>(end - begin);
+      (MultiplyPow5(exp) <<= exp) += u;  // *this = *this * 10^exp + u
+    }
+  }
+
+  void PushBack(Type digit) {
+    RAPIDJSON_ASSERT(count_ < kCapacity);
+    digits_[count_++] = digit;
+  }
+
+  static uint64_t ParseUint64(const char *begin, const char *end) {
+    uint64_t r = 0;
+    for (const char *p = begin; p != end; ++p) {
+      RAPIDJSON_ASSERT(*p >= '0' && *p <= '9');
+      r = r * 10u + static_cast<unsigned>(*p - '0');
+    }
+    return r;
+  }
+
+  // Assume a * b + k < 2^128
+  static uint64_t MulAdd64(uint64_t a, uint64_t b, uint64_t k,
+                           uint64_t *outHigh) {
+#if defined(_MSC_VER) && defined(_M_AMD64)
+    uint64_t low = _umul128(a, b, outHigh) + k;
+    if (low < k) (*outHigh)++;
+    return low;
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
+    defined(__x86_64__)
+    __extension__ typedef unsigned __int128 uint128;
+    uint128 p = static_cast<uint128>(a) * static_cast<uint128>(b);
+    p += k;
+    *outHigh = static_cast<uint64_t>(p >> 64);
+    return static_cast<uint64_t>(p);
+#else
+    const uint64_t a0 = a & 0xFFFFFFFF, a1 = a >> 32, b0 = b & 0xFFFFFFFF,
+                   b1 = b >> 32;
+    uint64_t x0 = a0 * b0, x1 = a0 * b1, x2 = a1 * b0, x3 = a1 * b1;
+    x1 += (x0 >> 32);  // can't give carry
+    x1 += x2;
+    if (x1 < x2) x3 += (static_cast<uint64_t>(1) << 32);
+    uint64_t lo = (x1 << 32) + (x0 & 0xFFFFFFFF);
+    uint64_t hi = x3 + (x1 >> 32);
+
+    lo += k;
+    if (lo < k) hi++;
+    *outHigh = hi;
+    return lo;
+#endif
+  }
+
+  static const size_t kBitCount = 3328;  // 64bit * 54 > 10^1000
+  static const size_t kCapacity = kBitCount / sizeof(Type);
+  static const size_t kTypeBit = sizeof(Type) * 8;
+
+  Type digits_[kCapacity];
+  size_t count_;
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_BIGINTEGER_H_

livox/common/rapidjson/internal/clzll.h

@@ -0,0 +1,77 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_CLZLL_H_
+#define RAPIDJSON_CLZLL_H_
+
+#include "../rapidjson.h"
+
+#if defined(_MSC_VER)
+#include <intrin.h>
+#if defined(_WIN64)
+#pragma intrinsic(_BitScanReverse64)
+#else
+#pragma intrinsic(_BitScanReverse)
+#endif
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#if (defined(__GNUC__) && __GNUC__ >= 4) || \
+    RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+#define RAPIDJSON_CLZLL __builtin_clzll
+#else
+
+inline uint32_t clzll(uint64_t x) {
+  // Passing 0 to __builtin_clzll is UB in GCC and results in an
+  // infinite loop in the software implementation.
+  RAPIDJSON_ASSERT(x != 0);
+
+#if defined(_MSC_VER)
+  unsigned long r = 0;
+#if defined(_WIN64)
+  _BitScanReverse64(&r, x);
+#else
+  // Scan the high 32 bits.
+  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 - (r + 32);
+
+  // Scan the low 32 bits.
+  _BitScanReverse(&r, static_cast<uint32_t>(x & 0xFFFFFFFF));
+#endif  // _WIN64
+
+  return 63 - r;
+#else
+  uint32_t r;
+  while (!(x & (static_cast<uint64_t>(1) << 63))) {
+    x <<= 1;
+    ++r;
+  }
+
+  return r;
+#endif  // _MSC_VER
+}
+
+#define RAPIDJSON_CLZLL RAPIDJSON_NAMESPACE::internal::clzll
+#endif  // (defined(__GNUC__) && __GNUC__ >= 4) ||
+        // RAPIDJSON_HAS_BUILTIN(__builtin_clzll)
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_CLZLL_H_

livox/common/rapidjson/internal/diyfp.h

@@ -0,0 +1,305 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+// This is a C++ header-only implementation of Grisu2 algorithm from the
+// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
+// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
+
+#ifndef RAPIDJSON_DIYFP_H_
+#define RAPIDJSON_DIYFP_H_
+
+#include <limits>
+#include "../rapidjson.h"
+#include "clzll.h"
+
+#if defined(_MSC_VER) && defined(_M_AMD64) && !defined(__INTEL_COMPILER)
+#include <intrin.h>
+#pragma intrinsic(_umul128)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+struct DiyFp {
+  DiyFp() : f(), e() {}
+
+  DiyFp(uint64_t fp, int exp) : f(fp), e(exp) {}
+
+  explicit DiyFp(double d) {
+    union {
+      double d;
+      uint64_t u64;
+    } u = {d};
+
+    int biased_e =
+        static_cast<int>((u.u64 & kDpExponentMask) >> kDpSignificandSize);
+    uint64_t significand = (u.u64 & kDpSignificandMask);
+    if (biased_e != 0) {
+      f = significand + kDpHiddenBit;
+      e = biased_e - kDpExponentBias;
+    } else {
+      f = significand;
+      e = kDpMinExponent + 1;
+    }
+  }
+
+  DiyFp operator-(const DiyFp &rhs) const { return DiyFp(f - rhs.f, e); }
+
+  DiyFp operator*(const DiyFp &rhs) const {
+#if defined(_MSC_VER) && defined(_M_AMD64)
+    uint64_t h;
+    uint64_t l = _umul128(f, rhs.f, &h);
+    if (l & (uint64_t(1) << 63))  // rounding
+      h++;
+    return DiyFp(h, e + rhs.e + 64);
+#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && \
+    defined(__x86_64__)
+    __extension__ typedef unsigned __int128 uint128;
+    uint128 p = static_cast<uint128>(f) * static_cast<uint128>(rhs.f);
+    uint64_t h = static_cast<uint64_t>(p >> 64);
+    uint64_t l = static_cast<uint64_t>(p);
+    if (l & (uint64_t(1) << 63))  // rounding
+      h++;
+    return DiyFp(h, e + rhs.e + 64);
+#else
+    const uint64_t M32 = 0xFFFFFFFF;
+    const uint64_t a = f >> 32;
+    const uint64_t b = f & M32;
+    const uint64_t c = rhs.f >> 32;
+    const uint64_t d = rhs.f & M32;
+    const uint64_t ac = a * c;
+    const uint64_t bc = b * c;
+    const uint64_t ad = a * d;
+    const uint64_t bd = b * d;
+    uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32);
+    tmp += 1U << 31;  /// mult_round
+    return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64);
+#endif
+  }
+
+  DiyFp Normalize() const {
+    int s = static_cast<int>(RAPIDJSON_CLZLL(f));
+    return DiyFp(f << s, e - s);
+  }
+
+  DiyFp NormalizeBoundary() const {
+    DiyFp res = *this;
+    while (!(res.f & (kDpHiddenBit << 1))) {
+      res.f <<= 1;
+      res.e--;
+    }
+    res.f <<= (kDiySignificandSize - kDpSignificandSize - 2);
+    res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2);
+    return res;
+  }
+
+  void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const {
+    DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary();
+    DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2)
+                                   : DiyFp((f << 1) - 1, e - 1);
+    mi.f <<= mi.e - pl.e;
+    mi.e = pl.e;
+    *plus = pl;
+    *minus = mi;
+  }
+
+  double ToDouble() const {
+    union {
+      double d;
+      uint64_t u64;
+    } u;
+    RAPIDJSON_ASSERT(f <= kDpHiddenBit + kDpSignificandMask);
+    if (e < kDpDenormalExponent) {
+      // Underflow.
+      return 0.0;
+    }
+    if (e >= kDpMaxExponent) {
+      // Overflow.
+      return std::numeric_limits<double>::infinity();
+    }
+    const uint64_t be = (e == kDpDenormalExponent && (f & kDpHiddenBit) == 0)
+                            ? 0
+                            : static_cast<uint64_t>(e + kDpExponentBias);
+    u.u64 = (f & kDpSignificandMask) | (be << kDpSignificandSize);
+    return u.d;
+  }
+
+  static const int kDiySignificandSize = 64;
+  static const int kDpSignificandSize = 52;
+  static const int kDpExponentBias = 0x3FF + kDpSignificandSize;
+  static const int kDpMaxExponent = 0x7FF - kDpExponentBias;
+  static const int kDpMinExponent = -kDpExponentBias;
+  static const int kDpDenormalExponent = -kDpExponentBias + 1;
+  static const uint64_t kDpExponentMask =
+      RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
+  static const uint64_t kDpSignificandMask =
+      RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
+  static const uint64_t kDpHiddenBit =
+      RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
+
+  uint64_t f;
+  int e;
+};
+
+inline DiyFp GetCachedPowerByIndex(size_t index) {
+  // 10^-348, 10^-340, ..., 10^340
+  static const uint64_t kCachedPowers_F[] = {
+      RAPIDJSON_UINT64_C2(0xfa8fd5a0, 0x081c0288),
+      RAPIDJSON_UINT64_C2(0xbaaee17f, 0xa23ebf76),
+      RAPIDJSON_UINT64_C2(0x8b16fb20, 0x3055ac76),
+      RAPIDJSON_UINT64_C2(0xcf42894a, 0x5dce35ea),
+      RAPIDJSON_UINT64_C2(0x9a6bb0aa, 0x55653b2d),
+      RAPIDJSON_UINT64_C2(0xe61acf03, 0x3d1a45df),
+      RAPIDJSON_UINT64_C2(0xab70fe17, 0xc79ac6ca),
+      RAPIDJSON_UINT64_C2(0xff77b1fc, 0xbebcdc4f),
+      RAPIDJSON_UINT64_C2(0xbe5691ef, 0x416bd60c),
+      RAPIDJSON_UINT64_C2(0x8dd01fad, 0x907ffc3c),
+      RAPIDJSON_UINT64_C2(0xd3515c28, 0x31559a83),
+      RAPIDJSON_UINT64_C2(0x9d71ac8f, 0xada6c9b5),
+      RAPIDJSON_UINT64_C2(0xea9c2277, 0x23ee8bcb),
+      RAPIDJSON_UINT64_C2(0xaecc4991, 0x4078536d),
+      RAPIDJSON_UINT64_C2(0x823c1279, 0x5db6ce57),
+      RAPIDJSON_UINT64_C2(0xc2109436, 0x4dfb5637),
+      RAPIDJSON_UINT64_C2(0x9096ea6f, 0x3848984f),
+      RAPIDJSON_UINT64_C2(0xd77485cb, 0x25823ac7),
+      RAPIDJSON_UINT64_C2(0xa086cfcd, 0x97bf97f4),
+      RAPIDJSON_UINT64_C2(0xef340a98, 0x172aace5),
+      RAPIDJSON_UINT64_C2(0xb23867fb, 0x2a35b28e),
+      RAPIDJSON_UINT64_C2(0x84c8d4df, 0xd2c63f3b),
+      RAPIDJSON_UINT64_C2(0xc5dd4427, 0x1ad3cdba),
+      RAPIDJSON_UINT64_C2(0x936b9fce, 0xbb25c996),
+      RAPIDJSON_UINT64_C2(0xdbac6c24, 0x7d62a584),
+      RAPIDJSON_UINT64_C2(0xa3ab6658, 0x0d5fdaf6),
+      RAPIDJSON_UINT64_C2(0xf3e2f893, 0xdec3f126),
+      RAPIDJSON_UINT64_C2(0xb5b5ada8, 0xaaff80b8),
+      RAPIDJSON_UINT64_C2(0x87625f05, 0x6c7c4a8b),
+      RAPIDJSON_UINT64_C2(0xc9bcff60, 0x34c13053),
+      RAPIDJSON_UINT64_C2(0x964e858c, 0x91ba2655),
+      RAPIDJSON_UINT64_C2(0xdff97724, 0x70297ebd),
+      RAPIDJSON_UINT64_C2(0xa6dfbd9f, 0xb8e5b88f),
+      RAPIDJSON_UINT64_C2(0xf8a95fcf, 0x88747d94),
+      RAPIDJSON_UINT64_C2(0xb9447093, 0x8fa89bcf),
+      RAPIDJSON_UINT64_C2(0x8a08f0f8, 0xbf0f156b),
+      RAPIDJSON_UINT64_C2(0xcdb02555, 0x653131b6),
+      RAPIDJSON_UINT64_C2(0x993fe2c6, 0xd07b7fac),
+      RAPIDJSON_UINT64_C2(0xe45c10c4, 0x2a2b3b06),
+      RAPIDJSON_UINT64_C2(0xaa242499, 0x697392d3),
+      RAPIDJSON_UINT64_C2(0xfd87b5f2, 0x8300ca0e),
+      RAPIDJSON_UINT64_C2(0xbce50864, 0x92111aeb),
+      RAPIDJSON_UINT64_C2(0x8cbccc09, 0x6f5088cc),
+      RAPIDJSON_UINT64_C2(0xd1b71758, 0xe219652c),
+      RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000),
+      RAPIDJSON_UINT64_C2(0xe8d4a510, 0x00000000),
+      RAPIDJSON_UINT64_C2(0xad78ebc5, 0xac620000),
+      RAPIDJSON_UINT64_C2(0x813f3978, 0xf8940984),
+      RAPIDJSON_UINT64_C2(0xc097ce7b, 0xc90715b3),
+      RAPIDJSON_UINT64_C2(0x8f7e32ce, 0x7bea5c70),
+      RAPIDJSON_UINT64_C2(0xd5d238a4, 0xabe98068),
+      RAPIDJSON_UINT64_C2(0x9f4f2726, 0x179a2245),
+      RAPIDJSON_UINT64_C2(0xed63a231, 0xd4c4fb27),
+      RAPIDJSON_UINT64_C2(0xb0de6538, 0x8cc8ada8),
+      RAPIDJSON_UINT64_C2(0x83c7088e, 0x1aab65db),
+      RAPIDJSON_UINT64_C2(0xc45d1df9, 0x42711d9a),
+      RAPIDJSON_UINT64_C2(0x924d692c, 0xa61be758),
+      RAPIDJSON_UINT64_C2(0xda01ee64, 0x1a708dea),
+      RAPIDJSON_UINT64_C2(0xa26da399, 0x9aef774a),
+      RAPIDJSON_UINT64_C2(0xf209787b, 0xb47d6b85),
+      RAPIDJSON_UINT64_C2(0xb454e4a1, 0x79dd1877),
+      RAPIDJSON_UINT64_C2(0x865b8692, 0x5b9bc5c2),
+      RAPIDJSON_UINT64_C2(0xc83553c5, 0xc8965d3d),
+      RAPIDJSON_UINT64_C2(0x952ab45c, 0xfa97a0b3),
+      RAPIDJSON_UINT64_C2(0xde469fbd, 0x99a05fe3),
+      RAPIDJSON_UINT64_C2(0xa59bc234, 0xdb398c25),
+      RAPIDJSON_UINT64_C2(0xf6c69a72, 0xa3989f5c),
+      RAPIDJSON_UINT64_C2(0xb7dcbf53, 0x54e9bece),
+      RAPIDJSON_UINT64_C2(0x88fcf317, 0xf22241e2),
+      RAPIDJSON_UINT64_C2(0xcc20ce9b, 0xd35c78a5),
+      RAPIDJSON_UINT64_C2(0x98165af3, 0x7b2153df),
+      RAPIDJSON_UINT64_C2(0xe2a0b5dc, 0x971f303a),
+      RAPIDJSON_UINT64_C2(0xa8d9d153, 0x5ce3b396),
+      RAPIDJSON_UINT64_C2(0xfb9b7cd9, 0xa4a7443c),
+      RAPIDJSON_UINT64_C2(0xbb764c4c, 0xa7a44410),
+      RAPIDJSON_UINT64_C2(0x8bab8eef, 0xb6409c1a),
+      RAPIDJSON_UINT64_C2(0xd01fef10, 0xa657842c),
+      RAPIDJSON_UINT64_C2(0x9b10a4e5, 0xe9913129),
+      RAPIDJSON_UINT64_C2(0xe7109bfb, 0xa19c0c9d),
+      RAPIDJSON_UINT64_C2(0xac2820d9, 0x623bf429),
+      RAPIDJSON_UINT64_C2(0x80444b5e, 0x7aa7cf85),
+      RAPIDJSON_UINT64_C2(0xbf21e440, 0x03acdd2d),
+      RAPIDJSON_UINT64_C2(0x8e679c2f, 0x5e44ff8f),
+      RAPIDJSON_UINT64_C2(0xd433179d, 0x9c8cb841),
+      RAPIDJSON_UINT64_C2(0x9e19db92, 0xb4e31ba9),
+      RAPIDJSON_UINT64_C2(0xeb96bf6e, 0xbadf77d9),
+      RAPIDJSON_UINT64_C2(0xaf87023b, 0x9bf0ee6b)};
+  static const int16_t kCachedPowers_E[] = {
+      -1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
+      -927,  -901,  -874,  -847,  -821,  -794,  -768,  -741,  -715,  -688, -661,
+      -635,  -608,  -582,  -555,  -529,  -502,  -475,  -449,  -422,  -396, -369,
+      -343,  -316,  -289,  -263,  -236,  -210,  -183,  -157,  -130,  -103, -77,
+      -50,   -24,   3,     30,    56,    83,    109,   136,   162,   189,  216,
+      242,   269,   295,   322,   348,   375,   402,   428,   455,   481,  508,
+      534,   561,   588,   614,   641,   667,   694,   720,   747,   774,  800,
+      827,   853,   880,   907,   933,   960,   986,   1013,  1039,  1066};
+  RAPIDJSON_ASSERT(index < 87);
+  return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]);
+}
+
+inline DiyFp GetCachedPower(int e, int *K) {
+  // int k = static_cast<int>(ceil((-61 - e) * 0.30102999566398114)) + 374;
+  double dk = (-61 - e) * 0.30102999566398114 +
+              347;  // dk must be positive, so can do ceiling in positive
+  int k = static_cast<int>(dk);
+  if (dk - k > 0.0) k++;
+
+  unsigned index = static_cast<unsigned>((k >> 3) + 1);
+  *K = -(-348 + static_cast<int>(
+                    index << 3));  // decimal exponent no need lookup table
+
+  return GetCachedPowerByIndex(index);
+}
+
+inline DiyFp GetCachedPower10(int exp, int *outExp) {
+  RAPIDJSON_ASSERT(exp >= -348);
+  unsigned index = static_cast<unsigned>(exp + 348) / 8u;
+  *outExp = -348 + static_cast<int>(index) * 8;
+  return GetCachedPowerByIndex(index);
+}
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_DIYFP_H_

livox/common/rapidjson/internal/dtoa.h

@@ -0,0 +1,269 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+// This is a C++ header-only implementation of Grisu2 algorithm from the
+// publication: Loitsch, Florian. "Printing floating-point numbers quickly and
+// accurately with integers." ACM Sigplan Notices 45.6 (2010): 233-243.
+
+#ifndef RAPIDJSON_DTOA_
+#define RAPIDJSON_DTOA_
+
+#include "diyfp.h"
+#include "ieee754.h"
+#include "itoa.h"  // GetDigitsLut()
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+RAPIDJSON_DIAG_OFF(array - bounds)  // some gcc versions generate wrong warnings
+// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=59124
+#endif
+
+inline void GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest,
+                       uint64_t ten_kappa, uint64_t wp_w) {
+  while (rest < wp_w && delta - rest >= ten_kappa &&
+         (rest + ten_kappa < wp_w ||  /// closer
+          wp_w - rest > rest + ten_kappa - wp_w)) {
+    buffer[len - 1]--;
+    rest += ten_kappa;
+  }
+}
+
+inline int CountDecimalDigit32(uint32_t n) {
+  // Simple pure C++ implementation was faster than __builtin_clz version in
+  // this situation.
+  if (n < 10) return 1;
+  if (n < 100) return 2;
+  if (n < 1000) return 3;
+  if (n < 10000) return 4;
+  if (n < 100000) return 5;
+  if (n < 1000000) return 6;
+  if (n < 10000000) return 7;
+  if (n < 100000000) return 8;
+  // Will not reach 10 digits in DigitGen()
+  // if (n < 1000000000) return 9;
+  // return 10;
+  return 9;
+}
+
+inline void DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta,
+                     char *buffer, int *len, int *K) {
+  static const uint32_t kPow10[] = {1,         10,        100,     1000,
+                                    10000,     100000,    1000000, 10000000,
+                                    100000000, 1000000000};
+  const DiyFp one(uint64_t(1) << -Mp.e, Mp.e);
+  const DiyFp wp_w = Mp - W;
+  uint32_t p1 = static_cast<uint32_t>(Mp.f >> -one.e);
+  uint64_t p2 = Mp.f & (one.f - 1);
+  int kappa = CountDecimalDigit32(p1);  // kappa in [0, 9]
+  *len = 0;
+
+  while (kappa > 0) {
+    uint32_t d = 0;
+    switch (kappa) {
+      case 9:
+        d = p1 / 100000000;
+        p1 %= 100000000;
+        break;
+      case 8:
+        d = p1 / 10000000;
+        p1 %= 10000000;
+        break;
+      case 7:
+        d = p1 / 1000000;
+        p1 %= 1000000;
+        break;
+      case 6:
+        d = p1 / 100000;
+        p1 %= 100000;
+        break;
+      case 5:
+        d = p1 / 10000;
+        p1 %= 10000;
+        break;
+      case 4:
+        d = p1 / 1000;
+        p1 %= 1000;
+        break;
+      case 3:
+        d = p1 / 100;
+        p1 %= 100;
+        break;
+      case 2:
+        d = p1 / 10;
+        p1 %= 10;
+        break;
+      case 1:
+        d = p1;
+        p1 = 0;
+        break;
+      default:;
+    }
+    if (d || *len)
+      buffer[(*len)++] = static_cast<char>('0' + static_cast<char>(d));
+    kappa--;
+    uint64_t tmp = (static_cast<uint64_t>(p1) << -one.e) + p2;
+    if (tmp <= delta) {
+      *K += kappa;
+      GrisuRound(buffer, *len, delta, tmp,
+                 static_cast<uint64_t>(kPow10[kappa]) << -one.e, wp_w.f);
+      return;
+    }
+  }
+
+  // kappa = 0
+  for (;;) {
+    p2 *= 10;
+    delta *= 10;
+    char d = static_cast<char>(p2 >> -one.e);
+    if (d || *len) buffer[(*len)++] = static_cast<char>('0' + d);
+    p2 &= one.f - 1;
+    kappa--;
+    if (p2 < delta) {
+      *K += kappa;
+      int index = -kappa;
+      GrisuRound(buffer, *len, delta, p2, one.f,
+                 wp_w.f * (index < 9 ? kPow10[index] : 0));
+      return;
+    }
+  }
+}
+
+inline void Grisu2(double value, char *buffer, int *length, int *K) {
+  const DiyFp v(value);
+  DiyFp w_m, w_p;
+  v.NormalizedBoundaries(&w_m, &w_p);
+
+  const DiyFp c_mk = GetCachedPower(w_p.e, K);
+  const DiyFp W = v.Normalize() * c_mk;
+  DiyFp Wp = w_p * c_mk;
+  DiyFp Wm = w_m * c_mk;
+  Wm.f++;
+  Wp.f--;
+  DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K);
+}
+
+inline char *WriteExponent(int K, char *buffer) {
+  if (K < 0) {
+    *buffer++ = '-';
+    K = -K;
+  }
+
+  if (K >= 100) {
+    *buffer++ = static_cast<char>('0' + static_cast<char>(K / 100));
+    K %= 100;
+    const char *d = GetDigitsLut() + K * 2;
+    *buffer++ = d[0];
+    *buffer++ = d[1];
+  } else if (K >= 10) {
+    const char *d = GetDigitsLut() + K * 2;
+    *buffer++ = d[0];
+    *buffer++ = d[1];
+  } else
+    *buffer++ = static_cast<char>('0' + static_cast<char>(K));
+
+  return buffer;
+}
+
+inline char *Prettify(char *buffer, int length, int k, int maxDecimalPlaces) {
+  const int kk = length + k;  // 10^(kk-1) <= v < 10^kk
+
+  if (0 <= k && kk <= 21) {
+    // 1234e7 -> 12340000000
+    for (int i = length; i < kk; i++) buffer[i] = '0';
+    buffer[kk] = '.';
+    buffer[kk + 1] = '0';
+    return &buffer[kk + 2];
+  } else if (0 < kk && kk <= 21) {
+    // 1234e-2 -> 12.34
+    std::memmove(&buffer[kk + 1], &buffer[kk],
+                 static_cast<size_t>(length - kk));
+    buffer[kk] = '.';
+    if (0 > k + maxDecimalPlaces) {
+      // When maxDecimalPlaces = 2, 1.2345 -> 1.23, 1.102 -> 1.1
+      // Remove extra trailing zeros (at least one) after truncation.
+      for (int i = kk + maxDecimalPlaces; i > kk + 1; i--)
+        if (buffer[i] != '0') return &buffer[i + 1];
+      return &buffer[kk + 2];  // Reserve one zero
+    } else
+      return &buffer[length + 1];
+  } else if (-6 < kk && kk <= 0) {
+    // 1234e-6 -> 0.001234
+    const int offset = 2 - kk;
+    std::memmove(&buffer[offset], &buffer[0], static_cast<size_t>(length));
+    buffer[0] = '0';
+    buffer[1] = '.';
+    for (int i = 2; i < offset; i++) buffer[i] = '0';
+    if (length - kk > maxDecimalPlaces) {
+      // When maxDecimalPlaces = 2, 0.123 -> 0.12, 0.102 -> 0.1
+      // Remove extra trailing zeros (at least one) after truncation.
+      for (int i = maxDecimalPlaces + 1; i > 2; i--)
+        if (buffer[i] != '0') return &buffer[i + 1];
+      return &buffer[3];  // Reserve one zero
+    } else
+      return &buffer[length + offset];
+  } else if (kk < -maxDecimalPlaces) {
+    // Truncate to zero
+    buffer[0] = '0';
+    buffer[1] = '.';
+    buffer[2] = '0';
+    return &buffer[3];
+  } else if (length == 1) {
+    // 1e30
+    buffer[1] = 'e';
+    return WriteExponent(kk - 1, &buffer[2]);
+  } else {
+    // 1234e30 -> 1.234e33
+    std::memmove(&buffer[2], &buffer[1], static_cast<size_t>(length - 1));
+    buffer[1] = '.';
+    buffer[length + 1] = 'e';
+    return WriteExponent(kk - 1, &buffer[0 + length + 2]);
+  }
+}
+
+inline char *dtoa(double value, char *buffer, int maxDecimalPlaces = 324) {
+  RAPIDJSON_ASSERT(maxDecimalPlaces >= 1);
+  Double d(value);
+  if (d.IsZero()) {
+    if (d.Sign()) *buffer++ = '-';  // -0.0, Issue #289
+    buffer[0] = '0';
+    buffer[1] = '.';
+    buffer[2] = '0';
+    return &buffer[3];
+  } else {
+    if (value < 0) {
+      *buffer++ = '-';
+      value = -value;
+    }
+    int length, K;
+    Grisu2(value, buffer, &length, &K);
+    return Prettify(buffer, length, K, maxDecimalPlaces);
+  }
+}
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_DTOA_

livox/common/rapidjson/internal/ieee754.h

@@ -0,0 +1,100 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_IEEE754_
+#define RAPIDJSON_IEEE754_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+class Double {
+ public:
+  Double() {}
+  Double(double d) : d_(d) {}
+  Double(uint64_t u) : u_(u) {}
+
+  double Value() const { return d_; }
+  uint64_t Uint64Value() const { return u_; }
+
+  double NextPositiveDouble() const {
+    RAPIDJSON_ASSERT(!Sign());
+    return Double(u_ + 1).Value();
+  }
+
+  bool Sign() const { return (u_ & kSignMask) != 0; }
+  uint64_t Significand() const { return u_ & kSignificandMask; }
+  int Exponent() const {
+    return static_cast<int>(((u_ & kExponentMask) >> kSignificandSize) -
+                            kExponentBias);
+  }
+
+  bool IsNan() const {
+    return (u_ & kExponentMask) == kExponentMask && Significand() != 0;
+  }
+  bool IsInf() const {
+    return (u_ & kExponentMask) == kExponentMask && Significand() == 0;
+  }
+  bool IsNanOrInf() const { return (u_ & kExponentMask) == kExponentMask; }
+  bool IsNormal() const {
+    return (u_ & kExponentMask) != 0 || Significand() == 0;
+  }
+  bool IsZero() const { return (u_ & (kExponentMask | kSignificandMask)) == 0; }
+
+  uint64_t IntegerSignificand() const {
+    return IsNormal() ? Significand() | kHiddenBit : Significand();
+  }
+  int IntegerExponent() const {
+    return (IsNormal() ? Exponent() : kDenormalExponent) - kSignificandSize;
+  }
+  uint64_t ToBias() const {
+    return (u_ & kSignMask) ? ~u_ + 1 : u_ | kSignMask;
+  }
+
+  static int EffectiveSignificandSize(int order) {
+    if (order >= -1021)
+      return 53;
+    else if (order <= -1074)
+      return 0;
+    else
+      return order + 1074;
+  }
+
+ private:
+  static const int kSignificandSize = 52;
+  static const int kExponentBias = 0x3FF;
+  static const int kDenormalExponent = 1 - kExponentBias;
+  static const uint64_t kSignMask = RAPIDJSON_UINT64_C2(0x80000000, 0x00000000);
+  static const uint64_t kExponentMask =
+      RAPIDJSON_UINT64_C2(0x7FF00000, 0x00000000);
+  static const uint64_t kSignificandMask =
+      RAPIDJSON_UINT64_C2(0x000FFFFF, 0xFFFFFFFF);
+  static const uint64_t kHiddenBit =
+      RAPIDJSON_UINT64_C2(0x00100000, 0x00000000);
+
+  union {
+    double d_;
+    uint64_t u_;
+  };
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_IEEE754_

livox/common/rapidjson/internal/itoa.h

@@ -0,0 +1,288 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ITOA_
+#define RAPIDJSON_ITOA_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+inline const char *GetDigitsLut() {
+  static const char cDigitsLut[200] = {
+      '0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0',
+      '7', '0', '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4',
+      '1', '5', '1', '6', '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2',
+      '2', '2', '3', '2', '4', '2', '5', '2', '6', '2', '7', '2', '8', '2', '9',
+      '3', '0', '3', '1', '3', '2', '3', '3', '3', '4', '3', '5', '3', '6', '3',
+      '7', '3', '8', '3', '9', '4', '0', '4', '1', '4', '2', '4', '3', '4', '4',
+      '4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', '5', '1', '5',
+      '2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', '9',
+      '6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6',
+      '7', '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4',
+      '7', '5', '7', '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8',
+      '2', '8', '3', '8', '4', '8', '5', '8', '6', '8', '7', '8', '8', '8', '9',
+      '9', '0', '9', '1', '9', '2', '9', '3', '9', '4', '9', '5', '9', '6', '9',
+      '7', '9', '8', '9', '9'};
+  return cDigitsLut;
+}
+
+inline char *u32toa(uint32_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+
+  const char *cDigitsLut = GetDigitsLut();
+
+  if (value < 10000) {
+    const uint32_t d1 = (value / 100) << 1;
+    const uint32_t d2 = (value % 100) << 1;
+
+    if (value >= 1000) *buffer++ = cDigitsLut[d1];
+    if (value >= 100) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 10) *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+  } else if (value < 100000000) {
+    // value = bbbbcccc
+    const uint32_t b = value / 10000;
+    const uint32_t c = value % 10000;
+
+    const uint32_t d1 = (b / 100) << 1;
+    const uint32_t d2 = (b % 100) << 1;
+
+    const uint32_t d3 = (c / 100) << 1;
+    const uint32_t d4 = (c % 100) << 1;
+
+    if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+    if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= 100000) *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+  } else {
+    // value = aabbbbcccc in decimal
+
+    const uint32_t a = value / 100000000;  // 1 to 42
+    value %= 100000000;
+
+    if (a >= 10) {
+      const unsigned i = a << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a));
+
+    const uint32_t b = value / 10000;  // 0 to 9999
+    const uint32_t c = value % 10000;  // 0 to 9999
+
+    const uint32_t d1 = (b / 100) << 1;
+    const uint32_t d2 = (b % 100) << 1;
+
+    const uint32_t d3 = (c / 100) << 1;
+    const uint32_t d4 = (c % 100) << 1;
+
+    *buffer++ = cDigitsLut[d1];
+    *buffer++ = cDigitsLut[d1 + 1];
+    *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+  }
+  return buffer;
+}
+
+inline char *i32toa(int32_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  uint32_t u = static_cast<uint32_t>(value);
+  if (value < 0) {
+    *buffer++ = '-';
+    u = ~u + 1;
+  }
+
+  return u32toa(u, buffer);
+}
+
+inline char *u64toa(uint64_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  const char *cDigitsLut = GetDigitsLut();
+  const uint64_t kTen8 = 100000000;
+  const uint64_t kTen9 = kTen8 * 10;
+  const uint64_t kTen10 = kTen8 * 100;
+  const uint64_t kTen11 = kTen8 * 1000;
+  const uint64_t kTen12 = kTen8 * 10000;
+  const uint64_t kTen13 = kTen8 * 100000;
+  const uint64_t kTen14 = kTen8 * 1000000;
+  const uint64_t kTen15 = kTen8 * 10000000;
+  const uint64_t kTen16 = kTen8 * kTen8;
+
+  if (value < kTen8) {
+    uint32_t v = static_cast<uint32_t>(value);
+    if (v < 10000) {
+      const uint32_t d1 = (v / 100) << 1;
+      const uint32_t d2 = (v % 100) << 1;
+
+      if (v >= 1000) *buffer++ = cDigitsLut[d1];
+      if (v >= 100) *buffer++ = cDigitsLut[d1 + 1];
+      if (v >= 10) *buffer++ = cDigitsLut[d2];
+      *buffer++ = cDigitsLut[d2 + 1];
+    } else {
+      // value = bbbbcccc
+      const uint32_t b = v / 10000;
+      const uint32_t c = v % 10000;
+
+      const uint32_t d1 = (b / 100) << 1;
+      const uint32_t d2 = (b % 100) << 1;
+
+      const uint32_t d3 = (c / 100) << 1;
+      const uint32_t d4 = (c % 100) << 1;
+
+      if (value >= 10000000) *buffer++ = cDigitsLut[d1];
+      if (value >= 1000000) *buffer++ = cDigitsLut[d1 + 1];
+      if (value >= 100000) *buffer++ = cDigitsLut[d2];
+      *buffer++ = cDigitsLut[d2 + 1];
+
+      *buffer++ = cDigitsLut[d3];
+      *buffer++ = cDigitsLut[d3 + 1];
+      *buffer++ = cDigitsLut[d4];
+      *buffer++ = cDigitsLut[d4 + 1];
+    }
+  } else if (value < kTen16) {
+    const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
+    const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
+
+    const uint32_t b0 = v0 / 10000;
+    const uint32_t c0 = v0 % 10000;
+
+    const uint32_t d1 = (b0 / 100) << 1;
+    const uint32_t d2 = (b0 % 100) << 1;
+
+    const uint32_t d3 = (c0 / 100) << 1;
+    const uint32_t d4 = (c0 % 100) << 1;
+
+    const uint32_t b1 = v1 / 10000;
+    const uint32_t c1 = v1 % 10000;
+
+    const uint32_t d5 = (b1 / 100) << 1;
+    const uint32_t d6 = (b1 % 100) << 1;
+
+    const uint32_t d7 = (c1 / 100) << 1;
+    const uint32_t d8 = (c1 % 100) << 1;
+
+    if (value >= kTen15) *buffer++ = cDigitsLut[d1];
+    if (value >= kTen14) *buffer++ = cDigitsLut[d1 + 1];
+    if (value >= kTen13) *buffer++ = cDigitsLut[d2];
+    if (value >= kTen12) *buffer++ = cDigitsLut[d2 + 1];
+    if (value >= kTen11) *buffer++ = cDigitsLut[d3];
+    if (value >= kTen10) *buffer++ = cDigitsLut[d3 + 1];
+    if (value >= kTen9) *buffer++ = cDigitsLut[d4];
+
+    *buffer++ = cDigitsLut[d4 + 1];
+    *buffer++ = cDigitsLut[d5];
+    *buffer++ = cDigitsLut[d5 + 1];
+    *buffer++ = cDigitsLut[d6];
+    *buffer++ = cDigitsLut[d6 + 1];
+    *buffer++ = cDigitsLut[d7];
+    *buffer++ = cDigitsLut[d7 + 1];
+    *buffer++ = cDigitsLut[d8];
+    *buffer++ = cDigitsLut[d8 + 1];
+  } else {
+    const uint32_t a = static_cast<uint32_t>(value / kTen16);  // 1 to 1844
+    value %= kTen16;
+
+    if (a < 10)
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a));
+    else if (a < 100) {
+      const uint32_t i = a << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else if (a < 1000) {
+      *buffer++ = static_cast<char>('0' + static_cast<char>(a / 100));
+
+      const uint32_t i = (a % 100) << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+    } else {
+      const uint32_t i = (a / 100) << 1;
+      const uint32_t j = (a % 100) << 1;
+      *buffer++ = cDigitsLut[i];
+      *buffer++ = cDigitsLut[i + 1];
+      *buffer++ = cDigitsLut[j];
+      *buffer++ = cDigitsLut[j + 1];
+    }
+
+    const uint32_t v0 = static_cast<uint32_t>(value / kTen8);
+    const uint32_t v1 = static_cast<uint32_t>(value % kTen8);
+
+    const uint32_t b0 = v0 / 10000;
+    const uint32_t c0 = v0 % 10000;
+
+    const uint32_t d1 = (b0 / 100) << 1;
+    const uint32_t d2 = (b0 % 100) << 1;
+
+    const uint32_t d3 = (c0 / 100) << 1;
+    const uint32_t d4 = (c0 % 100) << 1;
+
+    const uint32_t b1 = v1 / 10000;
+    const uint32_t c1 = v1 % 10000;
+
+    const uint32_t d5 = (b1 / 100) << 1;
+    const uint32_t d6 = (b1 % 100) << 1;
+
+    const uint32_t d7 = (c1 / 100) << 1;
+    const uint32_t d8 = (c1 % 100) << 1;
+
+    *buffer++ = cDigitsLut[d1];
+    *buffer++ = cDigitsLut[d1 + 1];
+    *buffer++ = cDigitsLut[d2];
+    *buffer++ = cDigitsLut[d2 + 1];
+    *buffer++ = cDigitsLut[d3];
+    *buffer++ = cDigitsLut[d3 + 1];
+    *buffer++ = cDigitsLut[d4];
+    *buffer++ = cDigitsLut[d4 + 1];
+    *buffer++ = cDigitsLut[d5];
+    *buffer++ = cDigitsLut[d5 + 1];
+    *buffer++ = cDigitsLut[d6];
+    *buffer++ = cDigitsLut[d6 + 1];
+    *buffer++ = cDigitsLut[d7];
+    *buffer++ = cDigitsLut[d7 + 1];
+    *buffer++ = cDigitsLut[d8];
+    *buffer++ = cDigitsLut[d8 + 1];
+  }
+
+  return buffer;
+}
+
+inline char *i64toa(int64_t value, char *buffer) {
+  RAPIDJSON_ASSERT(buffer != 0);
+  uint64_t u = static_cast<uint64_t>(value);
+  if (value < 0) {
+    *buffer++ = '-';
+    u = ~u + 1;
+  }
+
+  return u64toa(u, buffer);
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ITOA_

livox/common/rapidjson/internal/meta.h

@@ -0,0 +1,243 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_META_H_
+#define RAPIDJSON_INTERNAL_META_H_
+
+#include "../rapidjson.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(6334)
+#endif
+
+#if RAPIDJSON_HAS_CXX11_TYPETRAITS
+#include <type_traits>
+#endif
+
+//@cond RAPIDJSON_INTERNAL
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+// Helper to wrap/convert arbitrary types to void, useful for arbitrary type
+// matching
+template <typename T>
+struct Void {
+  typedef void Type;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// BoolType, TrueType, FalseType
+//
+template <bool Cond>
+struct BoolType {
+  static const bool Value = Cond;
+  typedef BoolType Type;
+};
+typedef BoolType<true> TrueType;
+typedef BoolType<false> FalseType;
+
+///////////////////////////////////////////////////////////////////////////////
+// SelectIf, BoolExpr, NotExpr, AndExpr, OrExpr
+//
+
+template <bool C>
+struct SelectIfImpl {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T1 Type;
+  };
+};
+template <>
+struct SelectIfImpl<false> {
+  template <typename T1, typename T2>
+  struct Apply {
+    typedef T2 Type;
+  };
+};
+template <bool C, typename T1, typename T2>
+struct SelectIfCond : SelectIfImpl<C>::template Apply<T1, T2> {};
+template <typename C, typename T1, typename T2>
+struct SelectIf : SelectIfCond<C::Value, T1, T2> {};
+
+template <bool Cond1, bool Cond2>
+struct AndExprCond : FalseType {};
+template <>
+struct AndExprCond<true, true> : TrueType {};
+template <bool Cond1, bool Cond2>
+struct OrExprCond : TrueType {};
+template <>
+struct OrExprCond<false, false> : FalseType {};
+
+template <typename C>
+struct BoolExpr : SelectIf<C, TrueType, FalseType>::Type {};
+template <typename C>
+struct NotExpr : SelectIf<C, FalseType, TrueType>::Type {};
+template <typename C1, typename C2>
+struct AndExpr : AndExprCond<C1::Value, C2::Value>::Type {};
+template <typename C1, typename C2>
+struct OrExpr : OrExprCond<C1::Value, C2::Value>::Type {};
+
+///////////////////////////////////////////////////////////////////////////////
+// AddConst, MaybeAddConst, RemoveConst
+template <typename T>
+struct AddConst {
+  typedef const T Type;
+};
+template <bool Constify, typename T>
+struct MaybeAddConst : SelectIfCond<Constify, const T, T> {};
+template <typename T>
+struct RemoveConst {
+  typedef T Type;
+};
+template <typename T>
+struct RemoveConst<const T> {
+  typedef T Type;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// IsSame, IsConst, IsMoreConst, IsPointer
+//
+template <typename T, typename U>
+struct IsSame : FalseType {};
+template <typename T>
+struct IsSame<T, T> : TrueType {};
+
+template <typename T>
+struct IsConst : FalseType {};
+template <typename T>
+struct IsConst<const T> : TrueType {};
+
+template <typename CT, typename T>
+struct IsMoreConst
+    : AndExpr<
+          IsSame<typename RemoveConst<CT>::Type, typename RemoveConst<T>::Type>,
+          BoolType<IsConst<CT>::Value >= IsConst<T>::Value>>::Type {};
+
+template <typename T>
+struct IsPointer : FalseType {};
+template <typename T>
+struct IsPointer<T *> : TrueType {};
+
+///////////////////////////////////////////////////////////////////////////////
+// IsBaseOf
+//
+#if RAPIDJSON_HAS_CXX11_TYPETRAITS
+
+template <typename B, typename D>
+struct IsBaseOf : BoolType<::std::is_base_of<B, D>::value> {};
+
+#else  // simplified version adopted from Boost
+
+template <typename B, typename D>
+struct IsBaseOfImpl {
+  RAPIDJSON_STATIC_ASSERT(sizeof(B) != 0);
+  RAPIDJSON_STATIC_ASSERT(sizeof(D) != 0);
+
+  typedef char (&Yes)[1];
+  typedef char (&No)[2];
+
+  template <typename T>
+  static Yes Check(const D *, T);
+  static No Check(const B *, int);
+
+  struct Host {
+    operator const B *() const;
+    operator const D *();
+  };
+
+  enum { Value = (sizeof(Check(Host(), 0)) == sizeof(Yes)) };
+};
+
+template <typename B, typename D>
+struct IsBaseOf : OrExpr<IsSame<B, D>, BoolExpr<IsBaseOfImpl<B, D>>>::Type {};
+
+#endif  // RAPIDJSON_HAS_CXX11_TYPETRAITS
+
+//////////////////////////////////////////////////////////////////////////
+// EnableIf / DisableIf
+//
+template <bool Condition, typename T = void>
+struct EnableIfCond {
+  typedef T Type;
+};
+template <typename T>
+struct EnableIfCond<false, T> { /* empty */
+};
+
+template <bool Condition, typename T = void>
+struct DisableIfCond {
+  typedef T Type;
+};
+template <typename T>
+struct DisableIfCond<true, T> { /* empty */
+};
+
+template <typename Condition, typename T = void>
+struct EnableIf : EnableIfCond<Condition::Value, T> {};
+
+template <typename Condition, typename T = void>
+struct DisableIf : DisableIfCond<Condition::Value, T> {};
+
+// SFINAE helpers
+struct SfinaeTag {};
+template <typename T>
+struct RemoveSfinaeTag;
+template <typename T>
+struct RemoveSfinaeTag<SfinaeTag &(*)(T)> {
+  typedef T Type;
+};
+
+#define RAPIDJSON_REMOVEFPTR_(type)                          \
+  typename ::RAPIDJSON_NAMESPACE::internal::RemoveSfinaeTag< \
+      ::RAPIDJSON_NAMESPACE::internal::SfinaeTag &(*)type>::Type
+
+#define RAPIDJSON_ENABLEIF(cond)                                            \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf<RAPIDJSON_REMOVEFPTR_( \
+      cond)>::Type * = NULL
+
+#define RAPIDJSON_DISABLEIF(cond)                                            \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf<RAPIDJSON_REMOVEFPTR_( \
+      cond)>::Type * = NULL
+
+#define RAPIDJSON_ENABLEIF_RETURN(cond, returntype)   \
+  typename ::RAPIDJSON_NAMESPACE::internal::EnableIf< \
+      RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
+
+#define RAPIDJSON_DISABLEIF_RETURN(cond, returntype)   \
+  typename ::RAPIDJSON_NAMESPACE::internal::DisableIf< \
+      RAPIDJSON_REMOVEFPTR_(cond), RAPIDJSON_REMOVEFPTR_(returntype)>::Type
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+//@endcond
+
+#if defined(_MSC_VER) && !defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_META_H_

livox/common/rapidjson/internal/pow10.h

@@ -0,0 +1,77 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_POW10_
+#define RAPIDJSON_POW10_
+
+#include "../rapidjson.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Computes integer powers of 10 in double (10.0^n).
+/*! This function uses lookup table for fast and accurate results.
+    \param n non-negative exponent. Must <= 308.
+    \return 10.0^n
+*/
+inline double Pow10(int n) {
+  static const double e[] = {
+      // 1e-0...1e308: 309 * 8 bytes = 2472 bytes
+      1e+0,   1e+1,   1e+2,   1e+3,   1e+4,   1e+5,   1e+6,   1e+7,   1e+8,
+      1e+9,   1e+10,  1e+11,  1e+12,  1e+13,  1e+14,  1e+15,  1e+16,  1e+17,
+      1e+18,  1e+19,  1e+20,  1e+21,  1e+22,  1e+23,  1e+24,  1e+25,  1e+26,
+      1e+27,  1e+28,  1e+29,  1e+30,  1e+31,  1e+32,  1e+33,  1e+34,  1e+35,
+      1e+36,  1e+37,  1e+38,  1e+39,  1e+40,  1e+41,  1e+42,  1e+43,  1e+44,
+      1e+45,  1e+46,  1e+47,  1e+48,  1e+49,  1e+50,  1e+51,  1e+52,  1e+53,
+      1e+54,  1e+55,  1e+56,  1e+57,  1e+58,  1e+59,  1e+60,  1e+61,  1e+62,
+      1e+63,  1e+64,  1e+65,  1e+66,  1e+67,  1e+68,  1e+69,  1e+70,  1e+71,
+      1e+72,  1e+73,  1e+74,  1e+75,  1e+76,  1e+77,  1e+78,  1e+79,  1e+80,
+      1e+81,  1e+82,  1e+83,  1e+84,  1e+85,  1e+86,  1e+87,  1e+88,  1e+89,
+      1e+90,  1e+91,  1e+92,  1e+93,  1e+94,  1e+95,  1e+96,  1e+97,  1e+98,
+      1e+99,  1e+100, 1e+101, 1e+102, 1e+103, 1e+104, 1e+105, 1e+106, 1e+107,
+      1e+108, 1e+109, 1e+110, 1e+111, 1e+112, 1e+113, 1e+114, 1e+115, 1e+116,
+      1e+117, 1e+118, 1e+119, 1e+120, 1e+121, 1e+122, 1e+123, 1e+124, 1e+125,
+      1e+126, 1e+127, 1e+128, 1e+129, 1e+130, 1e+131, 1e+132, 1e+133, 1e+134,
+      1e+135, 1e+136, 1e+137, 1e+138, 1e+139, 1e+140, 1e+141, 1e+142, 1e+143,
+      1e+144, 1e+145, 1e+146, 1e+147, 1e+148, 1e+149, 1e+150, 1e+151, 1e+152,
+      1e+153, 1e+154, 1e+155, 1e+156, 1e+157, 1e+158, 1e+159, 1e+160, 1e+161,
+      1e+162, 1e+163, 1e+164, 1e+165, 1e+166, 1e+167, 1e+168, 1e+169, 1e+170,
+      1e+171, 1e+172, 1e+173, 1e+174, 1e+175, 1e+176, 1e+177, 1e+178, 1e+179,
+      1e+180, 1e+181, 1e+182, 1e+183, 1e+184, 1e+185, 1e+186, 1e+187, 1e+188,
+      1e+189, 1e+190, 1e+191, 1e+192, 1e+193, 1e+194, 1e+195, 1e+196, 1e+197,
+      1e+198, 1e+199, 1e+200, 1e+201, 1e+202, 1e+203, 1e+204, 1e+205, 1e+206,
+      1e+207, 1e+208, 1e+209, 1e+210, 1e+211, 1e+212, 1e+213, 1e+214, 1e+215,
+      1e+216, 1e+217, 1e+218, 1e+219, 1e+220, 1e+221, 1e+222, 1e+223, 1e+224,
+      1e+225, 1e+226, 1e+227, 1e+228, 1e+229, 1e+230, 1e+231, 1e+232, 1e+233,
+      1e+234, 1e+235, 1e+236, 1e+237, 1e+238, 1e+239, 1e+240, 1e+241, 1e+242,
+      1e+243, 1e+244, 1e+245, 1e+246, 1e+247, 1e+248, 1e+249, 1e+250, 1e+251,
+      1e+252, 1e+253, 1e+254, 1e+255, 1e+256, 1e+257, 1e+258, 1e+259, 1e+260,
+      1e+261, 1e+262, 1e+263, 1e+264, 1e+265, 1e+266, 1e+267, 1e+268, 1e+269,
+      1e+270, 1e+271, 1e+272, 1e+273, 1e+274, 1e+275, 1e+276, 1e+277, 1e+278,
+      1e+279, 1e+280, 1e+281, 1e+282, 1e+283, 1e+284, 1e+285, 1e+286, 1e+287,
+      1e+288, 1e+289, 1e+290, 1e+291, 1e+292, 1e+293, 1e+294, 1e+295, 1e+296,
+      1e+297, 1e+298, 1e+299, 1e+300, 1e+301, 1e+302, 1e+303, 1e+304, 1e+305,
+      1e+306, 1e+307, 1e+308};
+  RAPIDJSON_ASSERT(n >= 0 && n <= 308);
+  return e[n];
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_POW10_

livox/common/rapidjson/internal/regex.h

@@ -0,0 +1,753 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_REGEX_H_
+#define RAPIDJSON_INTERNAL_REGEX_H_
+
+#include "../allocators.h"
+#include "../stream.h"
+#include "stack.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+RAPIDJSON_DIAG_OFF(switch - enum)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4512)  // assignment operator could not be generated
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#ifndef RAPIDJSON_REGEX_VERBOSE
+#define RAPIDJSON_REGEX_VERBOSE 0
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+///////////////////////////////////////////////////////////////////////////////
+// DecodedStream
+
+template <typename SourceStream, typename Encoding>
+class DecodedStream {
+ public:
+  DecodedStream(SourceStream &ss) : ss_(ss), codepoint_() { Decode(); }
+  unsigned Peek() { return codepoint_; }
+  unsigned Take() {
+    unsigned c = codepoint_;
+    if (c)  // No further decoding when '\0'
+      Decode();
+    return c;
+  }
+
+ private:
+  void Decode() {
+    if (!Encoding::Decode(ss_, &codepoint_)) codepoint_ = 0;
+  }
+
+  SourceStream &ss_;
+  unsigned codepoint_;
+};
+
+///////////////////////////////////////////////////////////////////////////////
+// GenericRegex
+
+static const SizeType kRegexInvalidState = ~SizeType(
+    0);  //!< Represents an invalid index in GenericRegex::State::out, out1
+static const SizeType kRegexInvalidRange = ~SizeType(0);
+
+template <typename Encoding, typename Allocator>
+class GenericRegexSearch;
+
+//! Regular expression engine with subset of ECMAscript grammar.
+/*!
+    Supported regular expression syntax:
+    - \c ab     Concatenation
+    - \c a|b    Alternation
+    - \c a?     Zero or one
+    - \c a*     Zero or more
+    - \c a+     One or more
+    - \c a{3}   Exactly 3 times
+    - \c a{3,}  At least 3 times
+    - \c a{3,5} 3 to 5 times
+    - \c (ab)   Grouping
+    - \c ^a     At the beginning
+    - \c a$     At the end
+    - \c .      Any character
+    - \c [abc]  Character classes
+    - \c [a-c]  Character class range
+    - \c [a-z0-9_] Character class combination
+    - \c [^abc] Negated character classes
+    - \c [^a-c] Negated character class range
+    - \c [\b]   Backspace (U+0008)
+    - \c \\| \\\\ ...  Escape characters
+    - \c \\f Form feed (U+000C)
+    - \c \\n Line feed (U+000A)
+    - \c \\r Carriage return (U+000D)
+    - \c \\t Tab (U+0009)
+    - \c \\v Vertical tab (U+000B)
+
+    \note This is a Thompson NFA engine, implemented with reference to
+        Cox, Russ. "Regular Expression Matching Can Be Simple And Fast (but is
+   slow in Java, Perl, PHP, Python, Ruby,...).",
+        https://swtch.com/~rsc/regexp/regexp1.html
+*/
+template <typename Encoding, typename Allocator = CrtAllocator>
+class GenericRegex {
+ public:
+  typedef Encoding EncodingType;
+  typedef typename Encoding::Ch Ch;
+  template <typename, typename>
+  friend class GenericRegexSearch;
+
+  GenericRegex(const Ch *source, Allocator *allocator = 0)
+      : ownAllocator_(allocator ? 0 : RAPIDJSON_NEW(Allocator)()),
+        allocator_(allocator ? allocator : ownAllocator_),
+        states_(allocator_, 256),
+        ranges_(allocator_, 256),
+        root_(kRegexInvalidState),
+        stateCount_(),
+        rangeCount_(),
+        anchorBegin_(),
+        anchorEnd_() {
+    GenericStringStream<Encoding> ss(source);
+    DecodedStream<GenericStringStream<Encoding>, Encoding> ds(ss);
+    Parse(ds);
+  }
+
+  ~GenericRegex() { RAPIDJSON_DELETE(ownAllocator_); }
+
+  bool IsValid() const { return root_ != kRegexInvalidState; }
+
+ private:
+  enum Operator {
+    kZeroOrOne,
+    kZeroOrMore,
+    kOneOrMore,
+    kConcatenation,
+    kAlternation,
+    kLeftParenthesis
+  };
+
+  static const unsigned kAnyCharacterClass = 0xFFFFFFFF;  //!< For '.'
+  static const unsigned kRangeCharacterClass = 0xFFFFFFFE;
+  static const unsigned kRangeNegationFlag = 0x80000000;
+
+  struct Range {
+    unsigned start;  //
+    unsigned end;
+    SizeType next;
+  };
+
+  struct State {
+    SizeType out;   //!< Equals to kInvalid for matching state
+    SizeType out1;  //!< Equals to non-kInvalid for split
+    SizeType rangeStart;
+    unsigned codepoint;
+  };
+
+  struct Frag {
+    Frag(SizeType s, SizeType o, SizeType m) : start(s), out(o), minIndex(m) {}
+    SizeType start;
+    SizeType out;  //!< link-list of all output states
+    SizeType minIndex;
+  };
+
+  State &GetState(SizeType index) {
+    RAPIDJSON_ASSERT(index < stateCount_);
+    return states_.template Bottom<State>()[index];
+  }
+
+  const State &GetState(SizeType index) const {
+    RAPIDJSON_ASSERT(index < stateCount_);
+    return states_.template Bottom<State>()[index];
+  }
+
+  Range &GetRange(SizeType index) {
+    RAPIDJSON_ASSERT(index < rangeCount_);
+    return ranges_.template Bottom<Range>()[index];
+  }
+
+  const Range &GetRange(SizeType index) const {
+    RAPIDJSON_ASSERT(index < rangeCount_);
+    return ranges_.template Bottom<Range>()[index];
+  }
+
+  template <typename InputStream>
+  void Parse(DecodedStream<InputStream, Encoding> &ds) {
+    Stack<Allocator> operandStack(allocator_, 256);   // Frag
+    Stack<Allocator> operatorStack(allocator_, 256);  // Operator
+    Stack<Allocator> atomCountStack(allocator_,
+                                    256);  // unsigned (Atom per parenthesis)
+
+    *atomCountStack.template Push<unsigned>() = 0;
+
+    unsigned codepoint;
+    while (ds.Peek() != 0) {
+      switch (codepoint = ds.Take()) {
+        case '^':
+          anchorBegin_ = true;
+          break;
+
+        case '$':
+          anchorEnd_ = true;
+          break;
+
+        case '|':
+          while (!operatorStack.Empty() &&
+                 *operatorStack.template Top<Operator>() < kAlternation)
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+              return;
+          *operatorStack.template Push<Operator>() = kAlternation;
+          *atomCountStack.template Top<unsigned>() = 0;
+          break;
+
+        case '(':
+          *operatorStack.template Push<Operator>() = kLeftParenthesis;
+          *atomCountStack.template Push<unsigned>() = 0;
+          break;
+
+        case ')':
+          while (!operatorStack.Empty() &&
+                 *operatorStack.template Top<Operator>() != kLeftParenthesis)
+            if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1)))
+              return;
+          if (operatorStack.Empty()) return;
+          operatorStack.template Pop<Operator>(1);
+          atomCountStack.template Pop<unsigned>(1);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '?':
+          if (!Eval(operandStack, kZeroOrOne)) return;
+          break;
+
+        case '*':
+          if (!Eval(operandStack, kZeroOrMore)) return;
+          break;
+
+        case '+':
+          if (!Eval(operandStack, kOneOrMore)) return;
+          break;
+
+        case '{': {
+          unsigned n, m;
+          if (!ParseUnsigned(ds, &n)) return;
+
+          if (ds.Peek() == ',') {
+            ds.Take();
+            if (ds.Peek() == '}')
+              m = kInfinityQuantifier;
+            else if (!ParseUnsigned(ds, &m) || m < n)
+              return;
+          } else
+            m = n;
+
+          if (!EvalQuantifier(operandStack, n, m) || ds.Peek() != '}') return;
+          ds.Take();
+        } break;
+
+        case '.':
+          PushOperand(operandStack, kAnyCharacterClass);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '[': {
+          SizeType range;
+          if (!ParseRange(ds, &range)) return;
+          SizeType s = NewState(kRegexInvalidState, kRegexInvalidState,
+                                kRangeCharacterClass);
+          GetState(s).rangeStart = range;
+          *operandStack.template Push<Frag>() = Frag(s, s, s);
+        }
+          ImplicitConcatenation(atomCountStack, operatorStack);
+          break;
+
+        case '\\':  // Escape character
+          if (!CharacterEscape(ds, &codepoint))
+            return;  // Unsupported escape character
+          // fall through to default
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+        default:  // Pattern character
+          PushOperand(operandStack, codepoint);
+          ImplicitConcatenation(atomCountStack, operatorStack);
+      }
+    }
+
+    while (!operatorStack.Empty())
+      if (!Eval(operandStack, *operatorStack.template Pop<Operator>(1))) return;
+
+    // Link the operand to matching state.
+    if (operandStack.GetSize() == sizeof(Frag)) {
+      Frag *e = operandStack.template Pop<Frag>(1);
+      Patch(e->out, NewState(kRegexInvalidState, kRegexInvalidState, 0));
+      root_ = e->start;
+
+#if RAPIDJSON_REGEX_VERBOSE
+      printf("root: %d\n", root_);
+      for (SizeType i = 0; i < stateCount_; i++) {
+        State &s = GetState(i);
+        printf("[%2d] out: %2d out1: %2d c: '%c'\n", i, s.out, s.out1,
+               (char)s.codepoint);
+      }
+      printf("\n");
+#endif
+    }
+  }
+
+  SizeType NewState(SizeType out, SizeType out1, unsigned codepoint) {
+    State *s = states_.template Push<State>();
+    s->out = out;
+    s->out1 = out1;
+    s->codepoint = codepoint;
+    s->rangeStart = kRegexInvalidRange;
+    return stateCount_++;
+  }
+
+  void PushOperand(Stack<Allocator> &operandStack, unsigned codepoint) {
+    SizeType s = NewState(kRegexInvalidState, kRegexInvalidState, codepoint);
+    *operandStack.template Push<Frag>() = Frag(s, s, s);
+  }
+
+  void ImplicitConcatenation(Stack<Allocator> &atomCountStack,
+                             Stack<Allocator> &operatorStack) {
+    if (*atomCountStack.template Top<unsigned>())
+      *operatorStack.template Push<Operator>() = kConcatenation;
+    (*atomCountStack.template Top<unsigned>())++;
+  }
+
+  SizeType Append(SizeType l1, SizeType l2) {
+    SizeType old = l1;
+    while (GetState(l1).out != kRegexInvalidState) l1 = GetState(l1).out;
+    GetState(l1).out = l2;
+    return old;
+  }
+
+  void Patch(SizeType l, SizeType s) {
+    for (SizeType next; l != kRegexInvalidState; l = next) {
+      next = GetState(l).out;
+      GetState(l).out = s;
+    }
+  }
+
+  bool Eval(Stack<Allocator> &operandStack, Operator op) {
+    switch (op) {
+      case kConcatenation:
+        RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag) * 2);
+        {
+          Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
+          Patch(e1.out, e2.start);
+          *operandStack.template Push<Frag>() =
+              Frag(e1.start, e2.out, Min(e1.minIndex, e2.minIndex));
+        }
+        return true;
+
+      case kAlternation:
+        if (operandStack.GetSize() >= sizeof(Frag) * 2) {
+          Frag e2 = *operandStack.template Pop<Frag>(1);
+          Frag e1 = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(e1.start, e2.start, 0);
+          *operandStack.template Push<Frag>() =
+              Frag(s, Append(e1.out, e2.out), Min(e1.minIndex, e2.minIndex));
+          return true;
+        }
+        return false;
+
+      case kZeroOrOne:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          *operandStack.template Push<Frag>() =
+              Frag(s, Append(e.out, s), e.minIndex);
+          return true;
+        }
+        return false;
+
+      case kZeroOrMore:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(s, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      case kOneOrMore:
+        if (operandStack.GetSize() >= sizeof(Frag)) {
+          Frag e = *operandStack.template Pop<Frag>(1);
+          SizeType s = NewState(kRegexInvalidState, e.start, 0);
+          Patch(e.out, s);
+          *operandStack.template Push<Frag>() = Frag(e.start, s, e.minIndex);
+          return true;
+        }
+        return false;
+
+      default:
+        // syntax error (e.g. unclosed kLeftParenthesis)
+        return false;
+    }
+  }
+
+  bool EvalQuantifier(Stack<Allocator> &operandStack, unsigned n, unsigned m) {
+    RAPIDJSON_ASSERT(n <= m);
+    RAPIDJSON_ASSERT(operandStack.GetSize() >= sizeof(Frag));
+
+    if (n == 0) {
+      if (m == 0)  // a{0} not support
+        return false;
+      else if (m == kInfinityQuantifier)
+        Eval(operandStack, kZeroOrMore);  // a{0,} -> a*
+      else {
+        Eval(operandStack, kZeroOrOne);  // a{0,5} -> a?
+        for (unsigned i = 0; i < m - 1; i++)
+          CloneTopOperand(operandStack);  // a{0,5} -> a? a? a? a? a?
+        for (unsigned i = 0; i < m - 1; i++)
+          Eval(operandStack, kConcatenation);  // a{0,5} -> a?a?a?a?a?
+      }
+      return true;
+    }
+
+    for (unsigned i = 0; i < n - 1; i++)  // a{3} -> a a a
+      CloneTopOperand(operandStack);
+
+    if (m == kInfinityQuantifier)
+      Eval(operandStack, kOneOrMore);  // a{3,} -> a a a+
+    else if (m > n) {
+      CloneTopOperand(operandStack);   // a{3,5} -> a a a a
+      Eval(operandStack, kZeroOrOne);  // a{3,5} -> a a a a?
+      for (unsigned i = n; i < m - 1; i++)
+        CloneTopOperand(operandStack);  // a{3,5} -> a a a a? a?
+      for (unsigned i = n; i < m; i++)
+        Eval(operandStack, kConcatenation);  // a{3,5} -> a a aa?a?
+    }
+
+    for (unsigned i = 0; i < n - 1; i++)
+      Eval(operandStack,
+           kConcatenation);  // a{3} -> aaa, a{3,} -> aaa+, a{3.5} -> aaaa?a?
+
+    return true;
+  }
+
+  static SizeType Min(SizeType a, SizeType b) { return a < b ? a : b; }
+
+  void CloneTopOperand(Stack<Allocator> &operandStack) {
+    const Frag src =
+        *operandStack
+             .template Top<Frag>();  // Copy constructor to prevent invalidation
+    SizeType count =
+        stateCount_ - src.minIndex;  // Assumes top operand contains states in
+                                     // [src->minIndex, stateCount_)
+    State *s = states_.template Push<State>(count);
+    memcpy(s, &GetState(src.minIndex), count * sizeof(State));
+    for (SizeType j = 0; j < count; j++) {
+      if (s[j].out != kRegexInvalidState) s[j].out += count;
+      if (s[j].out1 != kRegexInvalidState) s[j].out1 += count;
+    }
+    *operandStack.template Push<Frag>() =
+        Frag(src.start + count, src.out + count, src.minIndex + count);
+    stateCount_ += count;
+  }
+
+  template <typename InputStream>
+  bool ParseUnsigned(DecodedStream<InputStream, Encoding> &ds, unsigned *u) {
+    unsigned r = 0;
+    if (ds.Peek() < '0' || ds.Peek() > '9') return false;
+    while (ds.Peek() >= '0' && ds.Peek() <= '9') {
+      if (r >= 429496729 && ds.Peek() > '5')  // 2^32 - 1 = 4294967295
+        return false;                         // overflow
+      r = r * 10 + (ds.Take() - '0');
+    }
+    *u = r;
+    return true;
+  }
+
+  template <typename InputStream>
+  bool ParseRange(DecodedStream<InputStream, Encoding> &ds, SizeType *range) {
+    bool isBegin = true;
+    bool negate = false;
+    int step = 0;
+    SizeType start = kRegexInvalidRange;
+    SizeType current = kRegexInvalidRange;
+    unsigned codepoint;
+    while ((codepoint = ds.Take()) != 0) {
+      if (isBegin) {
+        isBegin = false;
+        if (codepoint == '^') {
+          negate = true;
+          continue;
+        }
+      }
+
+      switch (codepoint) {
+        case ']':
+          if (start == kRegexInvalidRange)
+            return false;   // Error: nothing inside []
+          if (step == 2) {  // Add trailing '-'
+            SizeType r = NewRange('-');
+            RAPIDJSON_ASSERT(current != kRegexInvalidRange);
+            GetRange(current).next = r;
+          }
+          if (negate) GetRange(start).start |= kRangeNegationFlag;
+          *range = start;
+          return true;
+
+        case '\\':
+          if (ds.Peek() == 'b') {
+            ds.Take();
+            codepoint = 0x0008;  // Escape backspace character
+          } else if (!CharacterEscape(ds, &codepoint))
+            return false;
+          // fall through to default
+          RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+        default:
+          switch (step) {
+            case 1:
+              if (codepoint == '-') {
+                step++;
+                break;
+              }
+              // fall through to step 0 for other characters
+              RAPIDJSON_DELIBERATE_FALLTHROUGH;
+
+            case 0: {
+              SizeType r = NewRange(codepoint);
+              if (current != kRegexInvalidRange) GetRange(current).next = r;
+              if (start == kRegexInvalidRange) start = r;
+              current = r;
+            }
+              step = 1;
+              break;
+
+            default:
+              RAPIDJSON_ASSERT(step == 2);
+              GetRange(current).end = codepoint;
+              step = 0;
+          }
+      }
+    }
+    return false;
+  }
+
+  SizeType NewRange(unsigned codepoint) {
+    Range *r = ranges_.template Push<Range>();
+    r->start = r->end = codepoint;
+    r->next = kRegexInvalidRange;
+    return rangeCount_++;
+  }
+
+  template <typename InputStream>
+  bool CharacterEscape(DecodedStream<InputStream, Encoding> &ds,
+                       unsigned *escapedCodepoint) {
+    unsigned codepoint;
+    switch (codepoint = ds.Take()) {
+      case '^':
+      case '$':
+      case '|':
+      case '(':
+      case ')':
+      case '?':
+      case '*':
+      case '+':
+      case '.':
+      case '[':
+      case ']':
+      case '{':
+      case '}':
+      case '\\':
+        *escapedCodepoint = codepoint;
+        return true;
+      case 'f':
+        *escapedCodepoint = 0x000C;
+        return true;
+      case 'n':
+        *escapedCodepoint = 0x000A;
+        return true;
+      case 'r':
+        *escapedCodepoint = 0x000D;
+        return true;
+      case 't':
+        *escapedCodepoint = 0x0009;
+        return true;
+      case 'v':
+        *escapedCodepoint = 0x000B;
+        return true;
+      default:
+        return false;  // Unsupported escape character
+    }
+  }
+
+  Allocator *ownAllocator_;
+  Allocator *allocator_;
+  Stack<Allocator> states_;
+  Stack<Allocator> ranges_;
+  SizeType root_;
+  SizeType stateCount_;
+  SizeType rangeCount_;
+
+  static const unsigned kInfinityQuantifier = ~0u;
+
+  // For SearchWithAnchoring()
+  bool anchorBegin_;
+  bool anchorEnd_;
+};
+
+template <typename RegexType, typename Allocator = CrtAllocator>
+class GenericRegexSearch {
+ public:
+  typedef typename RegexType::EncodingType Encoding;
+  typedef typename Encoding::Ch Ch;
+
+  GenericRegexSearch(const RegexType &regex, Allocator *allocator = 0)
+      : regex_(regex),
+        allocator_(allocator),
+        ownAllocator_(0),
+        state0_(allocator, 0),
+        state1_(allocator, 0),
+        stateSet_() {
+    RAPIDJSON_ASSERT(regex_.IsValid());
+    if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+    stateSet_ = static_cast<unsigned *>(allocator_->Malloc(GetStateSetSize()));
+    state0_.template Reserve<SizeType>(regex_.stateCount_);
+    state1_.template Reserve<SizeType>(regex_.stateCount_);
+  }
+
+  ~GenericRegexSearch() {
+    Allocator::Free(stateSet_);
+    RAPIDJSON_DELETE(ownAllocator_);
+  }
+
+  template <typename InputStream>
+  bool Match(InputStream &is) {
+    return SearchWithAnchoring(is, true, true);
+  }
+
+  bool Match(const Ch *s) {
+    GenericStringStream<Encoding> is(s);
+    return Match(is);
+  }
+
+  template <typename InputStream>
+  bool Search(InputStream &is) {
+    return SearchWithAnchoring(is, regex_.anchorBegin_, regex_.anchorEnd_);
+  }
+
+  bool Search(const Ch *s) {
+    GenericStringStream<Encoding> is(s);
+    return Search(is);
+  }
+
+ private:
+  typedef typename RegexType::State State;
+  typedef typename RegexType::Range Range;
+
+  template <typename InputStream>
+  bool SearchWithAnchoring(InputStream &is, bool anchorBegin, bool anchorEnd) {
+    DecodedStream<InputStream, Encoding> ds(is);
+
+    state0_.Clear();
+    Stack<Allocator> *current = &state0_, *next = &state1_;
+    const size_t stateSetSize = GetStateSetSize();
+    std::memset(stateSet_, 0, stateSetSize);
+
+    bool matched = AddState(*current, regex_.root_);
+    unsigned codepoint;
+    while (!current->Empty() && (codepoint = ds.Take()) != 0) {
+      std::memset(stateSet_, 0, stateSetSize);
+      next->Clear();
+      matched = false;
+      for (const SizeType *s = current->template Bottom<SizeType>();
+           s != current->template End<SizeType>(); ++s) {
+        const State &sr = regex_.GetState(*s);
+        if (sr.codepoint == codepoint ||
+            sr.codepoint == RegexType::kAnyCharacterClass ||
+            (sr.codepoint == RegexType::kRangeCharacterClass &&
+             MatchRange(sr.rangeStart, codepoint))) {
+          matched = AddState(*next, sr.out) || matched;
+          if (!anchorEnd && matched) return true;
+        }
+        if (!anchorBegin) AddState(*next, regex_.root_);
+      }
+      internal::Swap(current, next);
+    }
+
+    return matched;
+  }
+
+  size_t GetStateSetSize() const { return (regex_.stateCount_ + 31) / 32 * 4; }
+
+  // Return whether the added states is a match state
+  bool AddState(Stack<Allocator> &l, SizeType index) {
+    RAPIDJSON_ASSERT(index != kRegexInvalidState);
+
+    const State &s = regex_.GetState(index);
+    if (s.out1 != kRegexInvalidState) {  // Split
+      bool matched = AddState(l, s.out);
+      return AddState(l, s.out1) || matched;
+    } else if (!(stateSet_[index >> 5] & (1u << (index & 31)))) {
+      stateSet_[index >> 5] |= (1u << (index & 31));
+      *l.template PushUnsafe<SizeType>() = index;
+    }
+    return s.out ==
+           kRegexInvalidState;  // by using PushUnsafe() above, we can ensure s
+                                // is not validated due to reallocation.
+  }
+
+  bool MatchRange(SizeType rangeIndex, unsigned codepoint) const {
+    bool yes = (regex_.GetRange(rangeIndex).start &
+                RegexType::kRangeNegationFlag) == 0;
+    while (rangeIndex != kRegexInvalidRange) {
+      const Range &r = regex_.GetRange(rangeIndex);
+      if (codepoint >= (r.start & ~RegexType::kRangeNegationFlag) &&
+          codepoint <= r.end)
+        return yes;
+      rangeIndex = r.next;
+    }
+    return !yes;
+  }
+
+  const RegexType &regex_;
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  Stack<Allocator> state0_;
+  Stack<Allocator> state1_;
+  uint32_t *stateSet_;
+};
+
+typedef GenericRegex<UTF8<>> Regex;
+typedef GenericRegexSearch<Regex> RegexSearch;
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_REGEX_H_

livox/common/rapidjson/internal/stack.h

@@ -0,0 +1,245 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_STACK_H_
+#define RAPIDJSON_INTERNAL_STACK_H_
+
+#include <cstddef>
+#include "../allocators.h"
+#include "swap.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+///////////////////////////////////////////////////////////////////////////////
+// Stack
+
+//! A type-unsafe stack for storing different types of data.
+/*! \tparam Allocator Allocator for allocating stack memory.
+ */
+template <typename Allocator>
+class Stack {
+ public:
+  // Optimization note: Do not allocate memory for stack_ in constructor.
+  // Do it lazily when first Push() -> Expand() -> Resize().
+  Stack(Allocator *allocator, size_t stackCapacity)
+      : allocator_(allocator),
+        ownAllocator_(0),
+        stack_(0),
+        stackTop_(0),
+        stackEnd_(0),
+        initialCapacity_(stackCapacity) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Stack(Stack &&rhs)
+      : allocator_(rhs.allocator_),
+        ownAllocator_(rhs.ownAllocator_),
+        stack_(rhs.stack_),
+        stackTop_(rhs.stackTop_),
+        stackEnd_(rhs.stackEnd_),
+        initialCapacity_(rhs.initialCapacity_) {
+    rhs.allocator_ = 0;
+    rhs.ownAllocator_ = 0;
+    rhs.stack_ = 0;
+    rhs.stackTop_ = 0;
+    rhs.stackEnd_ = 0;
+    rhs.initialCapacity_ = 0;
+  }
+#endif
+
+  ~Stack() { Destroy(); }
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  Stack &operator=(Stack &&rhs) {
+    if (&rhs != this) {
+      Destroy();
+
+      allocator_ = rhs.allocator_;
+      ownAllocator_ = rhs.ownAllocator_;
+      stack_ = rhs.stack_;
+      stackTop_ = rhs.stackTop_;
+      stackEnd_ = rhs.stackEnd_;
+      initialCapacity_ = rhs.initialCapacity_;
+
+      rhs.allocator_ = 0;
+      rhs.ownAllocator_ = 0;
+      rhs.stack_ = 0;
+      rhs.stackTop_ = 0;
+      rhs.stackEnd_ = 0;
+      rhs.initialCapacity_ = 0;
+    }
+    return *this;
+  }
+#endif
+
+  void Swap(Stack &rhs) RAPIDJSON_NOEXCEPT {
+    internal::Swap(allocator_, rhs.allocator_);
+    internal::Swap(ownAllocator_, rhs.ownAllocator_);
+    internal::Swap(stack_, rhs.stack_);
+    internal::Swap(stackTop_, rhs.stackTop_);
+    internal::Swap(stackEnd_, rhs.stackEnd_);
+    internal::Swap(initialCapacity_, rhs.initialCapacity_);
+  }
+
+  void Clear() { stackTop_ = stack_; }
+
+  void ShrinkToFit() {
+    if (Empty()) {
+      // If the stack is empty, completely deallocate the memory.
+      Allocator::Free(stack_);  // NOLINT (+clang-analyzer-unix.Malloc)
+      stack_ = 0;
+      stackTop_ = 0;
+      stackEnd_ = 0;
+    } else
+      Resize(GetSize());
+  }
+
+  // Optimization note: try to minimize the size of this function for force
+  // inline. Expansion is run very infrequently, so it is moved to another
+  // (probably non-inline) function.
+  template <typename T>
+  RAPIDJSON_FORCEINLINE void Reserve(size_t count = 1) {
+    // Expand the stack if needed
+    if (RAPIDJSON_UNLIKELY(static_cast<std::ptrdiff_t>(sizeof(T) * count) >
+                           (stackEnd_ - stackTop_)))
+      Expand<T>(count);
+  }
+
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *Push(size_t count = 1) {
+    Reserve<T>(count);
+    return PushUnsafe<T>(count);
+  }
+
+  template <typename T>
+  RAPIDJSON_FORCEINLINE T *PushUnsafe(size_t count = 1) {
+    RAPIDJSON_ASSERT(stackTop_);
+    RAPIDJSON_ASSERT(static_cast<std::ptrdiff_t>(sizeof(T) * count) <=
+                     (stackEnd_ - stackTop_));
+    T *ret = reinterpret_cast<T *>(stackTop_);
+    stackTop_ += sizeof(T) * count;
+    return ret;
+  }
+
+  template <typename T>
+  T *Pop(size_t count) {
+    RAPIDJSON_ASSERT(GetSize() >= count * sizeof(T));
+    stackTop_ -= count * sizeof(T);
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  T *Top() {
+    RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
+    return reinterpret_cast<T *>(stackTop_ - sizeof(T));
+  }
+
+  template <typename T>
+  const T *Top() const {
+    RAPIDJSON_ASSERT(GetSize() >= sizeof(T));
+    return reinterpret_cast<T *>(stackTop_ - sizeof(T));
+  }
+
+  template <typename T>
+  T *End() {
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  const T *End() const {
+    return reinterpret_cast<T *>(stackTop_);
+  }
+
+  template <typename T>
+  T *Bottom() {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  template <typename T>
+  const T *Bottom() const {
+    return reinterpret_cast<T *>(stack_);
+  }
+
+  bool HasAllocator() const { return allocator_ != 0; }
+
+  Allocator &GetAllocator() {
+    RAPIDJSON_ASSERT(allocator_);
+    return *allocator_;
+  }
+
+  bool Empty() const { return stackTop_ == stack_; }
+  size_t GetSize() const { return static_cast<size_t>(stackTop_ - stack_); }
+  size_t GetCapacity() const { return static_cast<size_t>(stackEnd_ - stack_); }
+
+ private:
+  template <typename T>
+  void Expand(size_t count) {
+    // Only expand the capacity if the current stack exists. Otherwise just
+    // create a stack with initial capacity.
+    size_t newCapacity;
+    if (stack_ == 0) {
+      if (!allocator_) ownAllocator_ = allocator_ = RAPIDJSON_NEW(Allocator)();
+      newCapacity = initialCapacity_;
+    } else {
+      newCapacity = GetCapacity();
+      newCapacity += (newCapacity + 1) / 2;
+    }
+    size_t newSize = GetSize() + sizeof(T) * count;
+    if (newCapacity < newSize) newCapacity = newSize;
+
+    Resize(newCapacity);
+  }
+
+  void Resize(size_t newCapacity) {
+    const size_t size = GetSize();  // Backup the current size
+    stack_ = static_cast<char *>(
+        allocator_->Realloc(stack_, GetCapacity(), newCapacity));
+    stackTop_ = stack_ + size;
+    stackEnd_ = stack_ + newCapacity;
+  }
+
+  void Destroy() {
+    Allocator::Free(stack_);
+    RAPIDJSON_DELETE(ownAllocator_);  // Only delete if it is owned by the stack
+  }
+
+  // Prohibit copy constructor & assignment operator.
+  Stack(const Stack &);
+  Stack &operator=(const Stack &);
+
+  Allocator *allocator_;
+  Allocator *ownAllocator_;
+  char *stack_;
+  char *stackTop_;
+  char *stackEnd_;
+  size_t initialCapacity_;
+};
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_STACK_H_

livox/common/rapidjson/internal/strfunc.h

@@ -0,0 +1,74 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_STRFUNC_H_
+#define RAPIDJSON_INTERNAL_STRFUNC_H_
+
+#include <cwchar>
+#include "../stream.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Custom strlen() which works on different character types.
+/*! \tparam Ch Character type (e.g. char, wchar_t, short)
+    \param s Null-terminated input string.
+    \return Number of characters in the string.
+    \note This has the same semantics as strlen(), the return value is not
+   number of Unicode codepoints.
+*/
+template <typename Ch>
+inline SizeType StrLen(const Ch *s) {
+  RAPIDJSON_ASSERT(s != 0);
+  const Ch *p = s;
+  while (*p) ++p;
+  return SizeType(p - s);
+}
+
+template <>
+inline SizeType StrLen(const char *s) {
+  return SizeType(std::strlen(s));
+}
+
+template <>
+inline SizeType StrLen(const wchar_t *s) {
+  return SizeType(std::wcslen(s));
+}
+
+//! Returns number of code points in a encoded string.
+template <typename Encoding>
+bool CountStringCodePoint(const typename Encoding::Ch *s, SizeType length,
+                          SizeType *outCount) {
+  RAPIDJSON_ASSERT(s != 0);
+  RAPIDJSON_ASSERT(outCount != 0);
+  GenericStringStream<Encoding> is(s);
+  const typename Encoding::Ch *end = s + length;
+  SizeType count = 0;
+  while (is.src_ < end) {
+    unsigned codepoint;
+    if (!Encoding::Decode(is, &codepoint)) return false;
+    count++;
+  }
+  *outCount = count;
+  return true;
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_INTERNAL_STRFUNC_H_

livox/common/rapidjson/internal/strtod.h

@@ -0,0 +1,303 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_STRTOD_
+#define RAPIDJSON_STRTOD_
+
+#include <climits>
+#include <limits>
+#include "biginteger.h"
+#include "diyfp.h"
+#include "ieee754.h"
+#include "pow10.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+inline double FastPath(double significand, int exp) {
+  if (exp < -308)
+    return 0.0;
+  else if (exp >= 0)
+    return significand * internal::Pow10(exp);
+  else
+    return significand / internal::Pow10(-exp);
+}
+
+inline double StrtodNormalPrecision(double d, int p) {
+  if (p < -308) {
+    // Prevent expSum < -308, making Pow10(p) = 0
+    d = FastPath(d, -308);
+    d = FastPath(d, p + 308);
+  } else
+    d = FastPath(d, p);
+  return d;
+}
+
+template <typename T>
+inline T Min3(T a, T b, T c) {
+  T m = a;
+  if (m > b) m = b;
+  if (m > c) m = c;
+  return m;
+}
+
+inline int CheckWithinHalfULP(double b, const BigInteger &d, int dExp) {
+  const Double db(b);
+  const uint64_t bInt = db.IntegerSignificand();
+  const int bExp = db.IntegerExponent();
+  const int hExp = bExp - 1;
+
+  int dS_Exp2 = 0, dS_Exp5 = 0, bS_Exp2 = 0, bS_Exp5 = 0, hS_Exp2 = 0,
+      hS_Exp5 = 0;
+
+  // Adjust for decimal exponent
+  if (dExp >= 0) {
+    dS_Exp2 += dExp;
+    dS_Exp5 += dExp;
+  } else {
+    bS_Exp2 -= dExp;
+    bS_Exp5 -= dExp;
+    hS_Exp2 -= dExp;
+    hS_Exp5 -= dExp;
+  }
+
+  // Adjust for binary exponent
+  if (bExp >= 0)
+    bS_Exp2 += bExp;
+  else {
+    dS_Exp2 -= bExp;
+    hS_Exp2 -= bExp;
+  }
+
+  // Adjust for half ulp exponent
+  if (hExp >= 0)
+    hS_Exp2 += hExp;
+  else {
+    dS_Exp2 -= hExp;
+    bS_Exp2 -= hExp;
+  }
+
+  // Remove common power of two factor from all three scaled values
+  int common_Exp2 = Min3(dS_Exp2, bS_Exp2, hS_Exp2);
+  dS_Exp2 -= common_Exp2;
+  bS_Exp2 -= common_Exp2;
+  hS_Exp2 -= common_Exp2;
+
+  BigInteger dS = d;
+  dS.MultiplyPow5(static_cast<unsigned>(dS_Exp5)) <<=
+      static_cast<unsigned>(dS_Exp2);
+
+  BigInteger bS(bInt);
+  bS.MultiplyPow5(static_cast<unsigned>(bS_Exp5)) <<=
+      static_cast<unsigned>(bS_Exp2);
+
+  BigInteger hS(1);
+  hS.MultiplyPow5(static_cast<unsigned>(hS_Exp5)) <<=
+      static_cast<unsigned>(hS_Exp2);
+
+  BigInteger delta(0);
+  dS.Difference(bS, &delta);
+
+  return delta.Compare(hS);
+}
+
+inline bool StrtodFast(double d, int p, double *result) {
+  // Use fast path for string-to-double conversion if possible
+  // see
+  // http://www.exploringbinary.com/fast-path-decimal-to-floating-point-conversion/
+  if (p > 22 && p < 22 + 16) {
+    // Fast Path Cases In Disguise
+    d *= internal::Pow10(p - 22);
+    p = 22;
+  }
+
+  if (p >= -22 && p <= 22 && d <= 9007199254740991.0) {  // 2^53 - 1
+    *result = FastPath(d, p);
+    return true;
+  } else
+    return false;
+}
+
+// Compute an approximation and see if it is within 1/2 ULP
+inline bool StrtodDiyFp(const char *decimals, int dLen, int dExp,
+                        double *result) {
+  uint64_t significand = 0;
+  int i = 0;  // 2^64 - 1 = 18446744073709551615, 1844674407370955161 =
+              // 0x1999999999999999
+  for (; i < dLen; i++) {
+    if (significand > RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) ||
+        (significand == RAPIDJSON_UINT64_C2(0x19999999, 0x99999999) &&
+         decimals[i] > '5'))
+      break;
+    significand = significand * 10u + static_cast<unsigned>(decimals[i] - '0');
+  }
+
+  if (i < dLen && decimals[i] >= '5')  // Rounding
+    significand++;
+
+  int remaining = dLen - i;
+  const int kUlpShift = 3;
+  const int kUlp = 1 << kUlpShift;
+  int64_t error = (remaining == 0) ? 0 : kUlp / 2;
+
+  DiyFp v(significand, 0);
+  v = v.Normalize();
+  error <<= -v.e;
+
+  dExp += remaining;
+
+  int actualExp;
+  DiyFp cachedPower = GetCachedPower10(dExp, &actualExp);
+  if (actualExp != dExp) {
+    static const DiyFp kPow10[] = {
+        DiyFp(RAPIDJSON_UINT64_C2(0xa0000000, 0x00000000), -60),  // 10^1
+        DiyFp(RAPIDJSON_UINT64_C2(0xc8000000, 0x00000000), -57),  // 10^2
+        DiyFp(RAPIDJSON_UINT64_C2(0xfa000000, 0x00000000), -54),  // 10^3
+        DiyFp(RAPIDJSON_UINT64_C2(0x9c400000, 0x00000000), -50),  // 10^4
+        DiyFp(RAPIDJSON_UINT64_C2(0xc3500000, 0x00000000), -47),  // 10^5
+        DiyFp(RAPIDJSON_UINT64_C2(0xf4240000, 0x00000000), -44),  // 10^6
+        DiyFp(RAPIDJSON_UINT64_C2(0x98968000, 0x00000000), -40)   // 10^7
+    };
+    int adjustment = dExp - actualExp;
+    RAPIDJSON_ASSERT(adjustment >= 1 && adjustment < 8);
+    v = v * kPow10[adjustment - 1];
+    if (dLen + adjustment >
+        19)  // has more digits than decimal digits in 64-bit
+      error += kUlp / 2;
+  }
+
+  v = v * cachedPower;
+
+  error += kUlp + (error == 0 ? 0 : 1);
+
+  const int oldExp = v.e;
+  v = v.Normalize();
+  error <<= oldExp - v.e;
+
+  const int effectiveSignificandSize =
+      Double::EffectiveSignificandSize(64 + v.e);
+  int precisionSize = 64 - effectiveSignificandSize;
+  if (precisionSize + kUlpShift >= 64) {
+    int scaleExp = (precisionSize + kUlpShift) - 63;
+    v.f >>= scaleExp;
+    v.e += scaleExp;
+    error = (error >> scaleExp) + 1 + kUlp;
+    precisionSize -= scaleExp;
+  }
+
+  DiyFp rounded(v.f >> precisionSize, v.e + precisionSize);
+  const uint64_t precisionBits =
+      (v.f & ((uint64_t(1) << precisionSize) - 1)) * kUlp;
+  const uint64_t halfWay = (uint64_t(1) << (precisionSize - 1)) * kUlp;
+  if (precisionBits >= halfWay + static_cast<unsigned>(error)) {
+    rounded.f++;
+    if (rounded.f & (DiyFp::kDpHiddenBit
+                     << 1)) {  // rounding overflows mantissa (issue #340)
+      rounded.f >>= 1;
+      rounded.e++;
+    }
+  }
+
+  *result = rounded.ToDouble();
+
+  return halfWay - static_cast<unsigned>(error) >= precisionBits ||
+         precisionBits >= halfWay + static_cast<unsigned>(error);
+}
+
+inline double StrtodBigInteger(double approx, const char *decimals, int dLen,
+                               int dExp) {
+  RAPIDJSON_ASSERT(dLen >= 0);
+  const BigInteger dInt(decimals, static_cast<unsigned>(dLen));
+  Double a(approx);
+  int cmp = CheckWithinHalfULP(a.Value(), dInt, dExp);
+  if (cmp < 0)
+    return a.Value();  // within half ULP
+  else if (cmp == 0) {
+    // Round towards even
+    if (a.Significand() & 1)
+      return a.NextPositiveDouble();
+    else
+      return a.Value();
+  } else  // adjustment
+    return a.NextPositiveDouble();
+}
+
+inline double StrtodFullPrecision(double d, int p, const char *decimals,
+                                  size_t length, size_t decimalPosition,
+                                  int exp) {
+  RAPIDJSON_ASSERT(d >= 0.0);
+  RAPIDJSON_ASSERT(length >= 1);
+
+  double result = 0.0;
+  if (StrtodFast(d, p, &result)) return result;
+
+  RAPIDJSON_ASSERT(length <= INT_MAX);
+  int dLen = static_cast<int>(length);
+
+  RAPIDJSON_ASSERT(length >= decimalPosition);
+  RAPIDJSON_ASSERT(length - decimalPosition <= INT_MAX);
+  int dExpAdjust = static_cast<int>(length - decimalPosition);
+
+  RAPIDJSON_ASSERT(exp >= INT_MIN + dExpAdjust);
+  int dExp = exp - dExpAdjust;
+
+  // Make sure length+dExp does not overflow
+  RAPIDJSON_ASSERT(dExp <= INT_MAX - dLen);
+
+  // Trim leading zeros
+  while (dLen > 0 && *decimals == '0') {
+    dLen--;
+    decimals++;
+  }
+
+  // Trim trailing zeros
+  while (dLen > 0 && decimals[dLen - 1] == '0') {
+    dLen--;
+    dExp++;
+  }
+
+  if (dLen == 0) {  // Buffer only contains zeros.
+    return 0.0;
+  }
+
+  // Trim right-most digits
+  const int kMaxDecimalDigit = 767 + 1;
+  if (dLen > kMaxDecimalDigit) {
+    dExp += dLen - kMaxDecimalDigit;
+    dLen = kMaxDecimalDigit;
+  }
+
+  // If too small, underflow to zero.
+  // Any x <= 10^-324 is interpreted as zero.
+  if (dLen + dExp <= -324) return 0.0;
+
+  // If too large, overflow to infinity.
+  // Any x >= 10^309 is interpreted as +infinity.
+  if (dLen + dExp > 309) return std::numeric_limits<double>::infinity();
+
+  if (StrtodDiyFp(decimals, dLen, dExp, &result)) return result;
+
+  // Use approximation from StrtodDiyFp and make adjustment with BigInteger
+  // comparison
+  return StrtodBigInteger(result, decimals, dLen, dExp);
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_STRTOD_

livox/common/rapidjson/internal/swap.h

@@ -0,0 +1,50 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_INTERNAL_SWAP_H_
+#define RAPIDJSON_INTERNAL_SWAP_H_
+
+#include "../rapidjson.h"
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+namespace internal {
+
+//! Custom swap() to avoid dependency on C++ <algorithm> header
+/*! \tparam T Type of the arguments to swap, should be instantiated with
+   primitive C++ types only. \note This has the same semantics as std::swap().
+*/
+template <typename T>
+inline void Swap(T &a, T &b) RAPIDJSON_NOEXCEPT {
+  T tmp = a;
+  a = b;
+  b = tmp;
+}
+
+}  // namespace internal
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_INTERNAL_SWAP_H_

livox/common/rapidjson/istreamwrapper.h

@@ -0,0 +1,161 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_ISTREAMWRAPPER_H_
+#define RAPIDJSON_ISTREAMWRAPPER_H_
+
+#include <ios>
+#include <iosfwd>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#elif defined(_MSC_VER)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(4351)  // new behavior: elements of array 'array' will be
+                          // default initialized
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of \c std::basic_istream into RapidJSON's Stream concept.
+/*!
+    The classes can be wrapped including but not limited to:
+
+    - \c std::istringstream
+    - \c std::stringstream
+    - \c std::wistringstream
+    - \c std::wstringstream
+    - \c std::ifstream
+    - \c std::fstream
+    - \c std::wifstream
+    - \c std::wfstream
+
+    \tparam StreamType Class derived from \c std::basic_istream.
+*/
+
+template <typename StreamType>
+class BasicIStreamWrapper {
+ public:
+  typedef typename StreamType::char_type Ch;
+
+  //! Constructor.
+  /*!
+      \param stream stream opened for read.
+  */
+  BasicIStreamWrapper(StreamType &stream)
+      : stream_(stream),
+        buffer_(peekBuffer_),
+        bufferSize_(4),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    Read();
+  }
+
+  //! Constructor.
+  /*!
+      \param stream stream opened for read.
+      \param buffer user-supplied buffer.
+      \param bufferSize size of buffer in bytes. Must >=4 bytes.
+  */
+  BasicIStreamWrapper(StreamType &stream, char *buffer, size_t bufferSize)
+      : stream_(stream),
+        buffer_(buffer),
+        bufferSize_(bufferSize),
+        bufferLast_(0),
+        current_(buffer_),
+        readCount_(0),
+        count_(0),
+        eof_(false) {
+    RAPIDJSON_ASSERT(bufferSize >= 4);
+    Read();
+  }
+
+  Ch Peek() const { return *current_; }
+  Ch Take() {
+    Ch c = *current_;
+    Read();
+    return c;
+  }
+  size_t Tell() const {
+    return count_ + static_cast<size_t>(current_ - buffer_);
+  }
+
+  // Not implemented
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const {
+    return (current_ + 4 - !eof_ <= bufferLast_) ? current_ : 0;
+  }
+
+ private:
+  BasicIStreamWrapper();
+  BasicIStreamWrapper(const BasicIStreamWrapper &);
+  BasicIStreamWrapper &operator=(const BasicIStreamWrapper &);
+
+  void Read() {
+    if (current_ < bufferLast_)
+      ++current_;
+    else if (!eof_) {
+      count_ += readCount_;
+      readCount_ = bufferSize_;
+      bufferLast_ = buffer_ + readCount_ - 1;
+      current_ = buffer_;
+
+      if (!stream_.read(buffer_, static_cast<std::streamsize>(bufferSize_))) {
+        readCount_ = static_cast<size_t>(stream_.gcount());
+        *(bufferLast_ = buffer_ + readCount_) = '\0';
+        eof_ = true;
+      }
+    }
+  }
+
+  StreamType &stream_;
+  Ch peekBuffer_[4], *buffer_;
+  size_t bufferSize_;
+  Ch *bufferLast_;
+  Ch *current_;
+  size_t readCount_;
+  size_t count_;  //!< Number of characters read
+  bool eof_;
+};
+
+typedef BasicIStreamWrapper<std::istream> IStreamWrapper;
+typedef BasicIStreamWrapper<std::wistream> WIStreamWrapper;
+
+#if defined(__clang__) || defined(_MSC_VER)
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_ISTREAMWRAPPER_H_

livox/common/rapidjson/memorybuffer.h

@@ -0,0 +1,78 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_MEMORYBUFFER_H_
+#define RAPIDJSON_MEMORYBUFFER_H_
+
+#include "internal/stack.h"
+#include "stream.h"
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory output byte stream.
+/*!
+    This class is mainly for being wrapped by EncodedOutputStream or
+   AutoUTFOutputStream.
+
+    It is similar to FileWriteBuffer but the destination is an in-memory buffer
+   instead of a file.
+
+    Differences between MemoryBuffer and StringBuffer:
+    1. StringBuffer has Encoding but MemoryBuffer is only a byte buffer.
+    2. StringBuffer::GetString() returns a null-terminated string.
+   MemoryBuffer::GetBuffer() returns a buffer without terminator.
+
+    \tparam Allocator type for allocating memory buffer.
+    \note implements Stream concept
+*/
+template <typename Allocator = CrtAllocator>
+struct GenericMemoryBuffer {
+  typedef char Ch;  // byte
+
+  GenericMemoryBuffer(Allocator *allocator = 0,
+                      size_t capacity = kDefaultCapacity)
+      : stack_(allocator, capacity) {}
+
+  void Put(Ch c) { *stack_.template Push<Ch>() = c; }
+  void Flush() {}
+
+  void Clear() { stack_.Clear(); }
+  void ShrinkToFit() { stack_.ShrinkToFit(); }
+  Ch *Push(size_t count) { return stack_.template Push<Ch>(count); }
+  void Pop(size_t count) { stack_.template Pop<Ch>(count); }
+
+  const Ch *GetBuffer() const { return stack_.template Bottom<Ch>(); }
+
+  size_t GetSize() const { return stack_.GetSize(); }
+
+  static const size_t kDefaultCapacity = 256;
+  mutable internal::Stack<Allocator> stack_;
+};
+
+typedef GenericMemoryBuffer<> MemoryBuffer;
+
+//! Implement specialized version of PutN() with memset() for better
+//! performance.
+template <>
+inline void PutN(MemoryBuffer &memoryBuffer, char c, size_t n) {
+  std::memset(memoryBuffer.stack_.Push<char>(n), c, n * sizeof(c));
+}
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_MEMORYBUFFER_H_

livox/common/rapidjson/memorystream.h

@@ -0,0 +1,84 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_MEMORYSTREAM_H_
+#define RAPIDJSON_MEMORYSTREAM_H_
+
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(unreachable - code)
+RAPIDJSON_DIAG_OFF(missing - noreturn)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Represents an in-memory input byte stream.
+/*!
+    This class is mainly for being wrapped by EncodedInputStream or
+   AutoUTFInputStream.
+
+    It is similar to FileReadBuffer but the source is an in-memory buffer
+   instead of a file.
+
+    Differences between MemoryStream and StringStream:
+    1. StringStream has encoding but MemoryStream is a byte stream.
+    2. MemoryStream needs size of the source buffer and the buffer don't need to
+   be null terminated. StringStream assume null-terminated string as source.
+    3. MemoryStream supports Peek4() for encoding detection. StringStream is
+   specified with an encoding so it should not have Peek4(). \note implements
+   Stream concept
+*/
+struct MemoryStream {
+  typedef char Ch;  // byte
+
+  MemoryStream(const Ch *src, size_t size)
+      : src_(src), begin_(src), end_(src + size), size_(size) {}
+
+  Ch Peek() const { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_; }
+  Ch Take() { return RAPIDJSON_UNLIKELY(src_ == end_) ? '\0' : *src_++; }
+  size_t Tell() const { return static_cast<size_t>(src_ - begin_); }
+
+  Ch *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  void Put(Ch) { RAPIDJSON_ASSERT(false); }
+  void Flush() { RAPIDJSON_ASSERT(false); }
+  size_t PutEnd(Ch *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+  // For encoding detection only.
+  const Ch *Peek4() const { return Tell() + 4 <= size_ ? src_ : 0; }
+
+  const Ch *src_;    //!< Current read position.
+  const Ch *begin_;  //!< Original head of the string.
+  const Ch *end_;    //!< End of stream.
+  size_t size_;      //!< Size of the stream.
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_MEMORYBUFFER_H_

livox/common/rapidjson/msinttypes/inttypes.h

@@ -0,0 +1,316 @@
+// ISO C9x  compliant inttypes.h for Microsoft Visual Studio
+// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
+//
+//  Copyright (c) 2006-2013 Alexander Chemeris
+//
+// 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 product 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 AUTHOR ``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 AUTHOR 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.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// The above software in this distribution may have been modified by
+// THL A29 Limited ("Tencent Modifications").
+// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
+
+#ifndef _MSC_VER  // [
+#error "Use this header only with Microsoft Visual C++ compilers!"
+#endif  // _MSC_VER ]
+
+#ifndef _MSC_INTTYPES_H_  // [
+#define _MSC_INTTYPES_H_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+#include "stdint.h"
+
+// miloyip: VC supports inttypes.h since VC2013
+#if _MSC_VER >= 1800
+#include <inttypes.h>
+#else
+
+// 7.8 Format conversion of integer types
+
+typedef struct {
+  intmax_t quot;
+  intmax_t rem;
+} imaxdiv_t;
+
+// 7.8.1 Macros for format specifiers
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_FORMAT_MACROS)  // [   See footnote 185 at page 198
+
+// The fprintf macros for signed integers are:
+#define PRId8 "d"
+#define PRIi8 "i"
+#define PRIdLEAST8 "d"
+#define PRIiLEAST8 "i"
+#define PRIdFAST8 "d"
+#define PRIiFAST8 "i"
+
+#define PRId16 "hd"
+#define PRIi16 "hi"
+#define PRIdLEAST16 "hd"
+#define PRIiLEAST16 "hi"
+#define PRIdFAST16 "hd"
+#define PRIiFAST16 "hi"
+
+#define PRId32 "I32d"
+#define PRIi32 "I32i"
+#define PRIdLEAST32 "I32d"
+#define PRIiLEAST32 "I32i"
+#define PRIdFAST32 "I32d"
+#define PRIiFAST32 "I32i"
+
+#define PRId64 "I64d"
+#define PRIi64 "I64i"
+#define PRIdLEAST64 "I64d"
+#define PRIiLEAST64 "I64i"
+#define PRIdFAST64 "I64d"
+#define PRIiFAST64 "I64i"
+
+#define PRIdMAX "I64d"
+#define PRIiMAX "I64i"
+
+#define PRIdPTR "Id"
+#define PRIiPTR "Ii"
+
+// The fprintf macros for unsigned integers are:
+#define PRIo8 "o"
+#define PRIu8 "u"
+#define PRIx8 "x"
+#define PRIX8 "X"
+#define PRIoLEAST8 "o"
+#define PRIuLEAST8 "u"
+#define PRIxLEAST8 "x"
+#define PRIXLEAST8 "X"
+#define PRIoFAST8 "o"
+#define PRIuFAST8 "u"
+#define PRIxFAST8 "x"
+#define PRIXFAST8 "X"
+
+#define PRIo16 "ho"
+#define PRIu16 "hu"
+#define PRIx16 "hx"
+#define PRIX16 "hX"
+#define PRIoLEAST16 "ho"
+#define PRIuLEAST16 "hu"
+#define PRIxLEAST16 "hx"
+#define PRIXLEAST16 "hX"
+#define PRIoFAST16 "ho"
+#define PRIuFAST16 "hu"
+#define PRIxFAST16 "hx"
+#define PRIXFAST16 "hX"
+
+#define PRIo32 "I32o"
+#define PRIu32 "I32u"
+#define PRIx32 "I32x"
+#define PRIX32 "I32X"
+#define PRIoLEAST32 "I32o"
+#define PRIuLEAST32 "I32u"
+#define PRIxLEAST32 "I32x"
+#define PRIXLEAST32 "I32X"
+#define PRIoFAST32 "I32o"
+#define PRIuFAST32 "I32u"
+#define PRIxFAST32 "I32x"
+#define PRIXFAST32 "I32X"
+
+#define PRIo64 "I64o"
+#define PRIu64 "I64u"
+#define PRIx64 "I64x"
+#define PRIX64 "I64X"
+#define PRIoLEAST64 "I64o"
+#define PRIuLEAST64 "I64u"
+#define PRIxLEAST64 "I64x"
+#define PRIXLEAST64 "I64X"
+#define PRIoFAST64 "I64o"
+#define PRIuFAST64 "I64u"
+#define PRIxFAST64 "I64x"
+#define PRIXFAST64 "I64X"
+
+#define PRIoMAX "I64o"
+#define PRIuMAX "I64u"
+#define PRIxMAX "I64x"
+#define PRIXMAX "I64X"
+
+#define PRIoPTR "Io"
+#define PRIuPTR "Iu"
+#define PRIxPTR "Ix"
+#define PRIXPTR "IX"
+
+// The fscanf macros for signed integers are:
+#define SCNd8 "d"
+#define SCNi8 "i"
+#define SCNdLEAST8 "d"
+#define SCNiLEAST8 "i"
+#define SCNdFAST8 "d"
+#define SCNiFAST8 "i"
+
+#define SCNd16 "hd"
+#define SCNi16 "hi"
+#define SCNdLEAST16 "hd"
+#define SCNiLEAST16 "hi"
+#define SCNdFAST16 "hd"
+#define SCNiFAST16 "hi"
+
+#define SCNd32 "ld"
+#define SCNi32 "li"
+#define SCNdLEAST32 "ld"
+#define SCNiLEAST32 "li"
+#define SCNdFAST32 "ld"
+#define SCNiFAST32 "li"
+
+#define SCNd64 "I64d"
+#define SCNi64 "I64i"
+#define SCNdLEAST64 "I64d"
+#define SCNiLEAST64 "I64i"
+#define SCNdFAST64 "I64d"
+#define SCNiFAST64 "I64i"
+
+#define SCNdMAX "I64d"
+#define SCNiMAX "I64i"
+
+#ifdef _WIN64  // [
+#define SCNdPTR "I64d"
+#define SCNiPTR "I64i"
+#else  // _WIN64 ][
+#define SCNdPTR "ld"
+#define SCNiPTR "li"
+#endif  // _WIN64 ]
+
+// The fscanf macros for unsigned integers are:
+#define SCNo8 "o"
+#define SCNu8 "u"
+#define SCNx8 "x"
+#define SCNX8 "X"
+#define SCNoLEAST8 "o"
+#define SCNuLEAST8 "u"
+#define SCNxLEAST8 "x"
+#define SCNXLEAST8 "X"
+#define SCNoFAST8 "o"
+#define SCNuFAST8 "u"
+#define SCNxFAST8 "x"
+#define SCNXFAST8 "X"
+
+#define SCNo16 "ho"
+#define SCNu16 "hu"
+#define SCNx16 "hx"
+#define SCNX16 "hX"
+#define SCNoLEAST16 "ho"
+#define SCNuLEAST16 "hu"
+#define SCNxLEAST16 "hx"
+#define SCNXLEAST16 "hX"
+#define SCNoFAST16 "ho"
+#define SCNuFAST16 "hu"
+#define SCNxFAST16 "hx"
+#define SCNXFAST16 "hX"
+
+#define SCNo32 "lo"
+#define SCNu32 "lu"
+#define SCNx32 "lx"
+#define SCNX32 "lX"
+#define SCNoLEAST32 "lo"
+#define SCNuLEAST32 "lu"
+#define SCNxLEAST32 "lx"
+#define SCNXLEAST32 "lX"
+#define SCNoFAST32 "lo"
+#define SCNuFAST32 "lu"
+#define SCNxFAST32 "lx"
+#define SCNXFAST32 "lX"
+
+#define SCNo64 "I64o"
+#define SCNu64 "I64u"
+#define SCNx64 "I64x"
+#define SCNX64 "I64X"
+#define SCNoLEAST64 "I64o"
+#define SCNuLEAST64 "I64u"
+#define SCNxLEAST64 "I64x"
+#define SCNXLEAST64 "I64X"
+#define SCNoFAST64 "I64o"
+#define SCNuFAST64 "I64u"
+#define SCNxFAST64 "I64x"
+#define SCNXFAST64 "I64X"
+
+#define SCNoMAX "I64o"
+#define SCNuMAX "I64u"
+#define SCNxMAX "I64x"
+#define SCNXMAX "I64X"
+
+#ifdef _WIN64  // [
+#define SCNoPTR "I64o"
+#define SCNuPTR "I64u"
+#define SCNxPTR "I64x"
+#define SCNXPTR "I64X"
+#else  // _WIN64 ][
+#define SCNoPTR "lo"
+#define SCNuPTR "lu"
+#define SCNxPTR "lx"
+#define SCNXPTR "lX"
+#endif  // _WIN64 ]
+
+#endif  // __STDC_FORMAT_MACROS ]
+
+// 7.8.2 Functions for greatest-width integer types
+
+// 7.8.2.1 The imaxabs function
+#define imaxabs _abs64
+
+// 7.8.2.2 The imaxdiv function
+
+// This is modified version of div() function from Microsoft's div.c found
+// in %MSVC.NET%\crt\src\div.c
+#ifdef STATIC_IMAXDIV  // [
+static
+#else                  // STATIC_IMAXDIV ][
+_inline
+#endif                 // STATIC_IMAXDIV ]
+    imaxdiv_t __cdecl imaxdiv(intmax_t numer, intmax_t denom) {
+  imaxdiv_t result;
+
+  result.quot = numer / denom;
+  result.rem = numer % denom;
+
+  if (numer < 0 && result.rem > 0) {
+    // did division wrong; must fix up
+    ++result.quot;
+    result.rem -= denom;
+  }
+
+  return result;
+}
+
+// 7.8.2.3 The strtoimax and strtoumax functions
+#define strtoimax _strtoi64
+#define strtoumax _strtoui64
+
+// 7.8.2.4 The wcstoimax and wcstoumax functions
+#define wcstoimax _wcstoi64
+#define wcstoumax _wcstoui64
+
+#endif  // _MSC_VER >= 1800
+
+#endif  // _MSC_INTTYPES_H_ ]

livox/common/rapidjson/msinttypes/stdint.h

@@ -0,0 +1,301 @@
+// ISO C9x  compliant stdint.h for Microsoft Visual Studio
+// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
+//
+//  Copyright (c) 2006-2013 Alexander Chemeris
+//
+// 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 product 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 AUTHOR ``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 AUTHOR 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.
+//
+///////////////////////////////////////////////////////////////////////////////
+
+// The above software in this distribution may have been modified by
+// THL A29 Limited ("Tencent Modifications").
+// All Tencent Modifications are Copyright (C) 2015 THL A29 Limited.
+
+#ifndef _MSC_VER  // [
+#error "Use this header only with Microsoft Visual C++ compilers!"
+#endif  // _MSC_VER ]
+
+#ifndef _MSC_STDINT_H_  // [
+#define _MSC_STDINT_H_
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+// miloyip: Originally Visual Studio 2010 uses its own stdint.h. However it
+// generates warning with INT64_C(), so change to use this file for vs2010.
+#if _MSC_VER >= 1600  // [
+#include <stdint.h>
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
+
+#undef INT8_C
+#undef INT16_C
+#undef INT32_C
+#undef INT64_C
+#undef UINT8_C
+#undef UINT16_C
+#undef UINT32_C
+#undef UINT64_C
+
+// 7.18.4.1 Macros for minimum-width integer constants
+
+#define INT8_C(val) val##i8
+#define INT16_C(val) val##i16
+#define INT32_C(val) val##i32
+#define INT64_C(val) val##i64
+
+#define UINT8_C(val) val##ui8
+#define UINT16_C(val) val##ui16
+#define UINT32_C(val) val##ui32
+#define UINT64_C(val) val##ui64
+
+// 7.18.4.2 Macros for greatest-width integer constants
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C  //   [
+#define INTMAX_C INT64_C
+#endif             // INTMAX_C    ]
+#ifndef UINTMAX_C  //  [
+#define UINTMAX_C UINT64_C
+#endif  // UINTMAX_C   ]
+
+#endif  // __STDC_CONSTANT_MACROS ]
+
+#else  // ] _MSC_VER >= 1700 [
+
+#include <limits.h>
+
+// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
+// compiling for ARM we have to wrap <wchar.h> include with 'extern "C++" {}'
+// or compiler would give many errors like this:
+//   error C2733: second C linkage of overloaded function 'wmemchr' not allowed
+#if defined(__cplusplus) && !defined(_M_ARM)
+extern "C" {
+#endif
+#include <wchar.h>
+#if defined(__cplusplus) && !defined(_M_ARM)
+}
+#endif
+
+// Define _W64 macros to mark types changing their size, like intptr_t.
+#ifndef _W64
+#if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
+#define _W64 __w64
+#else
+#define _W64
+#endif
+#endif
+
+// 7.18.1 Integer types
+
+// 7.18.1.1 Exact-width integer types
+
+// Visual Studio 6 and Embedded Visual C++ 4 doesn't
+// realize that, e.g. char has the same size as __int8
+// so we give up on __intX for them.
+#if (_MSC_VER < 1300)
+typedef signed char int8_t;
+typedef signed short int16_t;
+typedef signed int int32_t;
+typedef unsigned char uint8_t;
+typedef unsigned short uint16_t;
+typedef unsigned int uint32_t;
+#else
+typedef signed __int8 int8_t;
+typedef signed __int16 int16_t;
+typedef signed __int32 int32_t;
+typedef unsigned __int8 uint8_t;
+typedef unsigned __int16 uint16_t;
+typedef unsigned __int32 uint32_t;
+#endif
+typedef signed __int64 int64_t;
+typedef unsigned __int64 uint64_t;
+
+// 7.18.1.2 Minimum-width integer types
+typedef int8_t int_least8_t;
+typedef int16_t int_least16_t;
+typedef int32_t int_least32_t;
+typedef int64_t int_least64_t;
+typedef uint8_t uint_least8_t;
+typedef uint16_t uint_least16_t;
+typedef uint32_t uint_least32_t;
+typedef uint64_t uint_least64_t;
+
+// 7.18.1.3 Fastest minimum-width integer types
+typedef int8_t int_fast8_t;
+typedef int16_t int_fast16_t;
+typedef int32_t int_fast32_t;
+typedef int64_t int_fast64_t;
+typedef uint8_t uint_fast8_t;
+typedef uint16_t uint_fast16_t;
+typedef uint32_t uint_fast32_t;
+typedef uint64_t uint_fast64_t;
+
+// 7.18.1.4 Integer types capable of holding object pointers
+#ifdef _WIN64  // [
+typedef signed __int64 intptr_t;
+typedef unsigned __int64 uintptr_t;
+#else          // _WIN64 ][
+typedef _W64 signed int intptr_t;
+typedef _W64 unsigned int uintptr_t;
+#endif  // _WIN64 ]
+
+// 7.18.1.5 Greatest-width integer types
+typedef int64_t intmax_t;
+typedef uint64_t uintmax_t;
+
+// 7.18.2 Limits of specified-width integer types
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_LIMIT_MACROS)  // [   See footnote 220 at page 257 and
+// footnote 221 at page 259
+
+// 7.18.2.1 Limits of exact-width integer types
+#define INT8_MIN ((int8_t)_I8_MIN)
+#define INT8_MAX _I8_MAX
+#define INT16_MIN ((int16_t)_I16_MIN)
+#define INT16_MAX _I16_MAX
+#define INT32_MIN ((int32_t)_I32_MIN)
+#define INT32_MAX _I32_MAX
+#define INT64_MIN ((int64_t)_I64_MIN)
+#define INT64_MAX _I64_MAX
+#define UINT8_MAX _UI8_MAX
+#define UINT16_MAX _UI16_MAX
+#define UINT32_MAX _UI32_MAX
+#define UINT64_MAX _UI64_MAX
+
+// 7.18.2.2 Limits of minimum-width integer types
+#define INT_LEAST8_MIN INT8_MIN
+#define INT_LEAST8_MAX INT8_MAX
+#define INT_LEAST16_MIN INT16_MIN
+#define INT_LEAST16_MAX INT16_MAX
+#define INT_LEAST32_MIN INT32_MIN
+#define INT_LEAST32_MAX INT32_MAX
+#define INT_LEAST64_MIN INT64_MIN
+#define INT_LEAST64_MAX INT64_MAX
+#define UINT_LEAST8_MAX UINT8_MAX
+#define UINT_LEAST16_MAX UINT16_MAX
+#define UINT_LEAST32_MAX UINT32_MAX
+#define UINT_LEAST64_MAX UINT64_MAX
+
+// 7.18.2.3 Limits of fastest minimum-width integer types
+#define INT_FAST8_MIN INT8_MIN
+#define INT_FAST8_MAX INT8_MAX
+#define INT_FAST16_MIN INT16_MIN
+#define INT_FAST16_MAX INT16_MAX
+#define INT_FAST32_MIN INT32_MIN
+#define INT_FAST32_MAX INT32_MAX
+#define INT_FAST64_MIN INT64_MIN
+#define INT_FAST64_MAX INT64_MAX
+#define UINT_FAST8_MAX UINT8_MAX
+#define UINT_FAST16_MAX UINT16_MAX
+#define UINT_FAST32_MAX UINT32_MAX
+#define UINT_FAST64_MAX UINT64_MAX
+
+// 7.18.2.4 Limits of integer types capable of holding object pointers
+#ifdef _WIN64  // [
+#define INTPTR_MIN INT64_MIN
+#define INTPTR_MAX INT64_MAX
+#define UINTPTR_MAX UINT64_MAX
+#else  // _WIN64 ][
+#define INTPTR_MIN INT32_MIN
+#define INTPTR_MAX INT32_MAX
+#define UINTPTR_MAX UINT32_MAX
+#endif  // _WIN64 ]
+
+// 7.18.2.5 Limits of greatest-width integer types
+#define INTMAX_MIN INT64_MIN
+#define INTMAX_MAX INT64_MAX
+#define UINTMAX_MAX UINT64_MAX
+
+// 7.18.3 Limits of other integer types
+
+#ifdef _WIN64  // [
+#define PTRDIFF_MIN _I64_MIN
+#define PTRDIFF_MAX _I64_MAX
+#else  // _WIN64 ][
+#define PTRDIFF_MIN _I32_MIN
+#define PTRDIFF_MAX _I32_MAX
+#endif  // _WIN64 ]
+
+#define SIG_ATOMIC_MIN INT_MIN
+#define SIG_ATOMIC_MAX INT_MAX
+
+#ifndef SIZE_MAX  // [
+#ifdef _WIN64     // [
+#define SIZE_MAX _UI64_MAX
+#else  // _WIN64 ][
+#define SIZE_MAX _UI32_MAX
+#endif  // _WIN64 ]
+#endif  // SIZE_MAX ]
+
+// WCHAR_MIN and WCHAR_MAX are also defined in <wchar.h>
+#ifndef WCHAR_MIN  // [
+#define WCHAR_MIN 0
+#endif             // WCHAR_MIN ]
+#ifndef WCHAR_MAX  // [
+#define WCHAR_MAX _UI16_MAX
+#endif  // WCHAR_MAX ]
+
+#define WINT_MIN 0
+#define WINT_MAX _UI16_MAX
+
+#endif  // __STDC_LIMIT_MACROS ]
+
+// 7.18.4 Limits of other integer types
+
+#if !defined(__cplusplus) || \
+    defined(__STDC_CONSTANT_MACROS)  // [   See footnote 224 at page 260
+
+// 7.18.4.1 Macros for minimum-width integer constants
+
+#define INT8_C(val) val##i8
+#define INT16_C(val) val##i16
+#define INT32_C(val) val##i32
+#define INT64_C(val) val##i64
+
+#define UINT8_C(val) val##ui8
+#define UINT16_C(val) val##ui16
+#define UINT32_C(val) val##ui32
+#define UINT64_C(val) val##ui64
+
+// 7.18.4.2 Macros for greatest-width integer constants
+// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
+// Check out Issue 9 for the details.
+#ifndef INTMAX_C  //   [
+#define INTMAX_C INT64_C
+#endif             // INTMAX_C    ]
+#ifndef UINTMAX_C  //  [
+#define UINTMAX_C UINT64_C
+#endif  // UINTMAX_C   ]
+
+#endif  // __STDC_CONSTANT_MACROS ]
+
+#endif  // _MSC_VER >= 1600 ]
+
+#endif  // _MSC_STDINT_H_ ]

livox/common/rapidjson/ostreamwrapper.h

@@ -0,0 +1,96 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_OSTREAMWRAPPER_H_
+#define RAPIDJSON_OSTREAMWRAPPER_H_
+
+#include <iosfwd>
+#include "stream.h"
+
+#ifdef __clang__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(padded)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Wrapper of \c std::basic_ostream into RapidJSON's Stream concept.
+/*!
+    The classes can be wrapped including but not limited to:
+
+    - \c std::ostringstream
+    - \c std::stringstream
+    - \c std::wpstringstream
+    - \c std::wstringstream
+    - \c std::ifstream
+    - \c std::fstream
+    - \c std::wofstream
+    - \c std::wfstream
+
+    \tparam StreamType Class derived from \c std::basic_ostream.
+*/
+
+template <typename StreamType>
+class BasicOStreamWrapper {
+ public:
+  typedef typename StreamType::char_type Ch;
+  BasicOStreamWrapper(StreamType &stream) : stream_(stream) {}
+
+  void Put(Ch c) { stream_.put(c); }
+
+  void Flush() { stream_.flush(); }
+
+  // Not implemented
+  char Peek() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char Take() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t Tell() const {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  char *PutBegin() {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+  size_t PutEnd(char *) {
+    RAPIDJSON_ASSERT(false);
+    return 0;
+  }
+
+ private:
+  BasicOStreamWrapper(const BasicOStreamWrapper &);
+  BasicOStreamWrapper &operator=(const BasicOStreamWrapper &);
+
+  StreamType &stream_;
+};
+
+typedef BasicOStreamWrapper<std::ostream> OStreamWrapper;
+typedef BasicOStreamWrapper<std::wostream> WOStreamWrapper;
+
+#ifdef __clang__
+RAPIDJSON_DIAG_POP
+#endif
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_OSTREAMWRAPPER_H_

livox/common/rapidjson/prettywriter.h

@@ -0,0 +1,333 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_PRETTYWRITER_H_
+#define RAPIDJSON_PRETTYWRITER_H_
+
+#include "writer.h"
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(effc++)
+#endif
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_PUSH
+RAPIDJSON_DIAG_OFF(c++ 98 - compat)
+#endif
+
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Combination of PrettyWriter format flags.
+/*! \see PrettyWriter::SetFormatOptions
+ */
+enum PrettyFormatOptions {
+  kFormatDefault = 0,         //!< Default pretty formatting.
+  kFormatSingleLineArray = 1  //!< Format arrays on a single line.
+};
+
+//! Writer with indentation and spacing.
+/*!
+    \tparam OutputStream Type of output os.
+    \tparam SourceEncoding Encoding of source string.
+    \tparam TargetEncoding Encoding of output stream.
+    \tparam StackAllocator Type of allocator for allocating memory of stack.
+*/
+template <typename OutputStream, typename SourceEncoding = UTF8<>,
+          typename TargetEncoding = UTF8<>,
+          typename StackAllocator = CrtAllocator,
+          unsigned writeFlags = kWriteDefaultFlags>
+class PrettyWriter : public Writer<OutputStream, SourceEncoding, TargetEncoding,
+                                   StackAllocator, writeFlags> {
+ public:
+  typedef Writer<OutputStream, SourceEncoding, TargetEncoding, StackAllocator,
+                 writeFlags>
+      Base;
+  typedef typename Base::Ch Ch;
+
+  //! Constructor
+  /*! \param os Output stream.
+      \param allocator User supplied allocator. If it is null, it will create a
+     private one. \param levelDepth Initial capacity of stack.
+  */
+  explicit PrettyWriter(OutputStream &os, StackAllocator *allocator = 0,
+                        size_t levelDepth = Base::kDefaultLevelDepth)
+      : Base(os, allocator, levelDepth),
+        indentChar_(' '),
+        indentCharCount_(4),
+        formatOptions_(kFormatDefault) {}
+
+  explicit PrettyWriter(StackAllocator *allocator = 0,
+                        size_t levelDepth = Base::kDefaultLevelDepth)
+      : Base(allocator, levelDepth), indentChar_(' '), indentCharCount_(4) {}
+
+#if RAPIDJSON_HAS_CXX11_RVALUE_REFS
+  PrettyWriter(PrettyWriter &&rhs)
+      : Base(std::forward<PrettyWriter>(rhs)),
+        indentChar_(rhs.indentChar_),
+        indentCharCount_(rhs.indentCharCount_),
+        formatOptions_(rhs.formatOptions_) {}
+#endif
+
+  //! Set custom indentation.
+  /*! \param indentChar       Character for indentation. Must be whitespace
+     character (' ', '\\t', '\\n', '\\r'). \param indentCharCount  Number of
+     indent characters for each indentation level. \note The default indentation
+     is 4 spaces.
+  */
+  PrettyWriter &SetIndent(Ch indentChar, unsigned indentCharCount) {
+    RAPIDJSON_ASSERT(indentChar == ' ' || indentChar == '\t' ||
+                     indentChar == '\n' || indentChar == '\r');
+    indentChar_ = indentChar;
+    indentCharCount_ = indentCharCount;
+    return *this;
+  }
+
+  //! Set pretty writer formatting options.
+  /*! \param options Formatting options.
+   */
+  PrettyWriter &SetFormatOptions(PrettyFormatOptions options) {
+    formatOptions_ = options;
+    return *this;
+  }
+
+  /*! @name Implementation of Handler
+      \see Handler
+  */
+  //@{
+
+  bool Null() {
+    PrettyPrefix(kNullType);
+    return Base::EndValue(Base::WriteNull());
+  }
+  bool Bool(bool b) {
+    PrettyPrefix(b ? kTrueType : kFalseType);
+    return Base::EndValue(Base::WriteBool(b));
+  }
+  bool Int(int i) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteInt(i));
+  }
+  bool Uint(unsigned u) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteUint(u));
+  }
+  bool Int64(int64_t i64) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteInt64(i64));
+  }
+  bool Uint64(uint64_t u64) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteUint64(u64));
+  }
+  bool Double(double d) {
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteDouble(d));
+  }
+
+  bool RawNumber(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    PrettyPrefix(kNumberType);
+    return Base::EndValue(Base::WriteString(str, length));
+  }
+
+  bool String(const Ch *str, SizeType length, bool copy = false) {
+    RAPIDJSON_ASSERT(str != 0);
+    (void)copy;
+    PrettyPrefix(kStringType);
+    return Base::EndValue(Base::WriteString(str, length));
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool String(const std::basic_string<Ch> &str) {
+    return String(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool StartObject() {
+    PrettyPrefix(kObjectType);
+    new (Base::level_stack_.template Push<typename Base::Level>())
+        typename Base::Level(false);
+    return Base::WriteStartObject();
+  }
+
+  bool Key(const Ch *str, SizeType length, bool copy = false) {
+    return String(str, length, copy);
+  }
+
+#if RAPIDJSON_HAS_STDSTRING
+  bool Key(const std::basic_string<Ch> &str) {
+    return Key(str.data(), SizeType(str.size()));
+  }
+#endif
+
+  bool EndObject(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
+                     sizeof(typename Base::Level));  // not inside an Object
+    RAPIDJSON_ASSERT(!Base::level_stack_.template Top<typename Base::Level>()
+                          ->inArray);  // currently inside an Array, not Object
+    RAPIDJSON_ASSERT(
+        0 ==
+        Base::level_stack_.template Top<typename Base::Level>()->valueCount %
+            2);  // Object has a Key without a Value
+
+    bool empty =
+        Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
+        0;
+
+    if (!empty) {
+      Base::os_->Put('\n');
+      WriteIndent();
+    }
+    bool ret = Base::EndValue(Base::WriteEndObject());
+    (void)ret;
+    RAPIDJSON_ASSERT(ret == true);
+    if (Base::level_stack_.Empty())  // end of json text
+      Base::Flush();
+    return true;
+  }
+
+  bool StartArray() {
+    PrettyPrefix(kArrayType);
+    new (Base::level_stack_.template Push<typename Base::Level>())
+        typename Base::Level(true);
+    return Base::WriteStartArray();
+  }
+
+  bool EndArray(SizeType memberCount = 0) {
+    (void)memberCount;
+    RAPIDJSON_ASSERT(Base::level_stack_.GetSize() >=
+                     sizeof(typename Base::Level));
+    RAPIDJSON_ASSERT(
+        Base::level_stack_.template Top<typename Base::Level>()->inArray);
+    bool empty =
+        Base::level_stack_.template Pop<typename Base::Level>(1)->valueCount ==
+        0;
+
+    if (!empty && !(formatOptions_ & kFormatSingleLineArray)) {
+      Base::os_->Put('\n');
+      WriteIndent();
+    }
+    bool ret = Base::EndValue(Base::WriteEndArray());
+    (void)ret;
+    RAPIDJSON_ASSERT(ret == true);
+    if (Base::level_stack_.Empty())  // end of json text
+      Base::Flush();
+    return true;
+  }
+
+  //@}
+
+  /*! @name Convenience extensions */
+  //@{
+
+  //! Simpler but slower overload.
+  bool String(const Ch *str) { return String(str, internal::StrLen(str)); }
+  bool Key(const Ch *str) { return Key(str, internal::StrLen(str)); }
+
+  //@}
+
+  //! Write a raw JSON value.
+  /*!
+      For user to write a stringified JSON as a value.
+
+      \param json A well-formed JSON value. It should not contain null character
+     within [0, length - 1] range. \param length Length of the json. \param type
+     Type of the root of json. \note When using PrettyWriter::RawValue(), the
+     result json may not be indented correctly.
+  */
+  bool RawValue(const Ch *json, size_t length, Type type) {
+    RAPIDJSON_ASSERT(json != 0);
+    PrettyPrefix(type);
+    return Base::EndValue(Base::WriteRawValue(json, length));
+  }
+
+ protected:
+  void PrettyPrefix(Type type) {
+    (void)type;
+    if (Base::level_stack_.GetSize() != 0) {  // this value is not at root
+      typename Base::Level *level =
+          Base::level_stack_.template Top<typename Base::Level>();
+
+      if (level->inArray) {
+        if (level->valueCount > 0) {
+          Base::os_->Put(
+              ',');  // add comma if it is not the first element in array
+          if (formatOptions_ & kFormatSingleLineArray) Base::os_->Put(' ');
+        }
+
+        if (!(formatOptions_ & kFormatSingleLineArray)) {
+          Base::os_->Put('\n');
+          WriteIndent();
+        }
+      } else {  // in object
+        if (level->valueCount > 0) {
+          if (level->valueCount % 2 == 0) {
+            Base::os_->Put(',');
+            Base::os_->Put('\n');
+          } else {
+            Base::os_->Put(':');
+            Base::os_->Put(' ');
+          }
+        } else
+          Base::os_->Put('\n');
+
+        if (level->valueCount % 2 == 0) WriteIndent();
+      }
+      if (!level->inArray && level->valueCount % 2 == 0)
+        RAPIDJSON_ASSERT(type == kStringType);  // if it's in object, then even
+                                                // number should be a name
+      level->valueCount++;
+    } else {
+      RAPIDJSON_ASSERT(
+          !Base::hasRoot_);  // Should only has one and only one root.
+      Base::hasRoot_ = true;
+    }
+  }
+
+  void WriteIndent() {
+    size_t count =
+        (Base::level_stack_.GetSize() / sizeof(typename Base::Level)) *
+        indentCharCount_;
+    PutN(*Base::os_, static_cast<typename OutputStream::Ch>(indentChar_),
+         count);
+  }
+
+  Ch indentChar_;
+  unsigned indentCharCount_;
+  PrettyFormatOptions formatOptions_;
+
+ private:
+  // Prohibit copy constructor & assignment operator.
+  PrettyWriter(const PrettyWriter &);
+  PrettyWriter &operator=(const PrettyWriter &);
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__clang__)
+RAPIDJSON_DIAG_POP
+#endif
+
+#ifdef __GNUC__
+RAPIDJSON_DIAG_POP
+#endif
+
+#endif  // RAPIDJSON_RAPIDJSON_H_

livox/common/rapidjson/rapidjson.h

@@ -0,0 +1,719 @@
+// Tencent is pleased to support the open source community by making RapidJSON
+// available.
+//
+// Copyright (C) 2015 THL A29 Limited, a Tencent company, and Milo Yip. All
+// rights reserved.
+//
+// Licensed under the MIT License (the "License"); you may not use this file
+// except in compliance with the License. You may obtain a copy of the License
+// at
+//
+// http://opensource.org/licenses/MIT
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+// WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+// License for the specific language governing permissions and limitations under
+// the License.
+
+#ifndef RAPIDJSON_RAPIDJSON_H_
+#define RAPIDJSON_RAPIDJSON_H_
+
+/*!\file rapidjson.h
+    \brief common definitions and configuration
+
+    \see RAPIDJSON_CONFIG
+ */
+
+/*! \defgroup RAPIDJSON_CONFIG RapidJSON configuration
+    \brief Configuration macros for library features
+
+    Some RapidJSON features are configurable to adapt the library to a wide
+    variety of platforms, environments and usage scenarios.  Most of the
+    features can be configured in terms of overridden or predefined
+    preprocessor macros at compile-time.
+
+    Some additional customization is available in the \ref RAPIDJSON_ERRORS
+   APIs.
+
+    \note These macros should be given on the compiler command-line
+          (where applicable)  to avoid inconsistent values when compiling
+          different translation units of a single application.
+ */
+
+#include <cstdlib>  // malloc(), realloc(), free(), size_t
+#include <cstring>  // memset(), memcpy(), memmove(), memcmp()
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_VERSION_STRING
+//
+// ALWAYS synchronize the following 3 macros with corresponding variables in
+// /CMakeLists.txt.
+//
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+// token stringification
+#define RAPIDJSON_STRINGIFY(x) RAPIDJSON_DO_STRINGIFY(x)
+#define RAPIDJSON_DO_STRINGIFY(x) #x
+
+// token concatenation
+#define RAPIDJSON_JOIN(X, Y) RAPIDJSON_DO_JOIN(X, Y)
+#define RAPIDJSON_DO_JOIN(X, Y) RAPIDJSON_DO_JOIN2(X, Y)
+#define RAPIDJSON_DO_JOIN2(X, Y) X##Y
+//!@endcond
+
+/*! \def RAPIDJSON_MAJOR_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Major version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_MINOR_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Minor version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_PATCH_VERSION
+    \ingroup RAPIDJSON_CONFIG
+    \brief Patch version of RapidJSON in integer.
+*/
+/*! \def RAPIDJSON_VERSION_STRING
+    \ingroup RAPIDJSON_CONFIG
+    \brief Version of RapidJSON in "<major>.<minor>.<patch>" string format.
+*/
+#define RAPIDJSON_MAJOR_VERSION 1
+#define RAPIDJSON_MINOR_VERSION 1
+#define RAPIDJSON_PATCH_VERSION 0
+#define RAPIDJSON_VERSION_STRING \
+  RAPIDJSON_STRINGIFY(           \
+      RAPIDJSON_MAJOR_VERSION.RAPIDJSON_MINOR_VERSION.RAPIDJSON_PATCH_VERSION)
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NAMESPACE_(BEGIN|END)
+/*! \def RAPIDJSON_NAMESPACE
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace
+
+    In order to avoid symbol clashes and/or "One Definition Rule" errors
+    between multiple inclusions of (different versions of) RapidJSON in
+    a single binary, users can customize the name of the main RapidJSON
+    namespace.
+
+    In case of a single nesting level, defining \c RAPIDJSON_NAMESPACE
+    to a custom name (e.g. \c MyRapidJSON) is sufficient.  If multiple
+    levels are needed, both \ref RAPIDJSON_NAMESPACE_BEGIN and \ref
+    RAPIDJSON_NAMESPACE_END need to be defined as well:
+
+    \code
+    // in some .cpp file
+    #define RAPIDJSON_NAMESPACE my::rapidjson
+    #define RAPIDJSON_NAMESPACE_BEGIN namespace my { namespace rapidjson {
+    #define RAPIDJSON_NAMESPACE_END   } }
+    #include "rapidjson/..."
+    \endcode
+
+    \see rapidjson
+ */
+/*! \def RAPIDJSON_NAMESPACE_BEGIN
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace (opening expression)
+    \see RAPIDJSON_NAMESPACE
+*/
+/*! \def RAPIDJSON_NAMESPACE_END
+    \ingroup RAPIDJSON_CONFIG
+    \brief   provide custom rapidjson namespace (closing expression)
+    \see RAPIDJSON_NAMESPACE
+*/
+#ifndef RAPIDJSON_NAMESPACE
+#define RAPIDJSON_NAMESPACE rapidjson
+#endif
+#ifndef RAPIDJSON_NAMESPACE_BEGIN
+#define RAPIDJSON_NAMESPACE_BEGIN namespace RAPIDJSON_NAMESPACE {
+#endif
+#ifndef RAPIDJSON_NAMESPACE_END
+#define RAPIDJSON_NAMESPACE_END }
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_HAS_STDSTRING
+
+#ifndef RAPIDJSON_HAS_STDSTRING
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_HAS_STDSTRING 1  // force generation of documentation
+#else
+#define RAPIDJSON_HAS_STDSTRING 0  // no std::string support by default
+#endif
+/*! \def RAPIDJSON_HAS_STDSTRING
+    \ingroup RAPIDJSON_CONFIG
+    \brief Enable RapidJSON support for \c std::string
+
+    By defining this preprocessor symbol to \c 1, several convenience functions
+   for using \ref rapidjson::GenericValue with \c std::string are enabled,
+   especially for construction and comparison.
+
+    \hideinitializer
+*/
+#endif  // !defined(RAPIDJSON_HAS_STDSTRING)
+
+#if RAPIDJSON_HAS_STDSTRING
+#include <string>
+#endif  // RAPIDJSON_HAS_STDSTRING
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NO_INT64DEFINE
+
+/*! \def RAPIDJSON_NO_INT64DEFINE
+    \ingroup RAPIDJSON_CONFIG
+    \brief Use external 64-bit integer types.
+
+    RapidJSON requires the 64-bit integer types \c int64_t and  \c uint64_t
+   types to be available at global scope.
+
+    If users have their own definition, define RAPIDJSON_NO_INT64DEFINE to
+    prevent RapidJSON from defining its own types.
+*/
+#ifndef RAPIDJSON_NO_INT64DEFINE
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#if defined(_MSC_VER) && (_MSC_VER < 1800)  // Visual Studio 2013
+#include "msinttypes/inttypes.h"
+#include "msinttypes/stdint.h"
+#else
+// Other compilers should have this.
+#include <inttypes.h>
+#include <stdint.h>
+#endif
+//!@endcond
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_NO_INT64DEFINE
+#endif
+#endif  // RAPIDJSON_NO_INT64TYPEDEF
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_FORCEINLINE
+
+#ifndef RAPIDJSON_FORCEINLINE
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#if defined(_MSC_VER) && defined(NDEBUG)
+#define RAPIDJSON_FORCEINLINE __forceinline
+#elif defined(__GNUC__) && __GNUC__ >= 4 && defined(NDEBUG)
+#define RAPIDJSON_FORCEINLINE __attribute__((always_inline))
+#else
+#define RAPIDJSON_FORCEINLINE
+#endif
+//!@endcond
+#endif  // RAPIDJSON_FORCEINLINE
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ENDIAN
+#define RAPIDJSON_LITTLEENDIAN 0  //!< Little endian machine
+#define RAPIDJSON_BIGENDIAN 1     //!< Big endian machine
+
+//! Endianness of the machine.
+/*!
+    \def RAPIDJSON_ENDIAN
+    \ingroup RAPIDJSON_CONFIG
+
+    GCC 4.6 provided macro for detecting endianness of the target machine. But
+   other compilers may not have this. User can define RAPIDJSON_ENDIAN to either
+    \ref RAPIDJSON_LITTLEENDIAN or \ref RAPIDJSON_BIGENDIAN.
+
+    Default detection implemented with reference to
+    \li
+   https://gcc.gnu.org/onlinedocs/gcc-4.6.0/cpp/Common-Predefined-Macros.html
+    \li http://www.boost.org/doc/libs/1_42_0/boost/detail/endian.hpp
+*/
+#ifndef RAPIDJSON_ENDIAN
+// Detect with GCC 4.6's macro
+#ifdef __BYTE_ORDER__
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#else
+#      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif  // __BYTE_ORDER__
+// Detect with GLIBC's endian.h
+#elif defined(__GLIBC__)
+#include <endian.h>
+#if (__BYTE_ORDER == __LITTLE_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif (__BYTE_ORDER == __BIG_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#else
+#      error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif  // __GLIBC__
+// Detect with _LITTLE_ENDIAN and _BIG_ENDIAN macro
+#elif defined(_LITTLE_ENDIAN) && !defined(_BIG_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(_BIG_ENDIAN) && !defined(_LITTLE_ENDIAN)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+// Detect with architecture macros
+#elif defined(__sparc) || defined(__sparc__) || defined(_POWER) || \
+    defined(__powerpc__) || defined(__ppc__) || defined(__hpux) || \
+    defined(__hppa) || defined(_MIPSEB) || defined(_POWER) ||      \
+    defined(__s390__)
+#define RAPIDJSON_ENDIAN RAPIDJSON_BIGENDIAN
+#elif defined(__i386__) || defined(__alpha__) || defined(__ia64) ||  \
+    defined(__ia64__) || defined(_M_IX86) || defined(_M_IA64) ||     \
+    defined(_M_ALPHA) || defined(__amd64) || defined(__amd64__) ||   \
+    defined(_M_AMD64) || defined(__x86_64) || defined(__x86_64__) || \
+    defined(_M_X64) || defined(__bfin__)
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(_MSC_VER) && (defined(_M_ARM) || defined(_M_ARM64))
+#define RAPIDJSON_ENDIAN RAPIDJSON_LITTLEENDIAN
+#elif defined(RAPIDJSON_DOXYGEN_RUNNING)
+#define RAPIDJSON_ENDIAN
+#else
+#    error Unknown machine endianness detected. User needs to define RAPIDJSON_ENDIAN.
+#endif
+#endif  // RAPIDJSON_ENDIAN
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_64BIT
+
+//! Whether using 64-bit architecture
+#ifndef RAPIDJSON_64BIT
+#if defined(__LP64__) || (defined(__x86_64__) && defined(__ILP32__)) || \
+    defined(_WIN64) || defined(__EMSCRIPTEN__)
+#define RAPIDJSON_64BIT 1
+#else
+#define RAPIDJSON_64BIT 0
+#endif
+#endif  // RAPIDJSON_64BIT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ALIGN
+
+//! Data alignment of the machine.
+/*! \ingroup RAPIDJSON_CONFIG
+    \param x pointer to align
+
+    Some machines require strict data alignment. The default is 8 bytes.
+    User can customize by defining the RAPIDJSON_ALIGN function macro.
+*/
+#ifndef RAPIDJSON_ALIGN
+#define RAPIDJSON_ALIGN(x) \
+  (((x) + static_cast<size_t>(7u)) & ~static_cast<size_t>(7u))
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_UINT64_C2
+
+//! Construct a 64-bit literal by a pair of 32-bit integer.
+/*!
+    64-bit literal with or without ULL suffix is prone to compiler warnings.
+    UINT64_C() is C macro which cause compilation problems.
+    Use this macro to define 64-bit constants by a pair of 32-bit integer.
+*/
+#ifndef RAPIDJSON_UINT64_C2
+#define RAPIDJSON_UINT64_C2(high32, low32) \
+  ((static_cast<uint64_t>(high32) << 32) | static_cast<uint64_t>(low32))
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+//! Use only lower 48-bit address for some pointers.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+
+    This optimization uses the fact that current X86-64 architecture only
+   implement lower 48-bit virtual address. The higher 16-bit can be used for
+   storing other data. \c GenericValue uses this optimization to reduce its size
+   form 24 bytes to 16 bytes in 64-bit architecture.
+*/
+#ifndef RAPIDJSON_48BITPOINTER_OPTIMIZATION
+#if defined(__amd64__) || defined(__amd64) || defined(__x86_64__) || \
+    defined(__x86_64) || defined(_M_X64) || defined(_M_AMD64)
+#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 1
+#else
+#define RAPIDJSON_48BITPOINTER_OPTIMIZATION 0
+#endif
+#endif  // RAPIDJSON_48BITPOINTER_OPTIMIZATION
+
+#if RAPIDJSON_48BITPOINTER_OPTIMIZATION == 1
+#if RAPIDJSON_64BIT != 1
+#error RAPIDJSON_48BITPOINTER_OPTIMIZATION can only be set to 1 when RAPIDJSON_64BIT=1
+#endif
+#define RAPIDJSON_SETPOINTER(type, p, x)                                       \
+  (p = reinterpret_cast<type *>(                                               \
+       (reinterpret_cast<uintptr_t>(p) &                                       \
+        static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0xFFFF0000, 0x00000000))) | \
+       reinterpret_cast<uintptr_t>(reinterpret_cast<const void *>(x))))
+#define RAPIDJSON_GETPOINTER(type, p)  \
+  (reinterpret_cast<type *>(           \
+      reinterpret_cast<uintptr_t>(p) & \
+      static_cast<uintptr_t>(RAPIDJSON_UINT64_C2(0x0000FFFF, 0xFFFFFFFF))))
+#else
+#define RAPIDJSON_SETPOINTER(type, p, x) (p = (x))
+#define RAPIDJSON_GETPOINTER(type, p) (p)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_SSE2/RAPIDJSON_SSE42/RAPIDJSON_NEON/RAPIDJSON_SIMD
+
+/*! \def RAPIDJSON_SIMD
+    \ingroup RAPIDJSON_CONFIG
+    \brief Enable SSE2/SSE4.2/Neon optimization.
+
+    RapidJSON supports optimized implementations for some parsing operations
+    based on the SSE2, SSE4.2 or NEon SIMD extensions on modern Intel
+    or ARM compatible processors.
+
+    To enable these optimizations, three different symbols can be defined;
+    \code
+    // Enable SSE2 optimization.
+    #define RAPIDJSON_SSE2
+
+    // Enable SSE4.2 optimization.
+    #define RAPIDJSON_SSE42
+    \endcode
+
+    // Enable ARM Neon optimization.
+    #define RAPIDJSON_NEON
+    \endcode
+
+    \c RAPIDJSON_SSE42 takes precedence over SSE2, if both are defined.
+
+    If any of these symbols is defined, RapidJSON defines the macro
+    \c RAPIDJSON_SIMD to indicate the availability of the optimized code.
+*/
+#if defined(RAPIDJSON_SSE2) || defined(RAPIDJSON_SSE42) || \
+    defined(RAPIDJSON_NEON) || defined(RAPIDJSON_DOXYGEN_RUNNING)
+#define RAPIDJSON_SIMD
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NO_SIZETYPEDEFINE
+
+#ifndef RAPIDJSON_NO_SIZETYPEDEFINE
+/*! \def RAPIDJSON_NO_SIZETYPEDEFINE
+    \ingroup RAPIDJSON_CONFIG
+    \brief User-provided \c SizeType definition.
+
+    In order to avoid using 32-bit size types for indexing strings and arrays,
+    define this preprocessor symbol and provide the type rapidjson::SizeType
+    before including RapidJSON:
+    \code
+    #define RAPIDJSON_NO_SIZETYPEDEFINE
+    namespace rapidjson { typedef ::std::size_t SizeType; }
+    #include "rapidjson/..."
+    \endcode
+
+    \see rapidjson::SizeType
+*/
+#ifdef RAPIDJSON_DOXYGEN_RUNNING
+#define RAPIDJSON_NO_SIZETYPEDEFINE
+#endif
+RAPIDJSON_NAMESPACE_BEGIN
+//! Size type (for string lengths, array sizes, etc.)
+/*! RapidJSON uses 32-bit array/string indices even on 64-bit platforms,
+    instead of using \c size_t. Users may override the SizeType by defining
+    \ref RAPIDJSON_NO_SIZETYPEDEFINE.
+*/
+typedef unsigned SizeType;
+RAPIDJSON_NAMESPACE_END
+#endif
+
+// always import std::size_t to rapidjson namespace
+RAPIDJSON_NAMESPACE_BEGIN
+using std::size_t;
+RAPIDJSON_NAMESPACE_END
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_ASSERT
+
+//! Assertion.
+/*! \ingroup RAPIDJSON_CONFIG
+    By default, rapidjson uses C \c assert() for internal assertions.
+    User can override it by defining RAPIDJSON_ASSERT(x) macro.
+
+    \note Parsing errors are handled and can be customized by the
+          \ref RAPIDJSON_ERRORS APIs.
+*/
+#ifndef RAPIDJSON_ASSERT
+#include <cassert>
+#define RAPIDJSON_ASSERT(x) assert(x)
+#endif  // RAPIDJSON_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_STATIC_ASSERT
+
+// Prefer C++11 static_assert, if available
+#ifndef RAPIDJSON_STATIC_ASSERT
+#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1800)
+#define RAPIDJSON_STATIC_ASSERT(x) static_assert(x, RAPIDJSON_STRINGIFY(x))
+#endif  // C++11
+#endif  // RAPIDJSON_STATIC_ASSERT
+
+// Adopt C++03 implementation from boost
+#ifndef RAPIDJSON_STATIC_ASSERT
+#ifndef __clang__
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+#endif
+RAPIDJSON_NAMESPACE_BEGIN
+template <bool x>
+struct STATIC_ASSERTION_FAILURE;
+template <>
+struct STATIC_ASSERTION_FAILURE<true> {
+  enum { value = 1 };
+};
+template <size_t x>
+struct StaticAssertTest {};
+RAPIDJSON_NAMESPACE_END
+
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE __attribute__((unused))
+#else
+#define RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
+#endif
+#ifndef __clang__
+//!@endcond
+#endif
+
+/*! \def RAPIDJSON_STATIC_ASSERT
+    \brief (Internal) macro to check for conditions at compile-time
+    \param x compile-time condition
+    \hideinitializer
+ */
+#define RAPIDJSON_STATIC_ASSERT(x)                               \
+  typedef ::RAPIDJSON_NAMESPACE::StaticAssertTest<sizeof(        \
+      ::RAPIDJSON_NAMESPACE::STATIC_ASSERTION_FAILURE<bool(x)>)> \
+      RAPIDJSON_JOIN(StaticAssertTypedef, __LINE__)              \
+          RAPIDJSON_STATIC_ASSERT_UNUSED_ATTRIBUTE
+#endif  // RAPIDJSON_STATIC_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_LIKELY, RAPIDJSON_UNLIKELY
+
+//! Compiler branching hint for expression with high probability to be true.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+    \param x Boolean expression likely to be true.
+*/
+#ifndef RAPIDJSON_LIKELY
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_LIKELY(x) __builtin_expect(!!(x), 1)
+#else
+#define RAPIDJSON_LIKELY(x) (x)
+#endif
+#endif
+
+//! Compiler branching hint for expression with low probability to be true.
+/*!
+    \ingroup RAPIDJSON_CONFIG
+    \param x Boolean expression unlikely to be true.
+*/
+#ifndef RAPIDJSON_UNLIKELY
+#if defined(__GNUC__) || defined(__clang__)
+#define RAPIDJSON_UNLIKELY(x) __builtin_expect(!!(x), 0)
+#else
+#define RAPIDJSON_UNLIKELY(x) (x)
+#endif
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Helpers
+
+//!@cond RAPIDJSON_HIDDEN_FROM_DOXYGEN
+
+#define RAPIDJSON_MULTILINEMACRO_BEGIN do {
+#define RAPIDJSON_MULTILINEMACRO_END \
+  }                                  \
+  while ((void)0, 0)
+
+// adopted from Boost
+#define RAPIDJSON_VERSION_CODE(x, y, z) (((x)*100000) + ((y)*100) + (z))
+
+#if defined(__has_builtin)
+#define RAPIDJSON_HAS_BUILTIN(x) __has_builtin(x)
+#else
+#define RAPIDJSON_HAS_BUILTIN(x) 0
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_DIAG_PUSH/POP, RAPIDJSON_DIAG_OFF
+
+#if defined(__GNUC__)
+#define RAPIDJSON_GNUC \
+  RAPIDJSON_VERSION_CODE(__GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__)
+#endif
+
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
+                           RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 2, 0))
+
+#define RAPIDJSON_PRAGMA(x) _Pragma(RAPIDJSON_STRINGIFY(x))
+#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(GCC diagnostic x)
+#define RAPIDJSON_DIAG_OFF(x) \
+  RAPIDJSON_DIAG_PRAGMA(ignored RAPIDJSON_STRINGIFY(RAPIDJSON_JOIN(-W, x)))
+
+// push/pop support in Clang and GCC>=4.6
+#if defined(__clang__) || (defined(RAPIDJSON_GNUC) && \
+                           RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0))
+#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
+#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
+#else                       // GCC >= 4.2, < 4.6
+#define RAPIDJSON_DIAG_PUSH /* ignored */
+#define RAPIDJSON_DIAG_POP  /* ignored */
+#endif
+
+#elif defined(_MSC_VER)
+
+// pragma (MSVC specific)
+#define RAPIDJSON_PRAGMA(x) __pragma(x)
+#define RAPIDJSON_DIAG_PRAGMA(x) RAPIDJSON_PRAGMA(warning(x))
+
+#define RAPIDJSON_DIAG_OFF(x) RAPIDJSON_DIAG_PRAGMA(disable : x)
+#define RAPIDJSON_DIAG_PUSH RAPIDJSON_DIAG_PRAGMA(push)
+#define RAPIDJSON_DIAG_POP RAPIDJSON_DIAG_PRAGMA(pop)
+
+#else
+
+#define RAPIDJSON_DIAG_OFF(x) /* ignored */
+#define RAPIDJSON_DIAG_PUSH   /* ignored */
+#define RAPIDJSON_DIAG_POP    /* ignored */
+
+#endif  // RAPIDJSON_DIAG_*
+
+///////////////////////////////////////////////////////////////////////////////
+// C++11 features
+
+#ifndef RAPIDJSON_HAS_CXX11_RVALUE_REFS
+#if defined(__clang__)
+#if __has_feature(cxx_rvalue_references) &&           \
+    (defined(_MSC_VER) || defined(_LIBCPP_VERSION) || \
+     defined(__GLIBCXX__) && __GLIBCXX__ >= 20080306)
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
+#else
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
+#endif
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 3, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1600) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 1
+#else
+#define RAPIDJSON_HAS_CXX11_RVALUE_REFS 0
+#endif
+#endif  // RAPIDJSON_HAS_CXX11_RVALUE_REFS
+
+#ifndef RAPIDJSON_HAS_CXX11_NOEXCEPT
+#if defined(__clang__)
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT __has_feature(cxx_noexcept)
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1900) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT 1
+#else
+#define RAPIDJSON_HAS_CXX11_NOEXCEPT 0
+#endif
+#endif
+#if RAPIDJSON_HAS_CXX11_NOEXCEPT
+#define RAPIDJSON_NOEXCEPT noexcept
+#else
+#define RAPIDJSON_NOEXCEPT /* noexcept */
+#endif                     // RAPIDJSON_HAS_CXX11_NOEXCEPT
+
+// no automatic detection, yet
+#ifndef RAPIDJSON_HAS_CXX11_TYPETRAITS
+#if (defined(_MSC_VER) && _MSC_VER >= 1700)
+#define RAPIDJSON_HAS_CXX11_TYPETRAITS 1
+#else
+#define RAPIDJSON_HAS_CXX11_TYPETRAITS 0
+#endif
+#endif
+
+#ifndef RAPIDJSON_HAS_CXX11_RANGE_FOR
+#if defined(__clang__)
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR __has_feature(cxx_range_for)
+#elif (defined(RAPIDJSON_GNUC) &&                             \
+       (RAPIDJSON_GNUC >= RAPIDJSON_VERSION_CODE(4, 6, 0)) && \
+       defined(__GXX_EXPERIMENTAL_CXX0X__)) ||                \
+    (defined(_MSC_VER) && _MSC_VER >= 1700) ||                \
+    (defined(__SUNPRO_CC) && __SUNPRO_CC >= 0x5140 &&         \
+     defined(__GXX_EXPERIMENTAL_CXX0X__))
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR 1
+#else
+#define RAPIDJSON_HAS_CXX11_RANGE_FOR 0
+#endif
+#endif  // RAPIDJSON_HAS_CXX11_RANGE_FOR
+
+///////////////////////////////////////////////////////////////////////////////
+// C++17 features
+
+#if defined(__has_cpp_attribute)
+#if __has_cpp_attribute(fallthrough)
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH [[fallthrough]]
+#else
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH
+#endif
+#else
+#define RAPIDJSON_DELIBERATE_FALLTHROUGH
+#endif
+
+//!@endcond
+
+//! Assertion (in non-throwing contexts).
+/*! \ingroup RAPIDJSON_CONFIG
+   Some functions provide a \c noexcept guarantee, if the compiler supports it.
+   In these cases, the \ref RAPIDJSON_ASSERT macro cannot be overridden to
+   throw an exception.  This macro adds a separate customization point for
+   such cases.
+
+   Defaults to C \c assert() (as \ref RAPIDJSON_ASSERT), if \c noexcept is
+   supported, and to \ref RAPIDJSON_ASSERT otherwise.
+*/
+
+///////////////////////////////////////////////////////////////////////////////
+// RAPIDJSON_NOEXCEPT_ASSERT
+
+#ifndef RAPIDJSON_NOEXCEPT_ASSERT
+#ifdef RAPIDJSON_ASSERT_THROWS
+#if RAPIDJSON_HAS_CXX11_NOEXCEPT
+#define RAPIDJSON_NOEXCEPT_ASSERT(x)
+#else
+#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
+#endif  // RAPIDJSON_HAS_CXX11_NOEXCEPT
+#else
+#define RAPIDJSON_NOEXCEPT_ASSERT(x) RAPIDJSON_ASSERT(x)
+#endif  // RAPIDJSON_ASSERT_THROWS
+#endif  // RAPIDJSON_NOEXCEPT_ASSERT
+
+///////////////////////////////////////////////////////////////////////////////
+// new/delete
+
+#ifndef RAPIDJSON_NEW
+///! customization point for global \c new
+#define RAPIDJSON_NEW(TypeName) new TypeName
+#endif
+#ifndef RAPIDJSON_DELETE
+///! customization point for global \c delete
+#define RAPIDJSON_DELETE(x) delete x
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Type
+
+/*! \namespace rapidjson
+    \brief main RapidJSON namespace
+    \see RAPIDJSON_NAMESPACE
+*/
+RAPIDJSON_NAMESPACE_BEGIN
+
+//! Type of JSON value
+enum Type {
+  kNullType = 0,    //!< null
+  kFalseType = 1,   //!< false
+  kTrueType = 2,    //!< true
+  kObjectType = 3,  //!< object
+  kArrayType = 4,   //!< array
+  kStringType = 5,  //!< string
+  kNumberType = 6   //!< number
+};
+
+RAPIDJSON_NAMESPACE_END
+
+#endif  // RAPIDJSON_RAPIDJSON_H_