Add slam_in_autonomous_driving repo
This commit is contained in:
Hang Cui
117
workspace/slam_in_autonomous_driving/.clang-format
Normal file
117
workspace/slam_in_autonomous_driving/.clang-format
Normal file
@@ -0,0 +1,117 @@
|
||||
---
|
||||
Language: Cpp
|
||||
# BasedOnStyle: Google
|
||||
AccessModifierOffset: -1
|
||||
AlignAfterOpenBracket: Align
|
||||
AlignConsecutiveAssignments: false
|
||||
AlignConsecutiveDeclarations: false
|
||||
AlignEscapedNewlines: Left
|
||||
AlignOperands: true
|
||||
AlignTrailingComments: true
|
||||
AllowAllParametersOfDeclarationOnNextLine: true
|
||||
AllowShortBlocksOnASingleLine: false
|
||||
AllowShortCaseLabelsOnASingleLine: false
|
||||
AllowShortFunctionsOnASingleLine: All
|
||||
AllowShortIfStatementsOnASingleLine: Always
|
||||
AllowShortLoopsOnASingleLine: true
|
||||
AlwaysBreakAfterDefinitionReturnType: None
|
||||
AlwaysBreakAfterReturnType: None
|
||||
AlwaysBreakBeforeMultilineStrings: true
|
||||
AlwaysBreakTemplateDeclarations: true
|
||||
BinPackArguments: true
|
||||
BinPackParameters: true
|
||||
BraceWrapping:
|
||||
AfterClass: false
|
||||
AfterControlStatement: false
|
||||
AfterEnum: false
|
||||
AfterFunction: false
|
||||
AfterNamespace: false
|
||||
AfterObjCDeclaration: false
|
||||
AfterStruct: false
|
||||
AfterUnion: false
|
||||
AfterExternBlock: false
|
||||
BeforeCatch: false
|
||||
BeforeElse: false
|
||||
IndentBraces: false
|
||||
SplitEmptyFunction: true
|
||||
SplitEmptyRecord: true
|
||||
SplitEmptyNamespace: true
|
||||
BreakBeforeBinaryOperators: None
|
||||
BreakBeforeBraces: Attach
|
||||
BreakBeforeInheritanceComma: false
|
||||
BreakBeforeTernaryOperators: true
|
||||
BreakConstructorInitializersBeforeComma: false
|
||||
BreakConstructorInitializers: BeforeColon
|
||||
BreakAfterJavaFieldAnnotations: false
|
||||
BreakStringLiterals: true
|
||||
ColumnLimit: 120
|
||||
CommentPragmas: '^ IWYU pragma:'
|
||||
CompactNamespaces: false
|
||||
ConstructorInitializerAllOnOneLineOrOnePerLine: true
|
||||
ConstructorInitializerIndentWidth: 4
|
||||
ContinuationIndentWidth: 4
|
||||
Cpp11BracedListStyle: true
|
||||
DerivePointerAlignment: true
|
||||
DisableFormat: false
|
||||
ExperimentalAutoDetectBinPacking: false
|
||||
FixNamespaceComments: true
|
||||
ForEachMacros:
|
||||
- foreach
|
||||
- Q_FOREACH
|
||||
- BOOST_FOREACH
|
||||
IncludeBlocks: Preserve
|
||||
IncludeCategories:
|
||||
- Regex: '^<ext/.*\.h>'
|
||||
Priority: 2
|
||||
- Regex: '^<.*\.h>'
|
||||
Priority: 1
|
||||
- Regex: '^<.*'
|
||||
Priority: 2
|
||||
- Regex: '.*'
|
||||
Priority: 3
|
||||
IncludeIsMainRegex: '([-_](test|unittest))?$'
|
||||
IndentCaseLabels: true
|
||||
IndentPPDirectives: None
|
||||
IndentWidth: 4
|
||||
IndentWrappedFunctionNames: false
|
||||
JavaScriptQuotes: Leave
|
||||
JavaScriptWrapImports: true
|
||||
KeepEmptyLinesAtTheStartOfBlocks: false
|
||||
MacroBlockBegin: ''
|
||||
MacroBlockEnd: ''
|
||||
MaxEmptyLinesToKeep: 1
|
||||
NamespaceIndentation: None
|
||||
ObjCBlockIndentWidth: 2
|
||||
ObjCSpaceAfterProperty: false
|
||||
ObjCSpaceBeforeProtocolList: false
|
||||
PenaltyBreakAssignment: 2
|
||||
PenaltyBreakBeforeFirstCallParameter: 1
|
||||
PenaltyBreakComment: 300
|
||||
PenaltyBreakFirstLessLess: 120
|
||||
PenaltyBreakString: 1000
|
||||
PenaltyExcessCharacter: 1000000
|
||||
PenaltyReturnTypeOnItsOwnLine: 200
|
||||
PointerAlignment: Left
|
||||
RawStringFormats:
|
||||
- Delimiters: [pb]
|
||||
Language: TextProto
|
||||
BasedOnStyle: google
|
||||
ReflowComments: true
|
||||
SortIncludes: true
|
||||
SortUsingDeclarations: true
|
||||
SpaceAfterCStyleCast: false
|
||||
SpaceAfterTemplateKeyword: true
|
||||
SpaceBeforeAssignmentOperators: true
|
||||
SpaceBeforeParens: ControlStatements
|
||||
SpaceInEmptyParentheses: false
|
||||
SpacesBeforeTrailingComments: 2
|
||||
SpacesInAngles: false
|
||||
SpacesInContainerLiterals: true
|
||||
SpacesInCStyleCastParentheses: false
|
||||
SpacesInParentheses: false
|
||||
SpacesInSquareBrackets: false
|
||||
Standard: Auto
|
||||
TabWidth: 8
|
||||
UseTab: Never
|
||||
...
|
||||
|
||||
57
workspace/slam_in_autonomous_driving/.gitignore
vendored
Normal file
57
workspace/slam_in_autonomous_driving/.gitignore
vendored
Normal file
@@ -0,0 +1,57 @@
|
||||
# Prerequisites
|
||||
*.d
|
||||
|
||||
# Compiled Object files
|
||||
*.slo
|
||||
*.lo
|
||||
*.o
|
||||
*.obj
|
||||
|
||||
# Precompiled Headers
|
||||
*.gch
|
||||
*.pch
|
||||
|
||||
# Compiled Dynamic libraries
|
||||
*.so
|
||||
*.dylib
|
||||
*.dll
|
||||
|
||||
# Fortran module files
|
||||
*.mod
|
||||
*.smod
|
||||
|
||||
# Compiled Static libraries
|
||||
*.lai
|
||||
*.la
|
||||
*.a
|
||||
*.lib
|
||||
|
||||
# Executables
|
||||
*.exe
|
||||
*.out
|
||||
*.app
|
||||
|
||||
# latex
|
||||
*.aux
|
||||
*.blg
|
||||
*.log
|
||||
*.toc
|
||||
*.gz
|
||||
*.bbl
|
||||
|
||||
# cmake
|
||||
**build**
|
||||
|
||||
bin
|
||||
.idea
|
||||
data/ch6/frame*.txt
|
||||
data/ch6/*.png
|
||||
data/ch7/**.pcd
|
||||
**DS_Store
|
||||
|
||||
data/ch9/**.pcd
|
||||
data/ch3/state.txt
|
||||
data/ch3/gins.txt
|
||||
data/ch3/gnss_output.txt
|
||||
data/ch4/gins_preintg.txt
|
||||
|
||||
23
workspace/slam_in_autonomous_driving/CMakeLists.txt
Normal file
23
workspace/slam_in_autonomous_driving/CMakeLists.txt
Normal file
@@ -0,0 +1,23 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(slam_in_auto_driving)
|
||||
|
||||
set(CMAKE_BUILD_TYPE "Release")
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_FLAGS "-w")
|
||||
set(CMAKE_CXX_FLAGS_RELEASE "-O2 -g -ggdb ${CMAKE_CXX_FLAGS}")
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "-g ${CMAKE_CXX_FLAGS}")
|
||||
|
||||
add_definitions("-DCATKIN_ENABLE_TESTING=0")
|
||||
|
||||
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
|
||||
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
|
||||
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
|
||||
|
||||
option(BUILD_WITH_UBUNTU1804 OFF)
|
||||
|
||||
include(cmake/packages.cmake)
|
||||
include_directories(${PROJECT_SOURCE_DIR}/src)
|
||||
include_directories(${PROJECT_SOURCE_DIR})
|
||||
include_directories(${PROJECT_SOURCE_DIR}/build/devel/include)
|
||||
|
||||
add_subdirectory(src)
|
||||
22
workspace/slam_in_autonomous_driving/Dockerfile
Normal file
22
workspace/slam_in_autonomous_driving/Dockerfile
Normal file
@@ -0,0 +1,22 @@
|
||||
FROM osrf/ros:noetic-desktop-full
|
||||
|
||||
ADD docker/sources.list /etc/apt
|
||||
|
||||
RUN apt-get update \
|
||||
&& apt-get install -y ros-noetic-pcl-ros ros-noetic-velodyne-msgs libopencv-dev libgoogle-glog-dev libeigen3-dev libsuitesparse-dev libpcl-dev libyaml-cpp-dev libbtbb-dev libgmock-dev unzip python3-tk\
|
||||
&& mkdir /sad
|
||||
|
||||
COPY ./thirdparty/ /sad/
|
||||
|
||||
WORKDIR /sad/
|
||||
|
||||
RUN ls -la \
|
||||
&& rm -rf ./Pangolin \
|
||||
&& unzip ./Pangolin.zip \
|
||||
&& mkdir ./Pangolin/build \
|
||||
&& cmake ./Pangolin -B ./Pangolin/build \
|
||||
&& make -j4 -C ./Pangolin/build install \
|
||||
# && cmake ./g2o -B ./g2o/build \
|
||||
# && make -j4 -C ./g2o/build install \
|
||||
&& rm -rf /var/lib/apt/lists/* \
|
||||
&& rm -rf /sad
|
||||
27
workspace/slam_in_autonomous_driving/cmake/FindCSparse.cmake
Normal file
27
workspace/slam_in_autonomous_driving/cmake/FindCSparse.cmake
Normal file
@@ -0,0 +1,27 @@
|
||||
# Look for csparse; note the difference in the directory specifications!
|
||||
find_path(CSPARSE_INCLUDE_DIR NAMES cs.h
|
||||
PATHS
|
||||
/usr/include/suitesparse
|
||||
/usr/include
|
||||
/opt/local/include
|
||||
/usr/local/include
|
||||
/sw/include
|
||||
/usr/include/ufsparse
|
||||
/opt/local/include/ufsparse
|
||||
/usr/local/include/ufsparse
|
||||
/sw/include/ufsparse
|
||||
PATH_SUFFIXES
|
||||
suitesparse
|
||||
)
|
||||
|
||||
find_library(CSPARSE_LIBRARY NAMES cxsparse libcxsparse
|
||||
PATHS
|
||||
/usr/lib
|
||||
/usr/local/lib
|
||||
/opt/local/lib
|
||||
/sw/lib
|
||||
)
|
||||
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(CSPARSE DEFAULT_MSG
|
||||
CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)
|
||||
94
workspace/slam_in_autonomous_driving/cmake/FindCholmod.cmake
Normal file
94
workspace/slam_in_autonomous_driving/cmake/FindCholmod.cmake
Normal file
@@ -0,0 +1,94 @@
|
||||
# Cholmod lib usually requires linking to a blas and lapack library.
|
||||
# It is up to the user of this module to find a BLAS and link to it.
|
||||
|
||||
if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
|
||||
set(CHOLMOD_FIND_QUIETLY TRUE)
|
||||
endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
|
||||
|
||||
find_path(CHOLMOD_INCLUDE_DIR
|
||||
NAMES
|
||||
cholmod.h
|
||||
PATHS
|
||||
$ENV{CHOLMODDIR}
|
||||
${INCLUDE_INSTALL_DIR}
|
||||
PATH_SUFFIXES
|
||||
suitesparse
|
||||
ufsparse
|
||||
)
|
||||
|
||||
find_library(CHOLMOD_LIBRARY
|
||||
NAMES cholmod libcholmod
|
||||
PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY})
|
||||
|
||||
if(CHOLMOD_LIBRARIES)
|
||||
|
||||
get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
|
||||
|
||||
find_library(AMD_LIBRARY
|
||||
NAMES amd libamd
|
||||
PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
if (AMD_LIBRARY)
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
|
||||
else ()
|
||||
set(CHOLMOD_LIBRARIES FALSE)
|
||||
endif ()
|
||||
|
||||
endif(CHOLMOD_LIBRARIES)
|
||||
|
||||
if(CHOLMOD_LIBRARIES)
|
||||
|
||||
find_library(COLAMD_LIBRARY NAMES colamd libcolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
if (COLAMD_LIBRARY)
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
|
||||
else ()
|
||||
set(CHOLMOD_LIBRARIES FALSE)
|
||||
endif ()
|
||||
|
||||
endif(CHOLMOD_LIBRARIES)
|
||||
|
||||
if(CHOLMOD_LIBRARIES)
|
||||
|
||||
find_library(CAMD_LIBRARY NAMES camd libcamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
if (CAMD_LIBRARY)
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
|
||||
else ()
|
||||
set(CHOLMOD_LIBRARIES FALSE)
|
||||
endif ()
|
||||
|
||||
endif(CHOLMOD_LIBRARIES)
|
||||
|
||||
if(CHOLMOD_LIBRARIES)
|
||||
|
||||
find_library(CCOLAMD_LIBRARY NAMES ccolamd libccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
if (CCOLAMD_LIBRARY)
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
|
||||
else ()
|
||||
set(CHOLMOD_LIBRARIES FALSE)
|
||||
endif ()
|
||||
|
||||
endif(CHOLMOD_LIBRARIES)
|
||||
|
||||
if(CHOLMOD_LIBRARIES)
|
||||
|
||||
find_library(CHOLMOD_METIS_LIBRARY NAMES metis libmetis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
if (CHOLMOD_METIS_LIBRARY)
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
|
||||
endif ()
|
||||
|
||||
endif(CHOLMOD_LIBRARIES)
|
||||
|
||||
if(CHOLMOD_LIBRARIES)
|
||||
find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
|
||||
PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
|
||||
if (CHOLMOD_SUITESPARSECONFIG_LIBRARY)
|
||||
set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY})
|
||||
endif ()
|
||||
endif(CHOLMOD_LIBRARIES)
|
||||
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
|
||||
CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES)
|
||||
|
||||
mark_as_advanced(CHOLMOD_LIBRARIES)
|
||||
209
workspace/slam_in_autonomous_driving/cmake/FindGlog.cmake
Normal file
209
workspace/slam_in_autonomous_driving/cmake/FindGlog.cmake
Normal file
@@ -0,0 +1,209 @@
|
||||
# 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)
|
||||
#
|
||||
|
||||
# FindGlog.cmake - Find Google glog logging library.
|
||||
#
|
||||
# This module defines the following variables:
|
||||
#
|
||||
# GLOG_FOUND: TRUE iff glog is found.
|
||||
# GLOG_INCLUDE_DIRS: Include directories for glog.
|
||||
# GLOG_LIBRARIES: Libraries required to link glog.
|
||||
#
|
||||
# The following variables control the behaviour of this module:
|
||||
#
|
||||
# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to
|
||||
# search for glog includes, e.g: /timbuktu/include.
|
||||
# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to
|
||||
# search for glog libraries, e.g: /timbuktu/lib.
|
||||
#
|
||||
# The following variables are also defined by this module, but in line with
|
||||
# CMake recommended FindPackage() module style should NOT be referenced directly
|
||||
# by callers (use the plural variables detailed above instead). These variables
|
||||
# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
|
||||
# are NOT re-called (i.e. search for library is not repeated) if these variables
|
||||
# are set with valid values _in the CMake cache_. This means that if these
|
||||
# variables are set directly in the cache, either by the user in the CMake GUI,
|
||||
# or by the user passing -DVAR=VALUE directives to CMake when called (which
|
||||
# explicitly defines a cache variable), then they will be used verbatim,
|
||||
# bypassing the HINTS variables and other hard-coded search locations.
|
||||
#
|
||||
# GLOG_INCLUDE_DIR: Include directory for glog, not including the
|
||||
# include directory of any dependencies.
|
||||
# GLOG_LIBRARY: glog library, not including the libraries of any
|
||||
# dependencies.
|
||||
|
||||
# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when
|
||||
# FindGlog was invoked.
|
||||
macro(GLOG_RESET_FIND_LIBRARY_PREFIX)
|
||||
if (MSVC)
|
||||
set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}")
|
||||
endif (MSVC)
|
||||
endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX)
|
||||
|
||||
# Called if we failed to find glog 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(GLOG_REPORT_NOT_FOUND REASON_MSG)
|
||||
unset(GLOG_FOUND)
|
||||
unset(GLOG_INCLUDE_DIRS)
|
||||
unset(GLOG_LIBRARIES)
|
||||
# Make results of search visible in the CMake GUI if glog has not
|
||||
# been found so that user does not have to toggle to advanced view.
|
||||
mark_as_advanced(CLEAR GLOG_INCLUDE_DIR
|
||||
GLOG_LIBRARY)
|
||||
|
||||
glog_reset_find_library_prefix()
|
||||
|
||||
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
|
||||
# use the camelcase library name, not uppercase.
|
||||
if (Glog_FIND_QUIETLY)
|
||||
message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN})
|
||||
elseif (Glog_FIND_REQUIRED)
|
||||
message(FATAL_ERROR "Failed to find glog - " ${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 glog - " ${REASON_MSG} ${ARGN})
|
||||
endif ()
|
||||
endmacro(GLOG_REPORT_NOT_FOUND)
|
||||
|
||||
# Handle possible presence of lib prefix for libraries on MSVC, see
|
||||
# also GLOG_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)
|
||||
|
||||
# Search user-installed locations first, so that we prefer user installs
|
||||
# to system installs where both exist.
|
||||
list(APPEND GLOG_CHECK_INCLUDE_DIRS
|
||||
/usr/local/include
|
||||
/usr/local/homebrew/include # Mac OS X
|
||||
/opt/local/var/macports/software # Mac OS X.
|
||||
/opt/local/include
|
||||
/usr/include)
|
||||
# Windows (for C:/Program Files prefix).
|
||||
list(APPEND GLOG_CHECK_PATH_SUFFIXES
|
||||
glog/include
|
||||
glog/Include
|
||||
Glog/include
|
||||
Glog/Include)
|
||||
|
||||
list(APPEND GLOG_CHECK_LIBRARY_DIRS
|
||||
/usr/local/lib
|
||||
/usr/local/homebrew/lib # Mac OS X.
|
||||
/opt/local/lib
|
||||
/usr/lib)
|
||||
# Windows (for C:/Program Files prefix).
|
||||
list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES
|
||||
glog/lib
|
||||
glog/Lib
|
||||
Glog/lib
|
||||
Glog/Lib)
|
||||
|
||||
# Search supplied hint directories first if supplied.
|
||||
find_path(GLOG_INCLUDE_DIR
|
||||
NAMES glog/logging.h
|
||||
PATHS ${GLOG_INCLUDE_DIR_HINTS}
|
||||
${GLOG_CHECK_INCLUDE_DIRS}
|
||||
PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES})
|
||||
if (NOT GLOG_INCLUDE_DIR OR
|
||||
NOT EXISTS ${GLOG_INCLUDE_DIR})
|
||||
glog_report_not_found(
|
||||
"Could not find glog include directory, set GLOG_INCLUDE_DIR "
|
||||
"to directory containing glog/logging.h")
|
||||
endif (NOT GLOG_INCLUDE_DIR OR
|
||||
NOT EXISTS ${GLOG_INCLUDE_DIR})
|
||||
|
||||
find_library(GLOG_LIBRARY NAMES glog
|
||||
PATHS ${GLOG_LIBRARY_DIR_HINTS}
|
||||
${GLOG_CHECK_LIBRARY_DIRS}
|
||||
PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES})
|
||||
if (NOT GLOG_LIBRARY OR
|
||||
NOT EXISTS ${GLOG_LIBRARY})
|
||||
glog_report_not_found(
|
||||
"Could not find glog library, set GLOG_LIBRARY "
|
||||
"to full path to libglog.")
|
||||
endif (NOT GLOG_LIBRARY OR
|
||||
NOT EXISTS ${GLOG_LIBRARY})
|
||||
|
||||
# Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets
|
||||
# if called.
|
||||
set(GLOG_FOUND TRUE)
|
||||
|
||||
# Glog does not seem to provide any record of the version in its
|
||||
# source tree, thus cannot extract version.
|
||||
|
||||
# Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and
|
||||
# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
|
||||
# invalid, otherwise we would report the library as found.
|
||||
if (GLOG_INCLUDE_DIR AND
|
||||
NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
|
||||
glog_report_not_found(
|
||||
"Caller defined GLOG_INCLUDE_DIR:"
|
||||
" ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.")
|
||||
endif (GLOG_INCLUDE_DIR AND
|
||||
NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h)
|
||||
# TODO: This regex for glog library is pretty primitive, we use lowercase
|
||||
# for comparison to handle Windows using CamelCase library names, could
|
||||
# this check be better?
|
||||
string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY)
|
||||
if (GLOG_LIBRARY AND
|
||||
NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
|
||||
glog_report_not_found(
|
||||
"Caller defined GLOG_LIBRARY: "
|
||||
"${GLOG_LIBRARY} does not match glog.")
|
||||
endif (GLOG_LIBRARY AND
|
||||
NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*")
|
||||
|
||||
# Set standard CMake FindPackage variables if found.
|
||||
if (GLOG_FOUND)
|
||||
set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR})
|
||||
set(GLOG_LIBRARIES ${GLOG_LIBRARY})
|
||||
endif (GLOG_FOUND)
|
||||
|
||||
glog_reset_find_library_prefix()
|
||||
|
||||
# Handle REQUIRED / QUIET optional arguments.
|
||||
include(FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args(Glog DEFAULT_MSG
|
||||
GLOG_INCLUDE_DIRS GLOG_LIBRARIES)
|
||||
|
||||
# Only mark internal variables as advanced if we found glog, otherwise
|
||||
# leave them visible in the standard GUI for the user to set manually.
|
||||
if (GLOG_FOUND)
|
||||
mark_as_advanced(FORCE GLOG_INCLUDE_DIR
|
||||
GLOG_LIBRARY)
|
||||
endif (GLOG_FOUND)
|
||||
137
workspace/slam_in_autonomous_driving/cmake/FindPANGO.cmake
Executable file
137
workspace/slam_in_autonomous_driving/cmake/FindPANGO.cmake
Executable file
@@ -0,0 +1,137 @@
|
||||
# - Try to find Pango
|
||||
# Once done, this will define
|
||||
#
|
||||
# PANGO_FOUND - system has Pango
|
||||
# PANGO_INCLUDE_DIRS - the Pango include directories
|
||||
# PANGO_LIBRARIES - link these to use Pango
|
||||
# PANGO_DEFINITIONS - compiler flags
|
||||
# PANGOCAIRO_FOUND - system has Pangocairo
|
||||
# PANGOFT2_FOUND - system has Pangoft2
|
||||
|
||||
FIND_PACKAGE(PkgConfig)
|
||||
|
||||
# pango main library
|
||||
PKG_CHECK_MODULES(PC_PANGO QUIET pango)
|
||||
SET(PANGO_DEFINITIONS ${PC_PANGO_CFLAGS_OTHER})
|
||||
|
||||
SET(PANGO_INCLUDE_HINTS
|
||||
${PC_PANGO_INCLUDEDIR}
|
||||
${PC_PANGO_INCLUDE_DIRS}
|
||||
$ENV{PANGO_HOME}/include
|
||||
$ENV{PANGO_ROOT}/include
|
||||
/usr/local/include
|
||||
/usr/include
|
||||
/pango/include
|
||||
)
|
||||
FIND_PATH(PANGO_INCLUDE_DIR
|
||||
NAMES pango/pango.h
|
||||
HINTS ${PANGO_INCLUDE_HINTS}
|
||||
PATH_SUFFIXES pango pango-1.0 pango1.0
|
||||
)
|
||||
|
||||
SET(PANGO_LIBRARY_HINTS
|
||||
${PC_PANGO_LIBDIR}
|
||||
${PC_PANGO_LIBRARY_DIRS}
|
||||
$ENV{PANGO_HOME}/lib
|
||||
$ENV{PANGO_ROOT}/lib
|
||||
/usr/local/lib
|
||||
/usr/lib
|
||||
/lib
|
||||
/pango/lib
|
||||
)
|
||||
FIND_LIBRARY(PANGO_LIBRARY
|
||||
NAMES pango PANGO PANGO-1.0 pango-1.0
|
||||
HINTS ${PANGO_LIBRARY_HINTS}
|
||||
PATH_SUFFIXES pango pango-1.0 pango1.0
|
||||
)
|
||||
|
||||
SET(PANGO_INCLUDE_DIRS ${PANGO_INCLUDE_DIR})
|
||||
SET(PANGO_LIBRARIES ${PANGO_LIBRARY})
|
||||
|
||||
INCLUDE(FindPackageHandleStandardArgs)
|
||||
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Pango DEFAULT_MSG PANGO_INCLUDE_DIRS PANGO_LIBRARIES)
|
||||
MARK_AS_ADVANCED(PANGO_INCLUDE_DIR PANGO_LIBRARY)
|
||||
|
||||
IF(PANGO_FOUND)
|
||||
# dependencies
|
||||
FIND_PACKAGE(GLIB2 REQUIRED)
|
||||
IF(GLIB2_FOUND)
|
||||
SET(PANGO_INCLUDE_DIRS ${PANGO_INCLUDE_DIRS} ${GLIB2_INCLUDE_DIRS})
|
||||
SET(PANGO_LIBRARIES ${PANGO_LIBRARIES} ${GLIB2_LIBRARIES})
|
||||
ENDIF()
|
||||
|
||||
FIND_PACKAGE(GObject REQUIRED)
|
||||
IF(GLIB2_FOUND)
|
||||
SET(PANGO_INCLUDE_DIRS ${PANGO_INCLUDE_DIRS} ${GOBJECT_INCLUDE_DIRS})
|
||||
SET(PANGO_LIBRARIES ${PANGO_LIBRARIES} ${GOBJECT_LIBRARIES})
|
||||
ENDIF()
|
||||
|
||||
# pangocairo
|
||||
PKG_CHECK_MODULES(PC_PANGOCAIRO QUIET pangocairo)
|
||||
FIND_PATH(PANGOCAIRO_INCLUDE_DIRS
|
||||
NAMES pango/pangocairo.h
|
||||
HINTS ${PC_PANGOCAIRO_INCLUDEDIR}
|
||||
${PC_PANGOCAIRO_INCLUDE_DIRS}
|
||||
$ENV{PANGOCAIRO_HOME}/include
|
||||
$ENV{PANGOCAIRO_ROOT}/include
|
||||
/pangocairo/include
|
||||
${PANGO_INCLUDE_HINTS}
|
||||
PATH_SUFFIXES pango pango-1.0 pangocairo libpangocairo-1.0 pangocairo1.0
|
||||
)
|
||||
IF(PANGOCAIRO_INCLUDE_DIRS)
|
||||
SET(PANGO_INCLUDE_DIRS ${PANGO_INCLUDE_DIRS} ${PANGOCAIRO_INCLUDE_DIRS})
|
||||
ENDIF()
|
||||
FIND_LIBRARY(PANGOCAIRO_LIBRARIES
|
||||
NAMES pangocairo PANGOcairo PANGOcairo-1.0 pangocairo-1.0 libpangocairo-1.0
|
||||
HINTS ${PC_PANGOCAIRO_LIBDIR}
|
||||
${PC_PANGOCAIRO_LIBRARY_DIRS}
|
||||
$ENV{PANGOCAIRO_HOME}/lib
|
||||
$ENV{PANGOCAIRO_ROOT}/lib
|
||||
${PANGO_LIBRARY_HINTS}
|
||||
PATH_SUFFIXES pango pango-1.0 pangocairo libpangocairo-1.0 pangocairo1.0
|
||||
)
|
||||
IF(PANGOCAIRO_LIBRARIES)
|
||||
SET(PANGO_LIBRARIES ${PANGO_LIBRARIES} ${PANGOCAIRO_LIBRARIES})
|
||||
ENDIF()
|
||||
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Pangocairo DEFAULT_MSG PANGOCAIRO_INCLUDE_DIRS PANGOCAIRO_LIBRARIES)
|
||||
|
||||
# pangoft2
|
||||
PKG_CHECK_MODULES(PC_PANGOFT2 QUIET pangoft2)
|
||||
FIND_PATH(PANGOFT2_INCLUDE_DIRS
|
||||
NAMES pango/pangoft2.h
|
||||
HINTS ${PC_PANGOFT2_INCLUDEDIR}
|
||||
${PC_PANGOFT2_INCLUDE_DIRS}
|
||||
$ENV{PANGOFT2_HOME}/include
|
||||
$ENV{PANGOFT2_ROOT}/include
|
||||
/pangoft2/include
|
||||
${PANGO_INCLUDE_HINTS}
|
||||
PATH_SUFFIXES pango pangoft2 libpangoft2-1.0 pangoft21.0
|
||||
)
|
||||
IF(PANGOFT2_INCLUDE_DIRS)
|
||||
SET(PANGOFT2_INCLUDE_DIRS ${PANGO_INCLUDE_DIRS} ${PANGOFT2_INCLUDE_DIRS})
|
||||
FIND_PACKAGE(Freetype REQUIRED)
|
||||
IF(FREETYPE_FOUND)
|
||||
SET(PANGOFT2_INCLUDE_DIRS ${PANGOFT2_INCLUDE_DIRS} ${FREETYPE_INCLUDE_DIRS})
|
||||
ENDIF()
|
||||
FIND_PACKAGE(Fontconfig REQUIRED)
|
||||
IF(FONTCONFIG_FOUND)
|
||||
SET(PANGOFT2_INCLUDE_DIRS ${PANGOFT2_INCLUDE_DIRS} ${FONTCONFIG_INCLUDE_DIRS})
|
||||
ENDIF()
|
||||
ENDIF()
|
||||
FIND_LIBRARY(PANGOFT2_LIBRARIES
|
||||
NAMES pangoft2 pangoft2-1.0 libpangoft2-1.0
|
||||
HINTS ${PC_PANGOFT2_LIBDIR}
|
||||
${PC_PANGOFT2_LIBRARY_DIRS}
|
||||
$ENV{PANGOFT2_HOME}/lib
|
||||
$ENV{PANGOFT2_ROOT}/lib
|
||||
${PANGO_LIBRARY_HINTS}
|
||||
PATH_SUFFIXES pango pangoft2 libpangoft2-1.0 pangoft21.0
|
||||
)
|
||||
IF(PANGOFT2_LIBRARIES)
|
||||
SET(PANGOFT2_LIBRARIES ${PANGO_LIBRARIES} ${PANGOFT2_LIBRARIES})
|
||||
ENDIF()
|
||||
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Pangoft2 DEFAULT_MSG PANGOFT2_INCLUDE_DIRS PANGOFT2_LIBRARIES)
|
||||
|
||||
MARK_AS_ADVANCED(PANGOCAIRO_INCLUDE_DIRS PANGOCAIRO_LIBRARIES PANGOFT2_INCLUDE_DIRS PANGOFT2_LIBRARIES)
|
||||
|
||||
ENDIF()
|
||||
132
workspace/slam_in_autonomous_driving/cmake/packages.cmake
Normal file
132
workspace/slam_in_autonomous_driving/cmake/packages.cmake
Normal file
@@ -0,0 +1,132 @@
|
||||
# 引入该目录下的.cmake文件
|
||||
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
|
||||
|
||||
# livox ros driver
|
||||
add_subdirectory(thirdparty/livox_ros_driver)
|
||||
|
||||
include_directories(${CMAKE_BINARY_DIR}/devel/include) # 引用ros生成的msg header
|
||||
|
||||
# eigen 3
|
||||
find_package(Eigen3 REQUIRED)
|
||||
include_directories(${EIGEN3_INCLUDE_DIRS})
|
||||
|
||||
# sophus
|
||||
include_directories(${PROJECT_SOURCE_DIR}/thirdparty/sophus)
|
||||
|
||||
# glog
|
||||
find_package(Glog REQUIRED)
|
||||
include_directories(${Glog_INCLUDE_DIRS})
|
||||
|
||||
# csparse
|
||||
find_package(CSparse REQUIRED)
|
||||
include_directories(${CSPARSE_INCLUDE_DIR})
|
||||
|
||||
# cholmod
|
||||
find_package(Cholmod REQUIRED)
|
||||
include_directories(${CHOLMOD_INCLUDE_DIRS})
|
||||
|
||||
# pcl
|
||||
find_package(PCL REQUIRED)
|
||||
include_directories(${PCL_INCLUDE_DIRS})
|
||||
|
||||
# opencv
|
||||
find_package(OpenCV REQUIRED)
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
|
||||
# g2o 使用thirdparty中的
|
||||
include_directories(${PROJECT_SOURCE_DIR}/thirdparty/g2o/)
|
||||
set(g2o_libs
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_stuff.so
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_core.so
|
||||
# ${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_solver_cholmod.so
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_solver_dense.so
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_solver_csparse.so
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_csparse_extension.so
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o_types_sba.so
|
||||
${CSPARSE_LIBRARY}
|
||||
${CHOLMOD_LIBRARY}
|
||||
)
|
||||
|
||||
# ros
|
||||
# 为了2D scan, pointcloud2
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
pcl_ros
|
||||
pcl_conversions
|
||||
)
|
||||
include_directories(${catkin_INCLUDE_DIRS})
|
||||
|
||||
find_package(Pangolin REQUIRED)
|
||||
include_directories(${Pangolin_INCLUDE_DIRS})
|
||||
|
||||
# yaml-cpp
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
include_directories(${yaml-cpp_INCLUDE_DIRS})
|
||||
|
||||
# 其他thirdparty下的内容
|
||||
include_directories(${PROJECT_SOURCE_DIR}/thirdparty/)
|
||||
include_directories(${PROJECT_SOURCE_DIR}/thirdparty/velodyne/include)
|
||||
|
||||
if(BUILD_WITH_UBUNTU1804)
|
||||
function(extract_file filename extract_dir)
|
||||
message(STATUS "Extract ${filename} to ${extract_dir} ...")
|
||||
set(temp_dir ${extract_dir})
|
||||
if(EXISTS ${temp_dir})
|
||||
file(REMOVE_RECURSE ${temp_dir})
|
||||
endif()
|
||||
file(MAKE_DIRECTORY ${temp_dir})
|
||||
execute_process(COMMAND ${CMAKE_COMMAND} -E tar -xvzf ${filename}
|
||||
WORKING_DIRECTORY ${temp_dir})
|
||||
endfunction()
|
||||
|
||||
set(TBB_ROOT_DIR ${PROJECT_SOURCE_DIR}/thirdparty/tbb/oneTBB-2019_U8/oneTBB-2019_U8)
|
||||
set(TBB_BUILD_DIR "tbb_build_dir=${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}")
|
||||
set(TBB_BUILD_PREFIX "tbb_build_prefix=tbb")
|
||||
|
||||
extract_file(${PROJECT_SOURCE_DIR}/thirdparty/tbb/2019_U8.tar.gz ${PROJECT_SOURCE_DIR}/thirdparty/tbb/oneTBB-2019_U8)
|
||||
|
||||
include(${TBB_ROOT_DIR}/cmake/TBBBuild.cmake)
|
||||
|
||||
#message(STATUS "======TBB_BUILD_DIR = ${TBB_BUILD_DIR}")
|
||||
#message(STATUS "======TBB_BUILD_PREFIX = ${TBB_BUILD_PREFIX}")
|
||||
|
||||
tbb_build(TBB_ROOT ${TBB_ROOT_DIR}
|
||||
compiler=gcc-9
|
||||
stdver=c++17
|
||||
${TBB_BUILD_DIR}
|
||||
${TBB_BUILD_PREFIX}
|
||||
CONFIG_DIR
|
||||
TBB_DIR)
|
||||
|
||||
find_package(TBB REQUIRED)
|
||||
|
||||
include_directories(${PROJECT_SOURCE_DIR}/thirdparty/tbb/oneTBB-2019_U8/oneTBB-2019_U8/include)
|
||||
link_directories(${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}/tbb_release)
|
||||
|
||||
set(third_party_libs
|
||||
${catkin_LIBRARIES}
|
||||
${g2o_libs}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
${Pangolin_LIBRARIES}
|
||||
glog gflags
|
||||
${yaml-cpp_LIBRARIES}
|
||||
yaml-cpp
|
||||
TBB::tbb
|
||||
)
|
||||
else()
|
||||
set(third_party_libs
|
||||
${catkin_LIBRARIES}
|
||||
${g2o_libs}
|
||||
${OpenCV_LIBS}
|
||||
${PCL_LIBRARIES}
|
||||
${Pangolin_LIBRARIES}
|
||||
glog gflags
|
||||
${yaml-cpp_LIBRARIES}
|
||||
yaml-cpp
|
||||
tbb
|
||||
)
|
||||
endif ()
|
||||
15
workspace/slam_in_autonomous_driving/config/avia.yaml
Normal file
15
workspace/slam_in_autonomous_driving/config/avia.yaml
Normal file
@@ -0,0 +1,15 @@
|
||||
preprocess:
|
||||
lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
|
||||
scan_line: 6
|
||||
time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效
|
||||
|
||||
mapping:
|
||||
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
|
||||
extrinsic_R: [ 1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1 ]
|
||||
|
||||
point_filter_num: 3
|
||||
max_iteration: 10
|
||||
imu_has_gravity: true
|
||||
with_ui: false
|
||||
37
workspace/slam_in_autonomous_driving/config/mapping.yaml
Normal file
37
workspace/slam_in_autonomous_driving/config/mapping.yaml
Normal file
@@ -0,0 +1,37 @@
|
||||
YAML: 1.0
|
||||
bag_path: /home/xiang/Data/nclt/20130110.bag
|
||||
lio_yaml: ./config/velodyne_nclt.yaml
|
||||
map_data: ./data/ch9/map_data/
|
||||
origin:
|
||||
- 276726.5626579862
|
||||
- 4685961.4821172
|
||||
- 272.809
|
||||
rtk_has_rot: false
|
||||
rtk_outlier_th: 1.0
|
||||
rtk_pos_noise: 2.0
|
||||
rtk_height_noise_ratio: 10.0
|
||||
rtk_ang_noise: 5.0
|
||||
lidar_continuous_num: 5
|
||||
rtk_ext:
|
||||
t: [0, 0.24, -0.283]
|
||||
loop_closing:
|
||||
min_id_interval: 100
|
||||
min_distance: 30
|
||||
skip_id: 5
|
||||
ndt_score_th: 4.5
|
||||
common:
|
||||
dataset: nclt
|
||||
lid_topic: points_raw
|
||||
imu_topic: imu_raw
|
||||
time_sync_en: false
|
||||
preprocess:
|
||||
lidar_type: 2
|
||||
scan_line: 32
|
||||
time_scale: 1e-3
|
||||
mapping:
|
||||
extrinsic_est_en: true
|
||||
extrinsic_T: [0, 0, 0.28]
|
||||
extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1]
|
||||
point_filter_num: 3
|
||||
max_iteration: 10
|
||||
imu_has_gravity: true
|
||||
48
workspace/slam_in_autonomous_driving/config/velodyne.yaml
Normal file
48
workspace/slam_in_autonomous_driving/config/velodyne.yaml
Normal file
@@ -0,0 +1,48 @@
|
||||
common:
|
||||
dataset: "nclt"
|
||||
lid_topic: "points_raw"
|
||||
imu_topic: "imu_raw"
|
||||
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
|
||||
|
||||
preprocess:
|
||||
lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
|
||||
scan_line: 32
|
||||
blind: 4
|
||||
time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效
|
||||
|
||||
mapping:
|
||||
acc_cov: 0.1
|
||||
gyr_cov: 0.1
|
||||
b_acc_cov: 0.0001
|
||||
b_gyr_cov: 0.0001
|
||||
fov_degree: 180
|
||||
det_range: 100.0
|
||||
extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic
|
||||
extrinsic_T: [ 0, 0, 0.28 ]
|
||||
extrinsic_R: [ 1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1 ]
|
||||
|
||||
publish:
|
||||
path_publish_en: false
|
||||
scan_publish_en: true # false: close all the point cloud output
|
||||
scan_effect_pub_en: true # true: publish the pointscloud of effect point
|
||||
dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.
|
||||
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
|
||||
|
||||
path_save_en: true # 保存轨迹，用于精度计算和比较
|
||||
|
||||
pcd_save:
|
||||
pcd_save_en: false
|
||||
interval: -1 # how many LiDAR frames saved in each pcd file;
|
||||
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too many frames.
|
||||
|
||||
IMU:
|
||||
NoiseGyro: 1e-2 # 3 # 2.44e-4 #1e-3 # rad/s^0.5
|
||||
NoiseAcc: 1e-1 #2 # 1.47e-3 #1e-2 # m/s^1.5
|
||||
GyroWalk: 1e-6 # rad/s^1.5
|
||||
AccWalk: 1e-4 # m/s^2.5
|
||||
Frequency: 200.0
|
||||
|
||||
point_filter_num: 7
|
||||
max_iteration: 4
|
||||
@@ -0,0 +1,15 @@
|
||||
preprocess:
|
||||
lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
|
||||
scan_line: 32
|
||||
time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效
|
||||
|
||||
mapping:
|
||||
extrinsic_T: [ 0, 0, 0.28 ]
|
||||
extrinsic_R: [ 1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1 ]
|
||||
|
||||
point_filter_num: 10
|
||||
max_iteration: 3
|
||||
imu_has_gravity: true
|
||||
with_ui: false
|
||||
@@ -0,0 +1,15 @@
|
||||
preprocess:
|
||||
lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
|
||||
scan_line: 32
|
||||
time_scale: 1e3 # 兼容不同数据集的时间单位
|
||||
|
||||
mapping:
|
||||
extrinsic_T: [ 0, 0, -0.28 ]
|
||||
extrinsic_R: [ 2.67949e-08,-1, 0,
|
||||
1,2.67949e-08, 0,
|
||||
0, 0, 1, ]
|
||||
|
||||
point_filter_num: 10
|
||||
max_iteration: 3
|
||||
imu_has_gravity: false
|
||||
with_ui: false
|
||||
@@ -0,0 +1,15 @@
|
||||
preprocess:
|
||||
lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
|
||||
scan_line: 32
|
||||
time_scale: 1e-3 # 兼容不同数据集的时间单位
|
||||
|
||||
mapping:
|
||||
extrinsic_T: [ -0.5, 1.4, 1.5 ]
|
||||
extrinsic_R: [ 1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1 ]
|
||||
|
||||
point_filter_num: 10
|
||||
max_iteration: 3
|
||||
imu_has_gravity: false
|
||||
with_ui: false
|
||||
@@ -0,0 +1,23 @@
|
||||
common:
|
||||
dataset: "wxb"
|
||||
lid_topic: "points_raw"
|
||||
imu_topic: "imu_raw"
|
||||
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
|
||||
|
||||
preprocess:
|
||||
lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
|
||||
scan_line: 32
|
||||
time_scale: 1e-3 # 兼容不同数据集的时间单位，仅对Velodyne LiDAR(lidar_type=2)生效
|
||||
|
||||
mapping:
|
||||
extrinsic_T: [ 0, 0, 0 ]
|
||||
extrinsic_R: [ 1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1 ]
|
||||
|
||||
|
||||
point_filter_num: 10
|
||||
max_iteration: 3
|
||||
imu_has_gravity: false
|
||||
with_ui: false
|
||||
|
||||
BIN
workspace/slam_in_autonomous_driving/data/ch10/global.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch10/global.pcd
Normal file
Binary file not shown.
397894
workspace/slam_in_autonomous_driving/data/ch3/10.txt
Normal file
397894
workspace/slam_in_autonomous_driving/data/ch3/10.txt
Normal file
File diff suppressed because it is too large
Load Diff
222320
workspace/slam_in_autonomous_driving/data/ch3/11.txt
Normal file
222320
workspace/slam_in_autonomous_driving/data/ch3/11.txt
Normal file
File diff suppressed because it is too large
Load Diff
1763
workspace/slam_in_autonomous_driving/data/ch3/dr.txt
Normal file
1763
workspace/slam_in_autonomous_driving/data/ch3/dr.txt
Normal file
File diff suppressed because it is too large
Load Diff
892
workspace/slam_in_autonomous_driving/data/ch4/gins.txt
Normal file
892
workspace/slam_in_autonomous_driving/data/ch4/gins.txt
Normal file
@@ -0,0 +1,892 @@
|
||||
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426297.41856861 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426297.51855731 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426297.61857772 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426297.71855021 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426297.81854868 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426297.91858721 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.01854825 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.11854506 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.21854734 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.3185637 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.41855741 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.5185554 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.61854768 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.71856046 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.81854892 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426298.91856313 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.01855373 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.11854792 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.2185626 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.31856155 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.41854763 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.51855254 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.61856437 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.71856618 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.81854439 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426299.91855526 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.01856732 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.11854553 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.21855378 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.31856799 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.41856647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.51855326 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.61855817 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.71860671 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.81854749 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426300.91854763 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.01855397 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.11855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.21857858 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.31854963 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.41855764 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.51856136 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.61854291 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.71858835 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.81855226 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426301.91856337 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.01854563 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.11859298 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.21855092 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.31856084 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.41858602 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.51856112 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.61855888 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.71855569 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.81854916 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426302.91855097 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.0185535 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.11855578 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.21856213 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.31855488 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.4185605 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.5185492 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.61857009 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.71855688 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.81856275 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426303.91855979 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.01856065 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.11855245 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.21855068 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.31855774 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.41855001 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.51854563 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.6185689 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.71856403 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.81855226 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426304.9185648 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.0185473 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.1185503 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.21870112 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.31854892 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.41855574 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.51854491 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.61856389 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.71855688 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.81855607 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426305.91855741 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.01855731 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.11854911 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.21855927 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.31854749 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.41855431 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.51854849 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.61854887 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.71854234 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.81855059 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426306.91854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.01855469 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.1185503 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.2185452 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.31855083 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.4185555 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.51856112 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.61855888 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.71859765 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.81854773 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426307.91854692 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.01856184 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.1185503 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.21856332 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.31856537 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.41854572 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.51855135 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.61856461 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.71854663 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.8185513 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426308.91855311 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.01854658 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.11856651 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.2185688 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.31855154 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.4185586 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.51855874 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.61854839 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.71855593 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.8185513 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426309.91856074 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.01854396 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.1185596 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.21855903 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.31856441 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.41855264 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.51855731 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.6185627 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.7185545 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.81854963 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426310.91855669 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.01855111 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.11854815 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.21856189 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.31856799 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.41855311 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.51856184 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.61855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.71858001 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.81855226 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426311.91855145 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.01855111 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.11855459 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.21855497 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.31856227 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.41855383 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.51855183 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.61856174 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.71856618 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.81856489 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426312.9186008 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.0185523 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.11852789 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.21855664 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.31855798 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.41855335 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.51858568 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.61859655 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.71855688 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.81855106 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426313.91855955 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.01867414 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.11855602 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.21856809 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.3185513 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.41854835 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.51856136 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.61855149 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.71855998 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.81854749 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426314.91853499 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.01855278 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.11855245 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.21855855 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.31855297 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.41854858 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.51855493 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.61858511 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.71869206 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.81854486 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426315.91855383 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.01854706 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.11855102 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.21854925 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.31855631 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.41855574 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.51859736 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.61855125 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.71863365 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.81855559 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426316.91856599 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.01854849 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.11854529 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.21855021 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.31858039 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.41855001 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.51855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.6185658 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.71854806 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.81855083 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426317.9185555 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.01855159 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.11855149 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.21855712 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.31854916 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.41855192 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.51855803 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.61856222 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.71857047 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.81855702 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426318.91856647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.0185535 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.1185472 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.21855569 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.31855822 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.41863823 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.51856351 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.61855936 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.71856427 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.81856489 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426319.91855478 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.01854777 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.11854219 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.21856475 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.31855464 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.41855645 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.51854563 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.61855006 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.71856689 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.81854367 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426320.91856384 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.0185554 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.11863136 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.21854877 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.31859922 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.41922092 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.5185535 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.61860013 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.71926475 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.81854272 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426321.91855621 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.01856494 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.11855006 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.21855021 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.3185575 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.41855502 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.51860952 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.61855173 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.71856165 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.81863499 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426322.91854787 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.01855469 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.1185441 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.21854854 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.31855488 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.4185586 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.51855087 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.61855316 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.71854949 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.81855893 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426323.91855478 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.01858521 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.11855078 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.21857166 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.31855011 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.41855717 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.51847386 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.61855507 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.7185638 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.81854463 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426324.91859412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.01855707 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.11854839 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.21857023 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.3185544 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.41854787 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.51854849 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.61857271 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.71855855 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.8185451 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426325.91875482 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.01855922 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.11854029 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.21861172 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.31856465 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.41854787 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.51856041 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.61858654 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.71855497 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.81854773 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426326.91855955 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.01856565 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.11855364 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.21855402 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.31854892 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.41854572 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.51855445 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.6185565 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.71856833 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.81855917 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426327.91855145 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.01854944 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.11854935 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.21856976 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.31856537 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.41854644 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.51854682 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.61855078 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.71855736 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.81855345 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426328.91856217 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.01855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.11855483 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.21857023 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.31868935 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.4185462 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.51854992 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.61866689 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.7185576 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426329.91854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.0185585 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.11855936 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.21856022 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.31855893 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.41855597 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.51855278 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.61855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.71855235 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.81854749 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426330.91856527 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.01855302 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.11854553 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.21855116 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.31871295 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.41855621 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.51854992 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.61855245 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.71856165 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.81855559 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426331.91854382 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.01855469 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.11855102 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.2185514 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.31859636 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.41855454 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.51855278 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.61855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.71856761 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.81855798 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426332.91856003 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.01856875 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.11855459 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.21855736 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.31854916 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.41855478 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.51855898 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.61854982 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.7185607 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.81854486 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426333.9185493 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.01855302 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.11854935 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.21855044 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.31856132 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.4185586 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.51855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.61855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.71855187 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.81854796 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426334.91856647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.01855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.11855316 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.21854901 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.31855679 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.41856337 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.51856327 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.61854315 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.71856713 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.81855369 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426335.91855884 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.01856184 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.11854196 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.21855831 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.31859112 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.41866875 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.51856303 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.61857271 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.71863818 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.81855559 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426336.91855717 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.01855588 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.11854601 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.21855831 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.31854987 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.41855335 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.51855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.61855221 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.71857429 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.81855559 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426337.91854763 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.01855659 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.11854839 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.21855617 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.31856012 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.41862464 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.51855373 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.61855292 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.71856403 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.81854773 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426338.91854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.01855636 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.11855245 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.2185545 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.3185637 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.41855621 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.51855278 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.61856174 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.71855617 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.81854796 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426339.91856337 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.01856136 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.11854672 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.21855426 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.31856465 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.41855741 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.51854682 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.61855483 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.71856809 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.81854534 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426340.91858029 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.01856446 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.11854887 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.21855783 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.31856227 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.41856003 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.51855302 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.61855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.71856141 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.81855035 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426341.91855645 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.01855135 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.11855459 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.21856141 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.31855178 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.41854787 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.51855516 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.6185596 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.71855569 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.81854463 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426342.91855574 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.01856184 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.11855197 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.2185545 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.31855702 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.41855431 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.51856279 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.61856031 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.71856809 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.81867027 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426343.91855931 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.01856017 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.11855698 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.21855903 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.31859398 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.41854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.5185461 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.61856222 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.71904683 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.81854558 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426344.91859341 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.01856136 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.11854839 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.21856165 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.31856585 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.41856647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.5186615 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.6185596 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.7185533 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.81855512 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426345.91855288 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.01855803 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.11855412 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.21856713 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.31855226 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.41855812 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.51854992 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.61856675 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.71855378 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.81854725 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426346.91854572 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.0185585 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.11855221 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.21855521 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.31855178 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.41855931 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.51862788 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.61855626 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.71856236 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.81855416 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426347.91855049 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.01855969 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.11855006 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.21856976 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.31856322 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.41854835 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.51855588 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.61855364 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.7185514 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.8185482 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426348.91855049 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.01854992 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.11856389 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.21855593 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.31856394 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.41855836 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.51857424 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.61859012 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.71860647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.81854415 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426349.91855311 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.01854944 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.11855316 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.21856332 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.31854987 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.41855645 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.5185554 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.61856365 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.71854591 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.81854749 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426350.91867995 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.01859403 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.11854672 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.21855187 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.31855702 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.41854501 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.51855469 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.61883068 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.71855617 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.81854558 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426351.91856098 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.01855826 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.11855507 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.21865439 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.31855345 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.41855288 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.51855636 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.61857724 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.7185545 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.81854463 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426352.91856647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.01856351 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.11854982 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.21854591 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.31859183 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.41856599 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.51855278 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.61856079 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.71856833 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.8185575 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426353.9185586 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.01855946 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.11855197 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.21858573 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.31854939 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.41856408 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.51855445 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.61855125 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.71855426 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.81855488 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426354.91857147 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.01857066 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.11855054 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.21856165 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.31855631 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.41854978 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.51855493 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.61856461 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.71855092 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.81855249 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426355.91859174 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.01856661 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.1185503 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.21854973 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.31855059 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.41855693 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.51855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.61855483 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.71856308 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.81855488 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426356.91855145 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.01855421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.11855197 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.21857595 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.31854749 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.41855717 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.51855063 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.61857462 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.7185638 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.81855106 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426357.91855979 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.01855612 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.11855698 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.21855402 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.31859875 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.41855836 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.51854897 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.6185534 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.7185638 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.81854939 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426358.91854906 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.01856112 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.11854982 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.21855545 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.31855488 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.41855478 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.51855779 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.6185534 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.71856713 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.81855369 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426359.91855836 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.01855111 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.1185472 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.21856236 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.31854868 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.41855788 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.51855206 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.61854887 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.71856451 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.81855345 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426360.91854882 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.01856041 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.11855936 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.21855712 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.31856132 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.41856337 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.51855111 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.61855912 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.71858287 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.81854224 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426361.91858959 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.01855898 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.11854267 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.21856284 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.31856132 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.41854906 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.51856685 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.61854553 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.71856356 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.81854534 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426362.91855121 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.01860404 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.11862469 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.2185514 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.3185544 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.41860628 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.51854849 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.61855245 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.71855164 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.81882358 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426363.91855311 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.01855397 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.11854672 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.2185955 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.31856561 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.41854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.51855516 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.61855626 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.71855545 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.81861258 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426364.91855145 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.01855612 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.11857772 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.2185607 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.3186028 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.4186058 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.51855135 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.61855507 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.71856785 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.81855226 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426365.91856098 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.01854801 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.11855578 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.21855998 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.31857276 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.41854739 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.51854968 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.61856246 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.71855164 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.81859517 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426366.91856432 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.01854873 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.11856437 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.21855974 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.31856012 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.41855526 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.51855111 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.6185894 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.71854472 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.8185432 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426367.91856122 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.0185349 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.11853671 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.2185483 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.31854463 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.41854525 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.51864409 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.61855292 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.71856165 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.8185389 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426368.91857791 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.01856041 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.11853957 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.21854734 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.31854296 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.41854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.51855636 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.61855125 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.71855092 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.81854701 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426369.91867042 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.01858425 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.11854553 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.21857929 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.31854725 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.41854906 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.5185535 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.61854744 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.71854782 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.81854486 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426370.91855645 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.01855302 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.11853552 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.21856117 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.31856132 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.41853929 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.51856351 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.61855984 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.71855426 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.81854296 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426371.91860223 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.01856399 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.11853671 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.21855378 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.31854582 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.41855121 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.51853585 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.61859322 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.71854854 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.81853557 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426372.91855764 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.01853395 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.11854625 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.21855354 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.31854558 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.41854453 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.51854086 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.61861444 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.71853971 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.8185482 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426373.91854811 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.01854467 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.11854696 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.21853638 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.31853914 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.41856647 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.51854062 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.61854362 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.71855307 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.81853628 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426374.91855359 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.01860213 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.11854076 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.21855998 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.31856036 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.41853666 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.51855016 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.61856508 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.71855593 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.81854296 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426375.91855049 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.01854134 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.11853242 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.21854353 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.31854987 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.41856289 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.51854229 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.6185894 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.71854305 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.8185358 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426376.91854644 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.01854277 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.118541 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.21854424 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.31857657 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.41853714 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.51853752 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.61854482 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.71855617 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.81853938 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426377.91855025 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.01854229 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.11853838 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.21854472 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.31854057 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.41863561 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.51853681 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.61853743 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.71854901 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.81854486 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426378.91854215 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.0185523 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.11853576 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.2185421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.31853247 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.41854405 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.51854873 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.6185329 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.71854115 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.81854248 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426379.91853881 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.01853871 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.11854458 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.2185421 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.31853676 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.41854191 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.51854277 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.61854577 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.71854997 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.8185358 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426380.91853642 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.01854539 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.11854386 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.21854186 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.31853032 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.41855454 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.51854014 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.61854076 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.71855807 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.81853437 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426381.91854286 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.0185504 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.11854362 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.21854687 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.31854057 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.4186058 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.51854825 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.6185441 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.71853685 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.81854057 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426382.91854453 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.01854277 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.11854076 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.21854591 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.31854296 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.41853595 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.51853752 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.61855292 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.71854401 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.81853437 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426383.91853595 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.01853657 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.11853576 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.21854115 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.31855249 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.41854906 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.51854014 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.61856055 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.71854377 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.81855416 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426384.91855121 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.01853657 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.11854577 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.21854925 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.31853652 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.41854119 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.51855206 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.61854172 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.71854877 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.81853604 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426385.91853547 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426386.018538 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426386.11854219 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426386.21853709 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426386.31855249 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426386.41854024 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
|
||||
1624426386.51855063 455342.536 4413646.58 37.7136002 -0.972287965 0 0 0.233786468 0 0 0 0 0 0 0 0 0
|
||||
BIN
workspace/slam_in_autonomous_driving/data/ch5/first.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch5/first.pcd
Normal file
Binary file not shown.
BIN
workspace/slam_in_autonomous_driving/data/ch5/map_example.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch5/map_example.pcd
Normal file
Binary file not shown.
BIN
workspace/slam_in_autonomous_driving/data/ch5/scan_example.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch5/scan_example.pcd
Normal file
Binary file not shown.
BIN
workspace/slam_in_autonomous_driving/data/ch5/second.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch5/second.pcd
Normal file
Binary file not shown.
@@ -0,0 +1 @@
|
||||
-0.0699018 -0.101214 1.02861e-07 0.999033 0.0323435 -0.0136968 -0.0264372
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1 @@
|
||||
-0.0699018 -0.101214 1.02861e-07 0.999033 0.0323435 -0.0136968 -0.0264372
|
||||
Binary file not shown.
Binary file not shown.
BIN
workspace/slam_in_autonomous_driving/data/ch8/after_undist.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch8/after_undist.pcd
Normal file
Binary file not shown.
BIN
workspace/slam_in_autonomous_driving/data/ch8/before_undist.pcd
Normal file
BIN
workspace/slam_in_autonomous_driving/data/ch8/before_undist.pcd
Normal file
Binary file not shown.
205
workspace/slam_in_autonomous_driving/data/log_lio_iekf_nclt.txt
Normal file
205
workspace/slam_in_autonomous_driving/data/log_lio_iekf_nclt.txt
Normal file
@@ -0,0 +1,205 @@
|
||||
imu init done, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
|
||||
=== frame 0
|
||||
I1118 17:14:25.421314 209159 lio_iekf.cc:137] pred: p: 04.90454e-05 -0.000504891 08.02587e-05, v: 0.000440792 -0.00511598 0.000799364, q: 0-0.00120653 -7.59703e-05 -9.70547e-06 00000.999999, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:07.768869 209159 lio_iekf.cc:155] post: p: -0.0153191 00.0503512 -0.0341755, v: 0.000479927 00.00466279 -0.00575974, q: -0.00121657 00.00206976 00.00160879 0000.999996, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:07.775338 209159 lio_iekf.cc:136] === frame 1
|
||||
I1118 17:15:07.775344 209159 lio_iekf.cc:137] pred: p: -0.0150588 00.0506864 -0.0347105, v: 0.00477512 0.00214923 -0.0051144, q: 0.000245399 00.00217255 00.00190123 0000.999996, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.890256 209159 lio_iekf.cc:155] post: p: -0.030806 0.0386739 00.021142, v: -0.0501599 -0.0260004 00.0399524, q: -0.000261551 000.00225525 000.00119186 00000.999997, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.893633 209159 lio_iekf.cc:136] === frame 2
|
||||
I1118 17:15:11.893646 209159 lio_iekf.cc:137] pred: p: -0.035667 0.0359853 0.0252403, v: -0.0455922 -0.0274532 000.040713, q: 0.00225679 0.00228433 0.00164527 000.999993, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.894618 209159 lio_iekf.cc:155] post: p: -0.0277568 00.0270789 00.0295162, v: -0.0105089 -0.0578568 00.0589583, q: 00.00219828 00.00260048 0.000873066 0000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.899643 209159 lio_iekf.cc:136] === frame 3
|
||||
I1118 17:15:11.899652 209159 lio_iekf.cc:137] pred: p: -0.0285529 00.0211308 00.0354642, v: -0.00535027 0-0.0607453 000.0594799, q: 0.00259265 0.00254714 0.00105252 000.999993, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.901063 209159 lio_iekf.cc:155] post: p: 00.00100406 -0.00416539 000.0161755, v: 000.0956111 00-0.156076 -0.00129277, q: 000.00738294 000.00131746 -0.000780396 00000.999972, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.904808 209159 lio_iekf.cc:136] === frame 4
|
||||
I1118 17:15:11.904814 209159 lio_iekf.cc:137] pred: p: 00.0105809 -0.0201856 00.0160716, v: 0000.0983173 000-0.168396 -0.000867878, q: 000.00823418 000.00128613 -0.000717163 00000.999965, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.906029 209159 lio_iekf.cc:155] post: p: 0.00843266 0-0.014723 00.0206042, v: 0.0923196 -0.152179 0.0138562, q: 000.00789518 000.00152049 -0.000322546 00000.999968, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.908725 209159 lio_iekf.cc:136] === frame 5
|
||||
I1118 17:15:11.908728 209159 lio_iekf.cc:137] pred: p: 00.0178833 -0.0306593 00.0220171, v: 0.0955118 -0.164659 0.0141552, q: 000.00763498 000.00160957 -0.000344936 00000.999969, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.909430 209159 lio_iekf.cc:155] post: p: -0.00470344 00.00639892 000.0288955, v: 00.0365583 -0.0654695 000.033711, q: 000.00457161 000.00261937 -0.000129411 00000.999986, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.912588 209159 lio_iekf.cc:136] === frame 6
|
||||
I1118 17:15:11.912595 209159 lio_iekf.cc:137] pred: p: -0.000848782 -0.000381971 0000.0322354, v: 00.0416474 -0.0721619 000.034013, q: 000.00303829 000.00259847 -0.000513327 00000.999992, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.912993 209159 lio_iekf.cc:155] post: p: -0.0108165 00.0115985 00.0430028, v: 00.0178083 -0.0410502 00.0622496, q: 00.00150854 00.00396589 0.000674097 0000.999991, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.914862 209159 lio_iekf.cc:136] === frame 7
|
||||
I1118 17:15:11.914866 209159 lio_iekf.cc:137] pred: p: -0.00864492 000.0073022 000.0492498, v: 00.0255669 -0.0447473 000.062585, q: -0.00036176 00.00389803 0.000664088 0000.999992, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.915254 209159 lio_iekf.cc:155] post: p: -0.00998406 000.0018454 000.0376213, v: 00.0202328 -0.0594128 00.0338251, q: 03.33809e-06 000.00321527 -2.83811e-05 00000.999995, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.917377 209159 lio_iekf.cc:136] === frame 8
|
||||
I1118 17:15:11.917410 209159 lio_iekf.cc:137] pred: p: -0.00759479 -0.00440334 000.0410847, v: 00.0268193 -0.0636386 00.0343541, q: 00.000523725 000.00313919 -0.000204003 00000.999995, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.918063 209159 lio_iekf.cc:155] post: p: 0.000752396 000.0207922 000.0334292, v: 00.0477717 -0.0110044 00.0142291, q: 000.00227905 000.00266603 -5.74178e-05 00000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.919911 209159 lio_iekf.cc:136] === frame 9
|
||||
I1118 17:15:11.919914 209159 lio_iekf.cc:137] pred: p: 0.00570258 00.0193502 00.0348469, v: 00.0532229 -0.0184184 00.0146345, q: 000.00206386 000.00261745 -7.93742e-06 00000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.920490 209159 lio_iekf.cc:155] post: p: 0.00191145 00.0201858 00.0180808, v: 00.0400973 -0.0187426 -0.0308192, q: 00.00149519 00.00122873 0.000768057 0000.999998, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.922688 209159 lio_iekf.cc:136] === frame 10
|
||||
I1118 17:15:11.922690 209159 lio_iekf.cc:137] pred: p: 0.00649512 00.0177244 00.0147163, v: 00.0431613 0-0.025969 -0.0303075, q: 00.00203652 000.0013238 0.000903168 0000.999997, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.923228 209159 lio_iekf.cc:155] post: p: -0.00150556 0-0.0025184 -0.00414911, v: 00.0155007 -0.0860391 -0.0822575, q: 000.00599938 -0.000183419 00.000153602 00000.999982, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.924659 209159 lio_iekf.cc:136] === frame 11
|
||||
I1118 17:15:11.924662 209159 lio_iekf.cc:137] pred: p: -0.000104462 00-0.0109358 00-0.0116132, v: 00.0153269 -0.0991016 -0.0819059, q: 000.00708928 -0.000185432 00.000385193 00000.999975, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.924885 209159 lio_iekf.cc:155] post: p: 00.00224026 0-0.0206669 0.000693994, v: 00.0241425 00-0.11959 -0.0453051, q: 00.00488904 0.000805058 -0.00062767 0000.999988, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.926889 209159 lio_iekf.cc:136] === frame 12
|
||||
I1118 17:15:11.926892 209159 lio_iekf.cc:137] pred: p: 00.00499685 0-0.0344078 -0.00424487, v: 00.0260986 0-0.130902 -0.0447542, q: 0000.0064845 00.000871289 -0.000509421 00000.999978, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.927434 209159 lio_iekf.cc:155] post: p: -0.0013856 -0.0180147 0.00077609, v: 0.00733519 -0.0920896 -0.0286954, q: 000.00923425 00.000369769 -0.000686988 00000.999957, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.929165 209159 lio_iekf.cc:136] === frame 13
|
||||
I1118 17:15:11.929169 209159 lio_iekf.cc:137] pred: p: -0.000683904 00-0.0270661 0-0.00182275, v: 00.0080873 0-0.106958 -0.0285007, q: 000.00924388 00.000358166 -0.000710999 00000.999957, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.929667 209159 lio_iekf.cc:155] post: p: -0.00578867 00.00215956 00.00700372, v: 0-0.0030371 0-0.0229413 -0.00447093, q: 00.00563486 00.00243229 5.64186e-05 0000.999981, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.931541 209159 lio_iekf.cc:136] === frame 14
|
||||
I1118 17:15:11.931545 209159 lio_iekf.cc:137] pred: p: 0-0.00583285 -0.000895607 000.00654662, v: 00.00220287 0-0.0335465 -0.00401146, q: 00.00605858 00.00232979 2.20827e-05 0000.999979, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.932104 209159 lio_iekf.cc:155] post: p: -0.00278004 00.00313029 000.0128976, v: 0.00997104 -0.0254463 00.0132513, q: 00.00821921 00.00118624 0.000199534 0000.999965, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.933557 209159 lio_iekf.cc:136] === frame 15
|
||||
I1118 17:15:11.933580 209159 lio_iekf.cc:137] pred: p: 0-0.00164956 -0.000153588 0000.0142476, v: 00.0125478 -0.0399885 00.0135861, q: 00.00860753 00.00122725 0.000401781 0000.999962, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.933826 209159 lio_iekf.cc:155] post: p: 00.00919599 -0.00954942 000.0229229, v: 000.045598 -0.0595688 00.0382537, q: 0.00620488 0.00203302 0.00163873 000.999977, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.935776 209159 lio_iekf.cc:136] === frame 16
|
||||
I1118 17:15:11.935781 209159 lio_iekf.cc:137] pred: p: 00.0136039 -0.0155058 00.0264916, v: 0.0494435 -0.068921 0.0386612, q: 0.00690578 0.00203102 0.00175541 000.999973, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.936312 209159 lio_iekf.cc:155] post: p: 000.0101917 0.000459489 0000.019997, v: 00.0386058 -0.0351932 00.0207056, q: 000.0101891 00.00142608 0.000216748 0000.999947, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.938040 209159 lio_iekf.cc:136] === frame 17
|
||||
I1118 17:15:11.938045 209159 lio_iekf.cc:137] pred: p: 000.0144818 -0.00425185 000.0222271, v: 00.0417763 -0.0531204 00.0210255, q: 000.0106075 00.00139463 -6.4051e-05 0000.999943, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.938546 209159 lio_iekf.cc:155] post: p: -0.00199832 -0.00755048 000.0180761, v: -0.00519676 0-0.0611281 00.00960466, q: 00.00943818 00.00015313 -0.00162587 0000.999954, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.940439 209159 lio_iekf.cc:136] === frame 18
|
||||
I1118 17:15:11.940443 209159 lio_iekf.cc:137] pred: p: -0.00249975 0-0.0144568 000.0190582, v: -0.00474474 0-0.0758661 00.00980882, q: 00.00890815 0.000114341 0-0.0015747 0000.999959, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.940969 209159 lio_iekf.cc:155] post: p: -0.0150766 0000.01334 00.0178194, v: 0-0.0374328 -0.00465068 00.00703531, q: 000.00740015 000.00137108 -0.000344167 00000.999972, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.942432 209159 lio_iekf.cc:136] === frame 19
|
||||
I1118 17:15:11.942435 209159 lio_iekf.cc:137] pred: p: -0.0185532 00.0123449 00.0185101, v: -0.0346348 0-0.015953 0.00724182, q: 000.00627905 000.00135494 -0.000248159 00000.999979, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.942687 209159 lio_iekf.cc:155] post: p: -0.0106226 0.00490207 00.0236433, v: -0.0107037 -0.0278636 00.0203764, q: 0.00249327 0.00215172 0.00130186 000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.944468 209159 lio_iekf.cc:136] === frame 20
|
||||
I1118 17:15:11.944471 209159 lio_iekf.cc:137] pred: p: -0.0114897 0.00179978 00.0257821, v: -0.00602089 00-0.031967 000.0208662, q: 0.00312941 0.00215868 0.00132527 000.999992, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.944958 209159 lio_iekf.cc:155] post: p: -0.00345185 -0.00717943 000.0165726, v: 000.0135267 0-0.0619038 -0.00248835, q: 000.00715173 000.00140423 -0.000189458 00000.999973, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.946609 209159 lio_iekf.cc:136] === frame 21
|
||||
I1118 17:15:11.946614 209159 lio_iekf.cc:137] pred: p: -0.00211647 0-0.0132064 000.0163657, v: 000.0162605 0-0.0725589 -0.00216985, q: 000.00740939 000.00137473 -0.000170376 00000.999972, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.947124 209159 lio_iekf.cc:155] post: p: -0.00651473 -0.00215236 000.0153109, v: 00.00488036 0-0.0455272 -0.00529396, q: 00.00698527 00.00175434 0.000569922 0000.999974, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.948858 209159 lio_iekf.cc:136] === frame 22
|
||||
I1118 17:15:11.948877 209159 lio_iekf.cc:137] pred: p: -0.00574653 -0.00782332 000.0147551, v: 00.00909847 0-0.0576614 -0.00485595, q: 000.0076439 00.00177245 0.000680381 0000.999969, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.949421 209159 lio_iekf.cc:155] post: p: -0.00382868 000.0130476 0000.017737, v: 000.0149043 -0.00876365 00.00267306, q: 00.00969612 00.00157242 0.000840327 0000.999951, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.951212 209159 lio_iekf.cc:136] === frame 23
|
||||
I1118 17:15:11.951216 209159 lio_iekf.cc:137] pred: p: -0.00227757 000.0115498 000.0180063, v: 00.0180914 -0.0231501 0.00300576, q: 00000.01088 000.0015886 0.000719275 0000.999939, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.951570 209159 lio_iekf.cc:155] post: p: -0.0145005 00.0174192 00.0175176, v: 0-0.0158562 -0.00520393 00.00199439, q: 00.00838138 00.00142165 -0.00218195 0000.999961, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.953176 209159 lio_iekf.cc:136] === frame 24
|
||||
I1118 17:15:11.953179 209159 lio_iekf.cc:137] pred: p: -0.016024 0.0162531 0.0177543, v: 0-0.012738 -0.0167324 0.00236756, q: 00.00892732 00.00137161 -0.00163546 0000.999958, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.953544 209159 lio_iekf.cc:155] post: p: 00-0.017527 0.000705961 000.0373498, v: -0.0126885 -0.0465573 00.0543639, q: 000.00589359 000.00350919 -0.000831264 00000.999976, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.955387 209159 lio_iekf.cc:136] === frame 25
|
||||
I1118 17:15:11.955391 209159 lio_iekf.cc:137] pred: p: 0-0.0183067 -0.00314154 000.0416631, v: -0.00701791 0-0.0507588 000.0546663, q: 00.00594996 00.00350621 -0.00100196 0000.999976, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.955890 209159 lio_iekf.cc:155] post: p: -0.0147757 0.00129334 00.0224162, v: -0.00105843 0-0.0430689 00.00257092, q: 000.00602707 000.00175077 -0.000476096 000000.99998, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.957621 209159 lio_iekf.cc:136] === frame 26
|
||||
I1118 17:15:11.957624 209159 lio_iekf.cc:137] pred: p: 0-0.0146316 -0.00430057 000.0227596, v: 0.00346116 -0.0499088 0.00308568, q: 000.00660257 000.00179712 -0.000478638 00000.999976, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.958143 209159 lio_iekf.cc:155] post: p: -0.00955999 000.0164956 000.0221032, v: 00000.018325 -0.000787753 00.000941459, q: 0.00804765 0.00199508 0.00148244 000.999965, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.960180 209159 lio_iekf.cc:136] === frame 27
|
||||
I1118 17:15:11.960184 209159 lio_iekf.cc:137] pred: p: -0.00752655 000.0159748 000.0222158, v: 000.0224374 -0.00968724 00.00125442, q: 0.00751929 0.00198793 0.00124661 000.999969, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.960719 209159 lio_iekf.cc:155] post: p: -0.00960678 0000.023717 000.0129138, v: 00.0149583 0.00793828 -0.0244125, q: 0.00745896 0.00091034 0.00116203 000.999971, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.962118 209159 lio_iekf.cc:136] === frame 28
|
||||
I1118 17:15:11.962121 209159 lio_iekf.cc:137] pred: p: -0.00802248 000.0240688 000.0104961, v: 0000.0168653 -0.000846577 00-0.0241823, q: 00.00610736 0.000903238 00.00135748 00000.99998, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.962345 209159 lio_iekf.cc:155] post: p: -0.013008 0.0198995 0.0130335, v: 00.00317311 -0.00774547 0-0.0173752, q: 000.00382281 000.00123227 -0.000341855 00000.999992, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.964265 209159 lio_iekf.cc:136] === frame 29
|
||||
I1118 17:15:11.964282 209159 lio_iekf.cc:137] pred: p: -0.0126156 00.0190335 00.0114875, v: 0.00553383 0-0.011468 -0.0170193, q: 000.00397387 000.00114611 -0.000728046 00000.999991, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.964799 209159 lio_iekf.cc:155] post: p: -0.0204661 0.00885791 00.0107593, v: -0.0168295 -0.0401255 -0.0182122, q: 00.00567619 000.0011173 -0.00140513 0000.999982, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.966563 209159 lio_iekf.cc:136] === frame 30
|
||||
I1118 17:15:11.966567 209159 lio_iekf.cc:137] pred: p: -0.0221681 0.00400176 0.00878148, v: -0.0141208 -0.0482997 -0.0178408, q: 00.00524587 00.00125018 -0.00112132 0000.999985, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.967144 209159 lio_iekf.cc:155] post: p: -0.0308646 00.0273728 0.00321953, v: -0.0392401 0.00365444 0-0.032642, q: 000.00804325 04.84804e-05 -9.67534e-05 00000.999968, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.969038 209159 lio_iekf.cc:136] === frame 31
|
||||
I1118 17:15:11.969040 209159 lio_iekf.cc:137] pred: p: 00-0.0348125 0000.0270795 -5.76973e-05, v: 0-0.0390182 -0.00944722 0-0.0323957, q: 000.00806638 000.00010777 -0.000151537 00000.999967, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.969590 209159 lio_iekf.cc:155] post: p: -0.0275045 00.0168235 00.0158835, v: -0.0159905 -0.0285702 00.0117468, q: 00.00628074 00.00183553 -0.00171256 0000.999977, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.971078 209159 lio_iekf.cc:136] === frame 32
|
||||
I1118 17:15:11.971081 209159 lio_iekf.cc:137] pred: p: -0.0289343 00.0134623 00.0170917, v: -0.0123124 -0.0380222 00.0121408, q: 00.00663759 00.00190125 -0.00125578 0000.999975, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.971312 209159 lio_iekf.cc:155] post: p: -0.0335291 000.011422 00.0223288, v: -0.0248202 -0.0431123 00.0269799, q: 00.00706802 00.00210039 -0.00271434 0000.999969, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.973183 209159 lio_iekf.cc:136] === frame 33
|
||||
I1118 17:15:11.973186 209159 lio_iekf.cc:137] pred: p: -0.0353633 0.00768912 00.0244735, v: -0.0216154 -0.0514039 00.0272669, q: 00.00723896 00.00212892 -0.00285546 0000.999967, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.973711 209159 lio_iekf.cc:155] post: p: -0.0406646 0.00223471 00.0242756, v: 0-0.036042 -0.0627385 00.0266659, q: 0.00573758 0.00235501 -0.0025138 000.999978, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.975383 209159 lio_iekf.cc:136] === frame 34
|
||||
I1118 17:15:11.975385 209159 lio_iekf.cc:137] pred: p: 0-0.0446373 -0.00579038 000.0274872, v: -0.0306631 -0.0720615 00.0272026, q: 000.0065722 00.00245001 -0.00270113 0000.999972, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.975878 209159 lio_iekf.cc:155] post: p: 0-0.032354 0.00698154 00.0176309, v: -0.000250842 00-0.0473594 000.00104029, q: 000.0099747 00.00066549 -0.00199537 0000.999948, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.977705 209159 lio_iekf.cc:136] === frame 35
|
||||
I1118 17:15:11.977710 209159 lio_iekf.cc:137] pred: p: -0.0323301 0.00138484 00.0177552, v: 0.000730281 0-0.0625483 000.0013314, q: 000.0103759 0.000646453 -0.00159964 0000.999945, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.978232 209159 lio_iekf.cc:155] post: p: -0.0337782 0.00874701 00.0137585, v: -0.00282612 00-0.047824 -0.00861813, q: 00.0121455 0.00113379 -0.0016715 000.999924, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.979840 209159 lio_iekf.cc:136] === frame 36
|
||||
I1118 17:15:11.979858 209159 lio_iekf.cc:137] pred: p: -0.0339622 0.00314178 00.0129249, v: -0.000910109 00-0.0662699 0-0.00838114, q: 00.0127904 0.00112578 -0.0016166 000.999916, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.980145 209159 lio_iekf.cc:155] post: p: -0.0470016 00.0168451 00.0147551, v: 0-0.0365023 0-0.0275621 -0.00325529, q: 00.00927839 00.00127644 -0.00159512 0000.999955, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.981890 209159 lio_iekf.cc:136] === frame 37
|
||||
I1118 17:15:11.981894 209159 lio_iekf.cc:137] pred: p: -0.0507362 000.013244 00.0144328, v: 0-0.0340429 0-0.0405466 -0.00290648, q: 0.00998882 0.00136084 -0.0015252 000.999948, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.982342 209159 lio_iekf.cc:155] post: p: -0.0253281 00.0107317 00.0289837, v: 0.0373826 -0.043742 0.0349919, q: 0000.0106274 000.00246312 -2.56926e-05 000000.99994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.984136 209159 lio_iekf.cc:136] === frame 38
|
||||
I1118 17:15:11.984139 209159 lio_iekf.cc:137] pred: p: -0.0216145 00.0060108 000.032282, v: 00.0417066 -0.0568265 00.0351998, q: 0000.0100201 000.00245466 -0.000286008 00000.999947, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.984676 209159 lio_iekf.cc:155] post: p: -0.0224169 00.0115669 00.0327741, v: 00.0402784 -0.0394379 00.0357205, q: 000.00776671 0000.0027215 -0.000371586 00000.999966, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.986378 209159 lio_iekf.cc:136] === frame 39
|
||||
I1118 17:15:11.986382 209159 lio_iekf.cc:137] pred: p: -0.0181039 0.00710886 00.0363843, v: 00.0453899 -0.0491096 00.0358994, q: 000.00627731 000.00267927 -0.000191602 00000.999977, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.986899 209159 lio_iekf.cc:155] post: p: -0.0072182 00.0117463 00.0271725, v: 00.073863 -0.040899 0.0106082, q: 00.00807588 00.00184913 0.000308965 0000.999966, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.988760 209159 lio_iekf.cc:136] === frame 40
|
||||
I1118 17:15:11.988763 209159 lio_iekf.cc:137] pred: p: 0.000428815 00.00693913 000.0282592, v: 0.0773209 -0.054111 0.0108618, q: 00.00714625 00.00178196 0.000468223 0000.999973, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.989274 209159 lio_iekf.cc:155] post: p: -0.00274461 -0.00475901 000.0343976, v: 00.0697324 -0.0743786 00.0272192, q: 000.0017629 00.00290237 0.000580373 0000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.990716 209159 lio_iekf.cc:136] === frame 41
|
||||
I1118 17:15:11.990720 209159 lio_iekf.cc:137] pred: p: 0.00512994 -0.0129419 00.0373723, v: 00.0757732 -0.0768013 00.0276777, q: 00.00176315 00.00295099 0.000712146 0000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.990968 209159 lio_iekf.cc:155] post: p: 0.00382828 -0.0119629 00.0286474, v: 0000.07057 -0.0802147 0.00467209, q: 00.00465419 00.00240829 0.000473393 0000.999986, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.992880 209159 lio_iekf.cc:136] === frame 42
|
||||
I1118 17:15:11.992884 209159 lio_iekf.cc:137] pred: p: 00.0111183 -0.0203906 00.0291311, v: 00.0751165 -0.0882057 0.00495365, q: 00.00368231 00.00244324 0.000759635 00000.99999, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.993322 209159 lio_iekf.cc:155] post: p: 00.0142869 -0.0046058 00.0252783, v: 000.0836109 0-0.0527855 -0.00587547, q: 0.00539567 00.0022904 0.00113727 000.999982, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.995007 209159 lio_iekf.cc:136] === frame 43
|
||||
I1118 17:15:11.995010 209159 lio_iekf.cc:137] pred: p: 00.0229552 -0.0105195 00.0247032, v: 000.0879199 0-0.0642372 -0.00554224, q: 0.00516391 0.00231061 0.00124638 000.999983, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.995541 209159 lio_iekf.cc:155] post: p: -0.00125801 00.00602515 0000.029749, v: 00.0240558 -0.0155837 0.00774458, q: -0.000774922 000.00317774 00.000926218 00000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.997438 209159 lio_iekf.cc:136] === frame 44
|
||||
I1118 17:15:11.997442 209159 lio_iekf.cc:137] pred: p: 0.00145917 00.0044732 00.0305545, v: 00.0300416 0-0.015309 0.00828491, q: 0.000186713 00.00325627 0.000800919 0000.999994, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.997939 209159 lio_iekf.cc:155] post: p: 0.00409701 -0.0103695 00.0280371, v: 00.0348392 -0.0608119 0.00215272, q: 000.00559483 000.00244804 -0.000129847 00000.999981, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:11.999414 209159 lio_iekf.cc:136] === frame 45
|
||||
I1118 17:15:11.999418 209159 lio_iekf.cc:137] pred: p: 0.00761241 -0.0166219 00.0282709, v: 00.0390343 0-0.070634 0.00270157, q: 00.00832741 00.00252936 0.000170725 0000.999962, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:11.999653 209159 lio_iekf.cc:155] post: p: 0.00852682 -0.0219793 00.0325859, v: 00.0422356 -0.0823777 00.0143882, q: 00.00764677 00.00296251 -0.00063212 0000.999966, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:12.001551 209159 lio_iekf.cc:136] === frame 46
|
||||
I1118 17:15:12.001555 209159 lio_iekf.cc:137] pred: p: 00.0131904 -0.0310293 00.0341103, v: 00.0477363 -0.0924026 00.0149382, q: 000.00978727 000.00303871 -0.000357891 00000.999947, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:12.002097 209159 lio_iekf.cc:155] post: p: -0.00571204 -0.00826453 0000.017529, v: -0.00684843 0-0.0381937 0-0.0281482, q: 00.00882581 00.00140222 0.000619265 00000.99996, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:12.003772 209159 lio_iekf.cc:136] === frame 47
|
||||
I1118 17:15:12.003774 209159 lio_iekf.cc:137] pred: p: -0.00624681 0-0.0120631 0000.015004, v: -0.00501371 0-0.0460501 0-0.0278583, q: 00.00944439 00.00136209 0.000965105 0000.999954, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:12.004245 209159 lio_iekf.cc:155] post: p: -0.00217722 0.000370151 00.00765073, v: 0.00408428 -0.0184988 -0.0479597, q: 0000.0110169 -0.000504146 000.00151735 00000.999938, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:12.006052 209159 lio_iekf.cc:136] === frame 48
|
||||
I1118 17:15:12.006054 209159 lio_iekf.cc:137] pred: p: -0.00182268 0-0.0023869 00.00241462, v: 0.00241319 -0.0318412 -0.0475872, q: 00.0125397 -0.0004074 0.00128603 0000.99992, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:12.006580 209159 lio_iekf.cc:155] post: p: 00.00712729 0.000455928 000.0108929, v: 00.0305433 -0.0181527 0-0.024302, q: 0.00981948 0.00130322 0.00107983 0000.99995, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:12.008213 209159 lio_iekf.cc:136] === frame 49
|
||||
I1118 17:15:12.008215 209159 lio_iekf.cc:137] pred: p: 00.00994338 -0.00146657 00.00872675, v: 00.0322507 -0.0247646 -0.0240732, q: 00.00928355 00.00125963 0.000998656 0000.999956, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:12.008530 209159 lio_iekf.cc:155] post: p: 000.0147639 2.52168e-05 000.0175967, v: 000.0469827 00-0.017585 0.000277126, q: 00.00804375 00.00144093 0.000434819 0000.999967, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
Failed to find match for field 'intensity'.
|
||||
I1118 17:15:12.010309 209159 lio_iekf.cc:136] === frame 50
|
||||
I1118 17:15:12.010313 209159 lio_iekf.cc:137] pred: p: 000.0201475 -0.00226075 000.0176451, v: 000.0493982 0-0.0233594 0.000536465, q: 000.0062863 000.0013803 0.000230497 0000.999979, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
I1118 17:15:12.010778 209159 lio_iekf.cc:155] post: p: 00.0148923 -0.0203415 000.016958, v: 0000.0345795 00-0.0711472 -5.14172e-05, q: 00.00817267 00.00122795 6.93271e-05 0000.999966, bg: 0.000293669 00.00302745 -0.00353565, ba: 6.72201e-05 2.65046e-06 -0.00493014
|
||||
BIN
workspace/slam_in_autonomous_driving/doc/2dmapping_demo.gif
Normal file
BIN
workspace/slam_in_autonomous_driving/doc/2dmapping_demo.gif
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 9.0 MiB |
BIN
workspace/slam_in_autonomous_driving/doc/lio_demo.gif
Normal file
BIN
workspace/slam_in_autonomous_driving/doc/lio_demo.gif
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 32 MiB |
BIN
workspace/slam_in_autonomous_driving/doc/lo_demo.gif
Normal file
BIN
workspace/slam_in_autonomous_driving/doc/lo_demo.gif
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 28 MiB |
8
workspace/slam_in_autonomous_driving/docker/docker_run.sh
Executable file
8
workspace/slam_in_autonomous_driving/docker/docker_run.sh
Executable file
@@ -0,0 +1,8 @@
|
||||
echo "please make sure running this script in root dir of slam_in_autonomous_driving. current working dir: $PWD"
|
||||
docker run -it \
|
||||
-e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix \
|
||||
-v "$PWD":/sad \
|
||||
--gpus all \
|
||||
--name sad \
|
||||
sad:v1 \
|
||||
/bin/bash
|
||||
17
workspace/slam_in_autonomous_driving/docker/sources.list
Normal file
17
workspace/slam_in_autonomous_driving/docker/sources.list
Normal file
@@ -0,0 +1,17 @@
|
||||
# 默认注释了源码镜像以提高 apt update 速度，如有需要可自行取消注释
|
||||
deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal main restricted universe multiverse
|
||||
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal main restricted universe multiverse
|
||||
deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-updates main restricted universe multiverse
|
||||
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-updates main restricted universe multiverse
|
||||
deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-backports main restricted universe multiverse
|
||||
# deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-backports main restricted universe multiverse
|
||||
|
||||
# deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-security main restricted universe multiverse
|
||||
# # deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-security main restricted universe multiverse
|
||||
|
||||
deb http://security.ubuntu.com/ubuntu/ focal-security main restricted universe multiverse
|
||||
# deb-src http://security.ubuntu.com/ubuntu/ focal-security main restricted universe multiverse
|
||||
|
||||
# 预发布软件源，不建议启用
|
||||
# deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-proposed main restricted universe multiverse
|
||||
# # deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu/ focal-proposed main restricted universe multiverse
|
||||
0
workspace/slam_in_autonomous_driving/lib/.gitkeep
Normal file
0
workspace/slam_in_autonomous_driving/lib/.gitkeep
Normal file
304
workspace/slam_in_autonomous_driving/readme.md
Normal file
304
workspace/slam_in_autonomous_driving/readme.md
Normal file
@@ -0,0 +1,304 @@
|
||||
## SLAM in Autonomous Driving Book (SAD Book)
|
||||
|
||||
This book systematically introduces readers to inertial navigation, integrated navigation, LiDAR mapping, LiDAR localization, LiDAR-inertial odometry, and related knowledge. This repository contains the source code accompanying the book and is publicly available for use.
|
||||
|
||||
The English translation is almost done and is open-source, check it out here: https://github.com/gaoxiang12/slam-in-ad-en/blob/main/sad-en.pdf
|
||||
|
||||
<img src="https://github.com/gaoxiang12/slam_in_autonomous_driving/assets/6635511/734af25b-d866-4dcf-a155-773190ba03d8" width="300" />
|
||||
|
||||
## Notes
|
||||
|
||||
- The book began printing on July 10, 2023, and is expected to be available within two weeks. I will update the links to various platforms at that time.
|
||||
- August 9, 2023: The book is currently in its second printing, with some content corrections from the first edition (though without signatures). For details, see the code updates.
|
||||
- Official page from Electronic Industry Publishing House: https://item.jd.com/10080292102089.html
|
||||
- JD.com self-operated store: https://item.jd.com/13797963.html
|
||||
|
||||
## Book Content Structure
|
||||
|
||||
- Chapter 1: Overview
|
||||
- Chapter 2: Review of mathematical fundamentals: geometry, kinematics, KF filter theory, and matrix Lie groups
|
||||
- Chapter 3: Error-state Kalman filter, inertial navigation, satellite navigation, and integrated navigation
|
||||
- Chapter 4: Pre-integration, graph optimization, and pre-integration-based integrated navigation
|
||||
- Chapter 5: Point cloud basics, nearest neighbor structures, and point cloud linear fitting
|
||||
- Chapter 6: 2D LiDAR mapping: scan matching, likelihood fields, submaps, 2D loop closure detection, and pose graph
|
||||
- Chapter 7: 3D LiDAR mapping: ICP, ICP variants, NDT, NDT LO, Loam-like LO, and loosely-coupled LIO
|
||||
- Chapter 8: Tightly-coupled LIO, IESKF, and pre-integration tightly-coupled LIO
|
||||
- Chapter 9: Offline mapping: frontend, backend, batch loop closure detection, map optimization, and tile export
|
||||
- Chapter 10: Fusion localization: LiDAR localization, initialization search, tile map loading, and EKF fusion
|
||||
|
||||
## Features of This Book
|
||||
|
||||
- This book likely offers the simplest mathematical derivations and code implementations among similar materials.
|
||||
- In this book, you will reproduce many classic algorithms and data structures in LiDAR SLAM:
|
||||
- You will derive and implement an Error-State Kalman Filter (ESKF), feed it IMU and GNSS data, and observe how it estimates its state.
|
||||
- You will also implement a pre-integration system for the same purpose and compare their performance.
|
||||
- Next, you will implement common algorithms in 2D LiDAR SLAM: scan matching, likelihood fields, submaps, occupancy grids, and use loop closure detection to build larger maps—all by yourself.
|
||||
- In LiDAR SLAM, you will implement a K-d tree for approximate nearest neighbor searches, then use it for ICP and point-to-plane ICP, discussing potential improvements.
|
||||
- You will implement the classic NDT algorithm, test its registration performance, and use it to build a LiDAR odometer—much faster than most existing LOs.
|
||||
- You will also implement a point-to-plane ICP LiDAR odometer, which is similarly fast and works similarly to Loam but simpler.
|
||||
- You will integrate IMU into the LiDAR odometer, implementing both loosely-coupled and tightly-coupled LIO systems, including derivations for iterative Kalman filters and pre-integration graph optimization.
|
||||
- You will adapt the system for offline operation to allow thorough loop closure detection, ultimately creating an offline mapping system.
|
||||
- Finally, you will segment the resulting map for real-time localization.
|
||||
- Most implementations in this book are significantly simpler than those in comparable libraries, making it easier to understand their workings without dealing with complex interfaces.
|
||||
- The book employs convenient concurrent programming, often resulting in more efficient implementations than existing algorithms (partly due to historical reasons).
|
||||
- Each chapter includes dynamic demonstrations like these:
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
We hope you enjoy the minimalist style of this book and discover the joy of algorithms.
|
||||
|
||||
## Datasets
|
||||
|
||||
- Dataset download links:
|
||||
- Baidu Cloud: https://pan.baidu.com/s/1ELOcF1UTKdfiKBAaXnE8sQ?pwd=feky (Extraction code: feky)
|
||||
- OneDrive: https://1drv.ms/u/s!AgNFVSzSYXMahcEZejoUwCaHRcactQ?e=YsOYy2
|
||||
|
||||
- Includes the following datasets (total 270GB; download selectively based on storage capacity):
|
||||
- UrbanLoco (ULHK, 3D LiDAR, urban road scenarios)
|
||||
- NCLT (3D LiDAR, RTK, campus scenarios)
|
||||
- WXB (3D LiDAR, campus scenarios)
|
||||
- 2dmapping (2D LiDAR, mall scenarios)
|
||||
- AVIA (DJI solid-state LiDAR)
|
||||
- UTBM (3D LiDAR, road scenarios)
|
||||
- Other built-in data:
|
||||
- Chapters 3–4 use text-formatted IMU and RTK data.
|
||||
- Chapter 7 uses some EPFL data for point cloud registration.
|
||||
- Store datasets in `./dataset/sad/` for default paths to work. Alternatively, manually specify paths or create symlinks if storage is limited.
|
||||
|
||||
## Compilation
|
||||
|
||||
- Recommended environment: Ubuntu 20.04. Older versions require GCC adjustments (C++17 support). For newer Ubuntu, install the corresponding ROS version.
|
||||
- Prerequisites:
|
||||
- ROS Noetic: http://wiki.ros.org/noetic/Installation/Ubuntu
|
||||
- Additional libraries:
|
||||
```bash
|
||||
sudo apt install -y ros-noetic-pcl-ros ros-noetic-velodyne-msgs libopencv-dev libgoogle-glog-dev libeigen3-dev libsuitesparse-dev libpcl-dev libyaml-cpp-dev libbtbb-dev libgmock-dev
|
||||
```
|
||||
- Pangolin: Compile `thirdparty/pangolin.zip` or install from https://github.com/stevenlovegrove/Pangolin
|
||||
- g2o: Compile `thirdparty/g2o` or install from https://github.com/RainerKuemmerle/g2o
|
||||
- Build commands:
|
||||
```bash
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
```
|
||||
- Executables are located in the `bin` directory.
|
||||
|
||||
### Ubuntu 18.04 Adaptation
|
||||
|
||||
For Ubuntu 18.04, install GCC-9 and compatible TBB, or use Docker.
|
||||
|
||||
**Install GCC-9:**
|
||||
```bash
|
||||
sudo add-apt-repository ppa:ubuntu-toolchain-r/test
|
||||
sudo update-alternatives --remove-all gcc
|
||||
sudo update-alternatives --remove-all g++
|
||||
|
||||
# Priority values (1 and 10); auto-mode defaults to higher priority.
|
||||
sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 1
|
||||
sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 10
|
||||
|
||||
sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-7 1
|
||||
sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-9 10
|
||||
```
|
||||
|
||||
**Check version:**
|
||||
```bash
|
||||
g++ -v
|
||||
```
|
||||
|
||||
**Build:**
|
||||
```bash
|
||||
mkdir build
|
||||
cd build
|
||||
cmake .. -DBUILD_WITH_UBUNTU1804=ON
|
||||
make -j8
|
||||
```
|
||||
|
||||
**Docker setup:**
|
||||
```bash
|
||||
docker build -t sad:v1 .
|
||||
./docker/docker_run.sh
|
||||
```
|
||||
Inside the container:
|
||||
```bash
|
||||
cd ./thirdparty/g2o
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
cd /sad
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
```
|
||||
|
||||
## FAQs
|
||||
|
||||
1. GUI crashes on certain 2023+ laptop models (GL hardware compatibility): https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/67
|
||||
2. Chapter 5 `test_nn` GMock errors during compilation: https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/18
|
||||
3. Compiler version issues: https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/4
|
||||
4. g2o compilation (`config.h` not found): https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/95
|
||||
|
||||
---
|
||||
|
||||
## SLAM in Autonomous Driving book (SAD book)
|
||||
|
||||
本书向读者系统介绍了惯性导航、组合导航、激光建图、激光定位、激光惯导里程计等知识。本仓库是书籍对应的源代码仓库，可以公开使用。
|
||||
|
||||
<img src="https://github.com/gaoxiang12/slam_in_autonomous_driving/assets/6635511/734af25b-d866-4dcf-a155-773190ba03d8" width="300" />
|
||||
|
||||
|
||||
|
||||
## 注意
|
||||
|
||||
- 本书已于2023.7.10开始印刷，预计在两周内上架。届时我会更新各平台的链接信息。
|
||||
- 2023.8.9 本书目前是第二次印刷，在第一次上修正了一部分内容（但没有签名了），详情见代码的推送。
|
||||
- 电子工业出版社官方：https://item.jd.com/10080292102089.html
|
||||
- 京东自营： https://item.jd.com/13797963.html
|
||||
|
||||
## 本书的内容编排
|
||||
|
||||
- 第1章，概述
|
||||
- 第2章，数学基础知识回顾，几何学、运动学、KF滤波器理论，矩阵李群
|
||||
- 第3章，误差状态卡尔曼滤波器，惯性导航、卫星导航、组合导航
|
||||
- 第4章，预积分，图优化，基于预积分的组合导航
|
||||
- 第5章，点云基础处理，各种最近邻结构，点云线性拟合
|
||||
- 第6章，2D激光建图，scan matching, 似然场，子地图，2D回环检测，pose graph
|
||||
- 第7章，3D激光建图，ICP，变种ICP，NDT，NDT LO, Loam-like LO，LIO松耦合
|
||||
- 第8章，紧耦合LIO，IESKF，预积分紧耦合LIO
|
||||
- 第9章，离线建图，前端，后端，批量回环检测，地图优化，切片导出
|
||||
- 第10章，融合定位，激光定位，初始化搜索，切片地图加载，EKF融合
|
||||
|
||||
## 本书的特点
|
||||
|
||||
- 本书大概是您能找到的类似材料中，数学推导和代码实现最为简单的书籍。
|
||||
- 在这本书里，您会复现许多激光SLAM中的经典算法和数据结构。
|
||||
- 您需要自己推导、实现一个误差状态卡尔曼滤波器(ESKF)，把IMU和GNSS的数据喂给它，看它如何推算自己的状态。
|
||||
- 您还会用预积分系统实现一样的功能，然后对比它们的运行方式。
|
||||
- 接下来您会实现一遍2D激光SLAM中的常见算法：扫描匹配、似然场、子地图，占据栅格，再用回环检测来构建一个更大的地图。这些都需要您自己来完成。
|
||||
- 在激光SLAM中，您也会自己实现一遍Kd树，处理近似最近邻，然后用这个Kd树来实现ICP，点面ICP，讨论它们有什么可以改进的地方。
|
||||
- 然后您会实现经典的NDT算法，测试它的配准性能，然后用它来搭建一个激光里程计。它比大部分现有LO快得多。
|
||||
- 您也会实现一个点面ICP的激光里程计，它也非常快。它工作的方式类似于Loam，但更简单。
|
||||
- 您会想要把IMU系统也放到激光里程计中。我们会实现松耦合和紧耦合的LIO系统。同样地，您需要推导一遍迭代卡尔曼滤波器和预积分图优化。
|
||||
- 您需要把上面的系统改成离线运行的，让回环检测运行地充分一些。最后将它做成一个离线的建图系统。
|
||||
- 最后，您可以对上述地图进行切分，然后用来做实时定位。
|
||||
- 本书的大部分实现都要比类似的算法库简单的多。您可以快速地理解它们的工作方式，不需要面对复杂的接口。
|
||||
- 本书会使用非常方便的并发编程。您会发现，本书的实现往往比现有算法更高效。当然这有一部分是历史原因造成的。
|
||||
- 本书每章都会配有动态演示，像这样：
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
希望您能喜欢本书的极简风格，发现算法的乐趣所在。
|
||||
|
||||
## 数据集
|
||||
|
||||
- 数据集下载链接：
|
||||
- 百度云链接: https://pan.baidu.com/s/1ELOcF1UTKdfiKBAaXnE8sQ?pwd=feky 提取码: feky
|
||||
- OneDrive链接：https://1drv.ms/u/s!AgNFVSzSYXMahcEZejoUwCaHRcactQ?e=YsOYy2
|
||||
|
||||
- 包含以下数据集。总量较大(270GB)，请视自己硬盘容量下载。
|
||||
- UrbanLoco (ULHK，3D激光，道路场景)
|
||||
- NCLT (3D激光，RTK，校园场景)
|
||||
- WXB (3D激光，园区场景)
|
||||
- 2dmapping (2D激光，商场场景)
|
||||
- AVIA (大疆固态激光)
|
||||
- UTBM (3D激光，道路场景)
|
||||
- 其他的内置数据
|
||||
- 第3,4章使用文本格式的IMU，RTK数据
|
||||
- 第7章使用了一部分EPFL的数据作为配准点云来源
|
||||
- 您应该将上述数据下载至./dataset/sad/目录下，这样许多默认参数可以正常工作。如果不那么做，您也可以手动指定这些文件路径。如果您硬盘容量不足，可以将其他硬盘的目录软链至此处。
|
||||
|
||||
## 编译
|
||||
|
||||
- 本书推荐的编译环境是Ubuntu 20.04。更老的Ubuntu版本需要适配gcc编译器，主要是C++17标准。更新的Ubuntu则需要您自己安装对应的ROS版本。
|
||||
- 在编译本书代码之前，请先安装以下库（如果您机器上没有安装的话）
|
||||
- ROS Noetic: http://wiki.ros.org/noetic/Installation/Ubuntu
|
||||
- 使用以下指令安装其余的库
|
||||
```bash
|
||||
sudo apt install -y ros-noetic-pcl-ros ros-noetic-velodyne-msgs libopencv-dev libgoogle-glog-dev libeigen3-dev libsuitesparse-dev libpcl-dev libyaml-cpp-dev libbtbb-dev libgmock-dev
|
||||
```
|
||||
- Pangolin: 编译安装thirdparty/pangolin.zip，或者 https://github.com/stevenlovegrove/Pangolin
|
||||
- 编译thirdparty/g2o，或者自行编译安装 https://github.com/RainerKuemmerle/g2o
|
||||
- 通过cmake, make安装本repo下的`thirdparty/g2o`库
|
||||
- 之后，使用通常的cmake, make方式就可以编译本书所有内容了。例如
|
||||
```bash
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
```
|
||||
- 编译后各章的可执行文件位于`bin`目录下
|
||||
|
||||
### 适配Ubuntu18.04
|
||||
|
||||
为了在Ubuntu18.04上编译运行，需要安装gcc-9，并且使用对应版本的TBB。或者在docker环境下使用。
|
||||
|
||||
**安装gcc-9**
|
||||
```bash
|
||||
sudo add-apt-repository ppa:ubuntu-toolchain-r/test
|
||||
sudo update-alternatives --remove-all gcc
|
||||
sudo update-alternatives --remove-all g++
|
||||
|
||||
#命令最后的1和10是优先级，如果使用auto选择模式，系统将默认使用优先级高的
|
||||
sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 1
|
||||
sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-9 10
|
||||
|
||||
sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-7 1
|
||||
sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-9 10
|
||||
```
|
||||
|
||||
**检查版本**
|
||||
```bash
|
||||
g++ -v
|
||||
```
|
||||
|
||||
**编译程序**
|
||||
```bash
|
||||
mkdir build
|
||||
cd build
|
||||
cmake .. -DBUILD_WITH_UBUNTU1804=ON
|
||||
make -j8
|
||||
```
|
||||
|
||||
**在docker环境下使用**
|
||||
```bash
|
||||
docker build -t sad:v1 .
|
||||
./docker/docker_run.sh
|
||||
```
|
||||
进入docker容器后
|
||||
```bash
|
||||
cd ./thirdparty/g2o
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
cd /sad
|
||||
mkdir build
|
||||
cd build
|
||||
cmake ..
|
||||
make -j8
|
||||
```
|
||||
|
||||
## 常见问题
|
||||
1. 图形界面在2023年以后特定型号的笔记本端导致桌面卡死（GL硬件兼容性）：https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/67
|
||||
2. 第5章test_nn编译时，gtest报gmock错误：https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/18
|
||||
3. 编译器版本问题：https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/4
|
||||
4. g2o编译问题（config.h找不到）： https://github.com/gaoxiang12/slam_in_autonomous_driving/issues/95
|
||||
|
||||
## TODO项
|
||||
|
||||
- LioPreiteg在某些数据集上不收敛
|
||||
|
||||
## NOTES
|
||||
|
||||
- [已确认] ULHK的IMU似乎和别家的不一样，已经去了gravity, iekf初期可能有问题
|
||||
- [已确认] NCLT的IMU在转包的时候转成了Lidar系，于是Lidar与IMU之间没有旋转的外参（本来Lidar是转了90度的），现在Lidar是X左Y后Z下，原车是X前Y右Z下。本书使用的NCLT数据均基于点云系,
|
||||
IMU的杆臂被忽略。
|
||||
- [已确认] NCLT的rtk fix并不是非常稳定，平均误差在米级
|
||||
58
workspace/slam_in_autonomous_driving/scripts/all_path.py
Executable file
58
workspace/slam_in_autonomous_driving/scripts/all_path.py
Executable file
@@ -0,0 +1,58 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
|
||||
def PlotPath(keyframes):
|
||||
fig = plt.figure('All path 3d')
|
||||
ax = Axes3D(fig)
|
||||
|
||||
# lidar pose
|
||||
p00, = ax.plot(keyframes[:, 5], keyframes[:, 6], keyframes[:, 7], 'y-')
|
||||
|
||||
# gps pose
|
||||
p01, = ax.plot(keyframes[:, 12], keyframes[:, 13], keyframes[:, 14], 'g-')
|
||||
|
||||
# opti1
|
||||
p02, = ax.plot(keyframes[:, 19], keyframes[:, 20], keyframes[:, 21], 'b-')
|
||||
|
||||
# opti2
|
||||
# p03, = ax.plot(keyframes[:, 26], keyframes[:, 27], keyframes[:, 28], 'r-')
|
||||
|
||||
plt.title("All path", fontsize=16)
|
||||
plt.grid()
|
||||
fig = plt.figure('All path 2d')
|
||||
|
||||
# lidar
|
||||
p0, = plt.plot(keyframes[:, 5], keyframes[:, 6], 'y-')
|
||||
|
||||
# gps
|
||||
p1, = plt.plot(keyframes[:, 12], keyframes[:, 13], 'g-')
|
||||
|
||||
# opti1
|
||||
p2, = plt.plot(keyframes[:, 19], keyframes[:, 20], 'b-')
|
||||
|
||||
# opti2
|
||||
# p3, = plt.plot(keyframes[:, 26], keyframes[:, 27], 'r-')
|
||||
# plt.legend(['Lidar', 'RTK', 'Opti1', 'Opti2'])
|
||||
plt.legend(['Lidar', 'RTK', 'Opti1'])
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
def LoadMappingTxt(filepath):
|
||||
keyframes = np.loadtxt(filepath)
|
||||
PlotPath(keyframes)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 2:
|
||||
print('Please input vaild param !!!')
|
||||
exit(1)
|
||||
else:
|
||||
path = sys.argv[1]
|
||||
LoadMappingTxt(path)
|
||||
exit(1)
|
||||
exit(1)
|
||||
@@ -0,0 +1,25 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
# 格式：t x y z qx qy qz qw
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 2:
|
||||
print('Please input valid file')
|
||||
exit(1)
|
||||
else:
|
||||
path = sys.argv[1]
|
||||
path_data = np.loadtxt(path)
|
||||
plt.rcParams['figure.figsize'] = (16.0, 12.0)
|
||||
|
||||
# 2D轨迹
|
||||
plt.scatter(path_data[:, 1], path_data[:, 2], s=2)
|
||||
plt.xlabel('X')
|
||||
plt.ylabel('Y')
|
||||
plt.grid()
|
||||
plt.title('2D trajectory')
|
||||
|
||||
plt.show()
|
||||
exit(1)
|
||||
@@ -0,0 +1,20 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
# 格式：t x y z qx qy qz qw
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 2:
|
||||
print('Please input valid file')
|
||||
exit(1)
|
||||
else:
|
||||
path = sys.argv[1]
|
||||
path_data = np.loadtxt(path)
|
||||
fig = plt.figure()
|
||||
ax = Axes3D(fig)
|
||||
ax.plot(path_data[:, 1], path_data[:, 2], path_data[:, 3])
|
||||
plt.title('3D path')
|
||||
plt.show()
|
||||
exit(1)
|
||||
@@ -0,0 +1,43 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
# 格式：t x y z qx qy qz qw vx vy vz
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 2:
|
||||
print('Please input valid file')
|
||||
exit(1)
|
||||
else:
|
||||
path = sys.argv[1]
|
||||
path_data = np.loadtxt(path)
|
||||
plt.rcParams['figure.figsize'] = (16.0, 12.0)
|
||||
|
||||
# 轨迹
|
||||
plt.subplot(121)
|
||||
plt.scatter(path_data[:, 1], path_data[:, 2], s=2)
|
||||
plt.xlabel('X')
|
||||
plt.ylabel('Y')
|
||||
plt.grid()
|
||||
plt.title('2D trajectory')
|
||||
|
||||
# 姿态
|
||||
plt.subplot(222)
|
||||
plt.plot(path_data[:, 0], path_data[:, 4], 'r')
|
||||
plt.plot(path_data[:, 0], path_data[:, 5], 'g')
|
||||
plt.plot(path_data[:, 0], path_data[:, 6], 'b')
|
||||
plt.plot(path_data[:, 0], path_data[:, 7], 'k')
|
||||
plt.title('q')
|
||||
plt.legend(['qw', 'qx', 'qy', 'qz'])
|
||||
|
||||
# 速度
|
||||
plt.subplot(224)
|
||||
plt.plot(path_data[:, 0], path_data[:, 8], 'r')
|
||||
plt.plot(path_data[:, 0], path_data[:, 9], 'g')
|
||||
plt.plot(path_data[:, 0], path_data[:, 10], 'b')
|
||||
plt.title('v')
|
||||
plt.legend(['vx', 'vy', 'vz'])
|
||||
|
||||
plt.show()
|
||||
exit(1)
|
||||
@@ -0,0 +1,53 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from mpl_toolkits.mplot3d import Axes3D
|
||||
|
||||
if __name__ == '__main__':
|
||||
if len(sys.argv) != 2:
|
||||
print('Please input valid file')
|
||||
exit(1)
|
||||
else:
|
||||
path = sys.argv[1]
|
||||
path_data = np.loadtxt(path)
|
||||
plt.rcParams['figure.figsize'] = (16.0, 12.0)
|
||||
|
||||
# 轨迹
|
||||
plt.subplot(121)
|
||||
plt.scatter(path_data[:, 1], path_data[:, 2], s=2)
|
||||
plt.xlabel('X')
|
||||
plt.ylabel('Y')
|
||||
plt.grid()
|
||||
plt.title('2D trajectory')
|
||||
|
||||
# 速度
|
||||
plt.subplot(322)
|
||||
plt.plot(path_data[:, 0], path_data[:, 8], 'r')
|
||||
plt.plot(path_data[:, 0], path_data[:, 9], 'g')
|
||||
plt.plot(path_data[:, 0], path_data[:, 10], 'b')
|
||||
plt.title('v')
|
||||
plt.legend(['vx', 'vy', 'vz'])
|
||||
|
||||
# 零偏
|
||||
plt.subplot(324)
|
||||
plt.plot(path_data[:, 0], path_data[:, 11], 'r')
|
||||
plt.plot(path_data[:, 0], path_data[:, 12], 'g')
|
||||
plt.plot(path_data[:, 0], path_data[:, 13], 'b')
|
||||
plt.title('bias gyro')
|
||||
plt.legend(['bg_x', 'bg_y', 'bg_z'])
|
||||
|
||||
plt.subplot(326)
|
||||
plt.plot(path_data[:, 0], path_data[:, 14], 'r')
|
||||
plt.plot(path_data[:, 0], path_data[:, 15], 'g')
|
||||
plt.plot(path_data[:, 0], path_data[:, 16], 'b')
|
||||
plt.title('bias acce')
|
||||
plt.legend(['ba_x', 'ba_y', 'ba_z'])
|
||||
|
||||
# 3D轨迹
|
||||
fig = plt.figure()
|
||||
ax = Axes3D(fig)
|
||||
ax.scatter(path_data[:, 1], path_data[:, 2], path_data[:, 3], s=2)
|
||||
|
||||
plt.show()
|
||||
exit(1)
|
||||
@@ -0,0 +1,18 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
method_names = ['Brute-force', 'Brute-force MT', '2D Grid8', '2D Grid8 MT', '3D Grid', '3D Grid MT', 'KD Tree',
|
||||
'KD Tree MT',
|
||||
'KD Tree PCL', 'Octo Tree', 'Octo Tree MT']
|
||||
time_usage = [821.109, 102.074, 7.6908, 0.7044, 3.1735, 0.3916, 9.6234, 2.0651, 11.8725, 31.9253, 4.7698]
|
||||
|
||||
fig, ax = plt.subplots()
|
||||
plt.bar(method_names, time_usage, color='rgbky')
|
||||
plt.title('NN methods')
|
||||
ax.set_yscale('log')
|
||||
# plt.xlabel("method name")
|
||||
plt.ylabel("runtime(s)")
|
||||
plt.grid()
|
||||
plt.show()
|
||||
@@ -0,0 +1,39 @@
|
||||
# coding=UTF-8
|
||||
import sys
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
method_names = ['Point-Point ICP', 'Point-Plane ICP', 'Point-Line ICP', 'NDT', 'PCL ICP', 'PCL NDT']
|
||||
time_usage = [399.808, 334.409, 492.718, 142.278, 2774.16, 255.947]
|
||||
|
||||
res_icp_point = [0.211044, 0.155513, 0.114218, 0.0837873, 0.0611407, 0.0444493, 0.0320967, 0.0229813]
|
||||
res_icp_plane = [0.0946098, 0.00664127, 0.000288226]
|
||||
res_icp_line = [0.20137, 0.138774, 0.0931768, 0.0618093, 0.0410299, 0.0276539, 0.0192892]
|
||||
res_ndt = [0.148351, 0.0597357, 0.0227735, 0.0100937, 0.00690136]
|
||||
res_icp_pcl = [0.0483]
|
||||
res_ndt_pcl = [0.1603]
|
||||
|
||||
# 用时
|
||||
plt.figure()
|
||||
plt.bar(method_names, time_usage, color='rgbkyc')
|
||||
plt.title('Align Time Usage')
|
||||
plt.xlabel("method name")
|
||||
plt.ylabel("runtime(ms)")
|
||||
plt.grid()
|
||||
plt.show()
|
||||
|
||||
# 收敛曲线
|
||||
plt.figure()
|
||||
plt.plot(range(0, len(res_icp_point)), res_icp_point, color='r', marker='o')
|
||||
plt.plot(range(0, len(res_icp_plane)), res_icp_plane, color='g', marker='o')
|
||||
plt.plot(range(0, len(res_icp_line)), res_icp_line, color='b', marker='o')
|
||||
plt.plot(range(0, len(res_ndt)), res_ndt, color='k', marker='o')
|
||||
plt.axhline(res_icp_pcl, color='y')
|
||||
plt.axhline(res_ndt_pcl, color='c')
|
||||
# plt.yscale('log')
|
||||
plt.title('Pose Convergence Speed')
|
||||
plt.xlabel("iteration")
|
||||
plt.ylabel("Pose error")
|
||||
plt.legend(method_names)
|
||||
plt.grid()
|
||||
plt.show()
|
||||
12
workspace/slam_in_autonomous_driving/src/CMakeLists.txt
Normal file
12
workspace/slam_in_autonomous_driving/src/CMakeLists.txt
Normal file
@@ -0,0 +1,12 @@
|
||||
add_subdirectory(common)
|
||||
add_subdirectory(ch2)
|
||||
add_subdirectory(ch3)
|
||||
add_subdirectory(ch4)
|
||||
add_subdirectory(ch5)
|
||||
add_subdirectory(ch6)
|
||||
add_subdirectory(ch7)
|
||||
add_subdirectory(ch8)
|
||||
add_subdirectory(ch9)
|
||||
add_subdirectory(ch10)
|
||||
|
||||
add_subdirectory(tools)
|
||||
15
workspace/slam_in_autonomous_driving/src/ch10/CMakeLists.txt
Normal file
15
workspace/slam_in_autonomous_driving/src/ch10/CMakeLists.txt
Normal file
@@ -0,0 +1,15 @@
|
||||
add_library(${PROJECT_NAME}.ch10
|
||||
fusion.cc
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}.ch10
|
||||
${PROJECT_NAME}.ch3
|
||||
${PROJECT_NAME}.ch7
|
||||
${third_party_libs}
|
||||
)
|
||||
|
||||
add_executable(run_fusion_offline run_fusion_offline.cc)
|
||||
target_link_libraries(run_fusion_offline
|
||||
${PROJECT_NAME}.ch10
|
||||
${third_party_libs}
|
||||
)
|
||||
316
workspace/slam_in_autonomous_driving/src/ch10/fusion.cc
Normal file
316
workspace/slam_in_autonomous_driving/src/ch10/fusion.cc
Normal file
@@ -0,0 +1,316 @@
|
||||
//
|
||||
// Created by xiang on 22-12-20.
|
||||
//
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <execution>
|
||||
|
||||
#include "common/lidar_utils.h"
|
||||
#include "fusion.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
Fusion::Fusion(const std::string& config_yaml) {
|
||||
config_yaml_ = config_yaml;
|
||||
StaticIMUInit::Options imu_init_options;
|
||||
imu_init_options.use_speed_for_static_checking_ = false; // 本节数据不需要轮速计
|
||||
imu_init_ = StaticIMUInit(imu_init_options);
|
||||
ndt_.setResolution(1.0);
|
||||
}
|
||||
|
||||
bool Fusion::Init() {
|
||||
// 地图原点
|
||||
auto yaml = YAML::LoadFile(config_yaml_);
|
||||
auto origin_data = yaml["origin"].as<std::vector<double>>();
|
||||
map_origin_ = Vec3d(origin_data[0], origin_data[1], origin_data[2]);
|
||||
|
||||
// 地图目录
|
||||
data_path_ = yaml["map_data"].as<std::string>();
|
||||
LoadMapIndex();
|
||||
|
||||
// lidar和IMU消息同步
|
||||
sync_ = std::make_shared<MessageSync>([this](const MeasureGroup& m) { ProcessMeasurements(m); });
|
||||
sync_->Init(config_yaml_);
|
||||
|
||||
// lidar和IMU外参
|
||||
std::vector<double> ext_t = yaml["mapping"]["extrinsic_T"].as<std::vector<double>>();
|
||||
std::vector<double> ext_r = yaml["mapping"]["extrinsic_R"].as<std::vector<double>>();
|
||||
Vec3d lidar_T_wrt_IMU = math::VecFromArray(ext_t);
|
||||
Mat3d lidar_R_wrt_IMU = math::MatFromArray(ext_r);
|
||||
TIL_ = SE3(lidar_R_wrt_IMU, lidar_T_wrt_IMU);
|
||||
|
||||
// ui
|
||||
ui_ = std::make_shared<ui::PangolinWindow>();
|
||||
ui_->Init();
|
||||
ui_->SetCurrentScanSize(50);
|
||||
return true;
|
||||
}
|
||||
|
||||
void Fusion::ProcessRTK(GNSSPtr gnss) {
|
||||
gnss->utm_pose_.translation() -= map_origin_; // 减掉地图原点
|
||||
last_gnss_ = gnss;
|
||||
}
|
||||
|
||||
void Fusion::ProcessMeasurements(const MeasureGroup& meas) {
|
||||
measures_ = meas;
|
||||
|
||||
if (imu_need_init_) {
|
||||
TryInitIMU();
|
||||
return;
|
||||
}
|
||||
|
||||
/// 以下三步与LIO一致，只是align完成地图匹配工作
|
||||
if (status_ == Status::WORKING) {
|
||||
Predict();
|
||||
Undistort();
|
||||
} else {
|
||||
scan_undistort_ = measures_.lidar_;
|
||||
}
|
||||
|
||||
Align();
|
||||
}
|
||||
|
||||
void Fusion::TryInitIMU() {
|
||||
for (auto imu : measures_.imu_) {
|
||||
imu_init_.AddIMU(*imu);
|
||||
}
|
||||
|
||||
if (imu_init_.InitSuccess()) {
|
||||
// 读取初始零偏，设置ESKF
|
||||
sad::ESKFD::Options options;
|
||||
// 噪声由初始化器估计
|
||||
// options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);
|
||||
// options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);
|
||||
options.update_bias_acce_ = false;
|
||||
options.update_bias_gyro_ = false;
|
||||
eskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());
|
||||
imu_need_init_ = false;
|
||||
|
||||
LOG(INFO) << "IMU初始化成功";
|
||||
}
|
||||
}
|
||||
|
||||
void Fusion::Predict() {
|
||||
imu_states_.clear();
|
||||
imu_states_.emplace_back(eskf_.GetNominalState());
|
||||
|
||||
/// 对IMU状态进行预测
|
||||
for (auto& imu : measures_.imu_) {
|
||||
eskf_.Predict(*imu);
|
||||
imu_states_.emplace_back(eskf_.GetNominalState());
|
||||
}
|
||||
}
|
||||
|
||||
void Fusion::Undistort() {
|
||||
auto cloud = measures_.lidar_;
|
||||
auto imu_state = eskf_.GetNominalState(); // 最后时刻的状态
|
||||
SE3 T_end = SE3(imu_state.R_, imu_state.p_);
|
||||
|
||||
/// 将所有点转到最后时刻状态上
|
||||
std::for_each(std::execution::par_unseq, cloud->points.begin(), cloud->points.end(), [&](auto& pt) {
|
||||
SE3 Ti = T_end;
|
||||
NavStated match;
|
||||
|
||||
// 根据pt.time查找时间，pt.time是该点打到的时间与雷达开始时间之差，单位为毫秒
|
||||
math::PoseInterp<NavStated>(
|
||||
measures_.lidar_begin_time_ + pt.time * 1e-3, imu_states_, [](const NavStated& s) { return s.timestamp_; },
|
||||
[](const NavStated& s) { return s.GetSE3(); }, Ti, match);
|
||||
|
||||
Vec3d pi = ToVec3d(pt);
|
||||
Vec3d p_compensate = TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;
|
||||
|
||||
pt.x = p_compensate(0);
|
||||
pt.y = p_compensate(1);
|
||||
pt.z = p_compensate(2);
|
||||
});
|
||||
scan_undistort_ = cloud;
|
||||
}
|
||||
|
||||
void Fusion::Align() {
|
||||
FullCloudPtr scan_undistort_trans(new FullPointCloudType);
|
||||
pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix());
|
||||
scan_undistort_ = scan_undistort_trans;
|
||||
current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
|
||||
current_scan_ = VoxelCloud(current_scan_, 0.5);
|
||||
|
||||
if (status_ == Status::WAITING_FOR_RTK) {
|
||||
// 若存在最近的RTK信号，则尝试初始化
|
||||
if (last_gnss_ != nullptr) {
|
||||
if (SearchRTK()) {
|
||||
status_ == Status::WORKING;
|
||||
ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
|
||||
ui_->UpdateNavState(eskf_.GetNominalState());
|
||||
}
|
||||
}
|
||||
} else {
|
||||
LidarLocalization();
|
||||
ui_->UpdateScan(current_scan_, eskf_.GetNominalSE3());
|
||||
ui_->UpdateNavState(eskf_.GetNominalState());
|
||||
}
|
||||
}
|
||||
|
||||
bool Fusion::SearchRTK() {
|
||||
if (init_has_failed_) {
|
||||
if ((last_gnss_->utm_pose_.translation() - last_searched_pos_.translation()).norm() < 20.0) {
|
||||
LOG(INFO) << "skip this position";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 由于RTK不带姿态，我们必须先搜索一定的角度范围
|
||||
std::vector<GridSearchResult> search_poses;
|
||||
LoadMap(last_gnss_->utm_pose_);
|
||||
|
||||
/// 由于RTK不带角度，这里按固定步长扫描RTK角度
|
||||
double grid_ang_range = 360.0, grid_ang_step = 10; // 角度搜索范围与步长
|
||||
for (double ang = 0; ang < grid_ang_range; ang += grid_ang_step) {
|
||||
SE3 pose(SO3::rotZ(ang * math::kDEG2RAD), Vec3d(0, 0, 0) + last_gnss_->utm_pose_.translation());
|
||||
GridSearchResult gr;
|
||||
gr.pose_ = pose;
|
||||
search_poses.emplace_back(gr);
|
||||
}
|
||||
|
||||
LOG(INFO) << "grid search poses: " << search_poses.size();
|
||||
std::for_each(std::execution::par_unseq, search_poses.begin(), search_poses.end(),
|
||||
[this](GridSearchResult& gr) { AlignForGrid(gr); });
|
||||
|
||||
// 选择最优的匹配结果
|
||||
auto max_ele = std::max_element(search_poses.begin(), search_poses.end(),
|
||||
[](const auto& g1, const auto& g2) { return g1.score_ < g2.score_; });
|
||||
LOG(INFO) << "max score: " << max_ele->score_ << ", pose: \n" << max_ele->result_pose_.matrix();
|
||||
if (max_ele->score_ > rtk_search_min_score_) {
|
||||
LOG(INFO) << "初始化成功, score: " << max_ele->score_ << ">" << rtk_search_min_score_;
|
||||
status_ = Status::WORKING;
|
||||
|
||||
/// 重置滤波器状态
|
||||
auto state = eskf_.GetNominalState();
|
||||
state.R_ = max_ele->result_pose_.so3();
|
||||
state.p_ = max_ele->result_pose_.translation();
|
||||
state.v_.setZero();
|
||||
eskf_.SetX(state, eskf_.GetGravity());
|
||||
|
||||
ESKFD::Mat18T cov;
|
||||
cov = ESKFD::Mat18T::Identity() * 1e-4;
|
||||
cov.block<12, 12>(6, 6) = Eigen::Matrix<double, 12, 12>::Identity() * 1e-6;
|
||||
eskf_.SetCov(cov);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
init_has_failed_ = true;
|
||||
last_searched_pos_ = last_gnss_->utm_pose_;
|
||||
return false;
|
||||
}
|
||||
|
||||
void Fusion::AlignForGrid(sad::Fusion::GridSearchResult& gr) {
|
||||
/// 多分辨率
|
||||
pcl::NormalDistributionsTransform<PointType, PointType> ndt;
|
||||
ndt.setTransformationEpsilon(0.05);
|
||||
ndt.setStepSize(0.7);
|
||||
ndt.setMaximumIterations(40);
|
||||
|
||||
ndt.setInputSource(current_scan_);
|
||||
auto map = ref_cloud_;
|
||||
|
||||
CloudPtr output(new PointCloudType);
|
||||
std::vector<double> res{10.0, 5.0, 4.0, 3.0};
|
||||
Mat4f T = gr.pose_.matrix().cast<float>();
|
||||
for (auto& r : res) {
|
||||
auto rough_map = VoxelCloud(map, r * 0.1);
|
||||
ndt.setInputTarget(rough_map);
|
||||
ndt.setResolution(r);
|
||||
ndt.align(*output, T);
|
||||
T = ndt.getFinalTransformation();
|
||||
}
|
||||
|
||||
gr.score_ = ndt.getTransformationProbability();
|
||||
gr.result_pose_ = Mat4ToSE3(ndt.getFinalTransformation());
|
||||
}
|
||||
|
||||
bool Fusion::LidarLocalization() {
|
||||
SE3 pred = eskf_.GetNominalSE3();
|
||||
LoadMap(pred);
|
||||
|
||||
ndt_.setInputCloud(current_scan_);
|
||||
CloudPtr output(new PointCloudType);
|
||||
ndt_.align(*output, pred.matrix().cast<float>());
|
||||
|
||||
SE3 pose = Mat4ToSE3(ndt_.getFinalTransformation());
|
||||
eskf_.ObserveSE3(pose, 1e-1, 1e-2);
|
||||
|
||||
LOG(INFO) << "lidar loc score: " << ndt_.getTransformationProbability();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Fusion::LoadMap(const SE3& pose) {
|
||||
int gx = floor((pose.translation().x() - 50.0) / 100);
|
||||
int gy = floor((pose.translation().y() - 50.0) / 100);
|
||||
Vec2i key(gx, gy);
|
||||
|
||||
// 一个区域的周边地图，我们认为9个就够了
|
||||
std::set<Vec2i, less_vec<2>> surrounding_index{
|
||||
key + Vec2i(0, 0), key + Vec2i(-1, 0), key + Vec2i(-1, -1), key + Vec2i(-1, 1), key + Vec2i(0, -1),
|
||||
key + Vec2i(0, 1), key + Vec2i(1, 0), key + Vec2i(1, -1), key + Vec2i(1, 1),
|
||||
};
|
||||
|
||||
// 加载必要区域
|
||||
bool map_data_changed = false;
|
||||
int cnt_new_loaded = 0, cnt_unload = 0;
|
||||
for (auto& k : surrounding_index) {
|
||||
if (map_data_index_.find(k) == map_data_index_.end()) {
|
||||
// 该地图数据不存在
|
||||
continue;
|
||||
}
|
||||
|
||||
if (map_data_.find(k) == map_data_.end()) {
|
||||
// 加载这个区块
|
||||
CloudPtr cloud(new PointCloudType);
|
||||
pcl::io::loadPCDFile(data_path_ + std::to_string(k[0]) + "_" + std::to_string(k[1]) + ".pcd", *cloud);
|
||||
map_data_.emplace(k, cloud);
|
||||
map_data_changed = true;
|
||||
cnt_new_loaded++;
|
||||
}
|
||||
}
|
||||
|
||||
// 卸载不需要的区域，这个稍微加大一点，不需要频繁卸载
|
||||
for (auto iter = map_data_.begin(); iter != map_data_.end();) {
|
||||
if ((iter->first - key).cast<float>().norm() > 3.0) {
|
||||
// 卸载本区块
|
||||
iter = map_data_.erase(iter);
|
||||
cnt_unload++;
|
||||
map_data_changed = true;
|
||||
} else {
|
||||
iter++;
|
||||
}
|
||||
}
|
||||
|
||||
LOG(INFO) << "new loaded: " << cnt_new_loaded << ", unload: " << cnt_unload;
|
||||
if (map_data_changed) {
|
||||
// rebuild ndt target map
|
||||
ref_cloud_.reset(new PointCloudType);
|
||||
for (auto& mp : map_data_) {
|
||||
*ref_cloud_ += *mp.second;
|
||||
}
|
||||
|
||||
LOG(INFO) << "rebuild global cloud, grids: " << map_data_.size();
|
||||
ndt_.setInputTarget(ref_cloud_);
|
||||
}
|
||||
|
||||
ui_->UpdatePointCloudGlobal(map_data_);
|
||||
}
|
||||
|
||||
void Fusion::LoadMapIndex() {
|
||||
std::ifstream fin(data_path_ + "/map_index.txt");
|
||||
while (!fin.eof()) {
|
||||
int x, y;
|
||||
fin >> x >> y;
|
||||
map_data_index_.emplace(Vec2i(x, y));
|
||||
}
|
||||
fin.close();
|
||||
}
|
||||
|
||||
void Fusion::ProcessIMU(IMUPtr imu) { sync_->ProcessIMU(imu); }
|
||||
|
||||
void Fusion::ProcessPointCloud(sensor_msgs::PointCloud2::Ptr cloud) { sync_->ProcessCloud(cloud); }
|
||||
|
||||
} // namespace sad
|
||||
128
workspace/slam_in_autonomous_driving/src/ch10/fusion.h
Normal file
128
workspace/slam_in_autonomous_driving/src/ch10/fusion.h
Normal file
@@ -0,0 +1,128 @@
|
||||
//
|
||||
// Created by xiang on 22-12-20.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_FUSION_H
|
||||
#define SLAM_IN_AUTO_DRIVING_FUSION_H
|
||||
|
||||
#include "ch3/eskf.hpp"
|
||||
#include "ch3/static_imu_init.h"
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/gnss.h"
|
||||
#include "common/imu.h"
|
||||
#include "common/message_def.h"
|
||||
#include "common/point_types.h"
|
||||
|
||||
#include "ch7/loosely_coupled_lio/cloud_convert.h"
|
||||
#include "ch7/loosely_coupled_lio/measure_sync.h"
|
||||
|
||||
#include "tools/ui/pangolin_window.h"
|
||||
|
||||
#include <pcl/registration/ndt.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 第10章显示的高精度融合定位，融合IMU、RTK、激光点云定位功能
|
||||
*
|
||||
* - NOTE 一些IMU的异常处理没有加在这里，有可能会被IMU带歪。
|
||||
*/
|
||||
class Fusion {
|
||||
public:
|
||||
explicit Fusion(const std::string& config_yaml);
|
||||
|
||||
enum class Status {
|
||||
WAITING_FOR_RTK, // 等待初始的RTK
|
||||
WORKING, // 正常工作
|
||||
};
|
||||
|
||||
/// 初始化，读取参数
|
||||
bool Init();
|
||||
|
||||
/// 处理输入的RTK
|
||||
void ProcessRTK(GNSSPtr gnss);
|
||||
void ProcessIMU(IMUPtr imu);
|
||||
void ProcessPointCloud(sensor_msgs::PointCloud2::Ptr cloud);
|
||||
|
||||
private:
|
||||
/// 读取某个点附近的地图
|
||||
void LoadMap(const SE3& pose);
|
||||
|
||||
/// 处理同步之后的IMU和雷达数据
|
||||
void ProcessMeasurements(const MeasureGroup& meas);
|
||||
|
||||
/// 读取地图的索引文件
|
||||
void LoadMapIndex();
|
||||
|
||||
/// 网格搜索时的结构
|
||||
struct GridSearchResult {
|
||||
SE3 pose_;
|
||||
SE3 result_pose_;
|
||||
double score_ = 0.0;
|
||||
};
|
||||
|
||||
/// 在初始RTK附近搜索车辆位置
|
||||
bool SearchRTK();
|
||||
|
||||
/// 对网格搜索的某个点进行配准，得到配准后位姿与配准分值
|
||||
void AlignForGrid(GridSearchResult& gr);
|
||||
|
||||
/// 激光定位
|
||||
bool LidarLocalization();
|
||||
|
||||
/// 使用IMU初始化
|
||||
void TryInitIMU();
|
||||
|
||||
/// 利用IMU预测状态信息
|
||||
/// 这段时间的预测数据会放入imu_states_里
|
||||
void Predict();
|
||||
|
||||
/// 对measures_中的点云去畸变
|
||||
void Undistort();
|
||||
|
||||
/// 执行一次配准和观测
|
||||
void Align();
|
||||
|
||||
/// 标志位
|
||||
Status status_ = Status::WAITING_FOR_RTK;
|
||||
|
||||
/// 数据
|
||||
std::string config_yaml_; // 配置文件路径
|
||||
Vec3d map_origin_ = Vec3d::Zero(); // 地图原点
|
||||
std::string data_path_; // 地图数据目录
|
||||
std::set<Vec2i, less_vec<2>> map_data_index_; // 哪些格子存在地图数据
|
||||
std::map<Vec2i, CloudPtr, less_vec<2>> map_data_; // 第9章建立的地图数据
|
||||
|
||||
std::shared_ptr<MessageSync> sync_ = nullptr; // 消息同步器
|
||||
StaticIMUInit imu_init_; // IMU静止初始化
|
||||
|
||||
/// 滤波器
|
||||
ESKFD eskf_;
|
||||
std::vector<NavStated> imu_states_; // ESKF预测期间的状态
|
||||
|
||||
FullCloudPtr scan_undistort_{new FullPointCloudType()}; // scan after undistortion
|
||||
CloudPtr current_scan_ = nullptr;
|
||||
|
||||
SE3 TIL_;
|
||||
MeasureGroup measures_; // sync IMU and lidar scan
|
||||
GNSSPtr last_gnss_ = nullptr;
|
||||
|
||||
bool init_has_failed_ = false; // 初始化是否失败过
|
||||
SE3 last_searched_pos_; // 上次搜索的GNSS位置
|
||||
|
||||
/// 激光定位
|
||||
bool imu_need_init_ = true; // 是否需要估计IMU初始零偏
|
||||
CloudPtr ref_cloud_ = nullptr; // NDT用于参考的点云
|
||||
pcl::NormalDistributionsTransform<PointType, PointType> ndt_;
|
||||
|
||||
/// 参数
|
||||
double rtk_search_min_score_ = 4.5;
|
||||
|
||||
// ui
|
||||
std::shared_ptr<ui::PangolinWindow> ui_ = nullptr;
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_FUSION_H
|
||||
@@ -0,0 +1,48 @@
|
||||
//
|
||||
// Created by xiang on 22-12-20.
|
||||
//
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include "common/io_utils.h"
|
||||
#include "fusion.h"
|
||||
|
||||
DEFINE_string(config_yaml, "./config/mapping.yaml", "配置文件");
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
sad::Fusion fusion(FLAGS_config_yaml);
|
||||
if (!fusion.Init()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
auto yaml = YAML::LoadFile(FLAGS_config_yaml);
|
||||
auto bag_path = yaml["bag_path"].as<std::string>();
|
||||
sad::RosbagIO rosbag_io(bag_path, sad::DatasetType::NCLT);
|
||||
|
||||
/// 把各种消息交给fusion
|
||||
rosbag_io
|
||||
.AddAutoRTKHandle([&fusion](GNSSPtr gnss) {
|
||||
fusion.ProcessRTK(gnss);
|
||||
return true;
|
||||
})
|
||||
.AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
|
||||
fusion.ProcessPointCloud(cloud);
|
||||
return true;
|
||||
})
|
||||
.AddImuHandle([&](IMUPtr imu) {
|
||||
fusion.ProcessIMU(imu);
|
||||
return true;
|
||||
})
|
||||
.Go();
|
||||
|
||||
LOG(INFO) << "done.";
|
||||
sleep(10);
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,5 @@
|
||||
add_executable(motion motion.cc)
|
||||
target_link_libraries(motion
|
||||
${PROJECT_NAME}.common
|
||||
${PROJECT_NAME}.tools
|
||||
)
|
||||
59
workspace/slam_in_autonomous_driving/src/ch2/motion.cc
Normal file
59
workspace/slam_in_autonomous_driving/src/ch2/motion.cc
Normal file
@@ -0,0 +1,59 @@
|
||||
//
|
||||
// Created by xiang on 22-12-29.
|
||||
//
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/math_utils.h"
|
||||
#include "tools/ui/pangolin_window.h"
|
||||
|
||||
/// 本节程序演示一个正在作圆周运动的车辆
|
||||
/// 车辆的角速度与线速度可以在flags中设置
|
||||
|
||||
DEFINE_double(angular_velocity, 10.0, "角速度（角度）制");
|
||||
DEFINE_double(linear_velocity, 5.0, "车辆前进线速度 m/s");
|
||||
DEFINE_bool(use_quaternion, false, "是否使用四元数计算");
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
/// 可视化
|
||||
sad::ui::PangolinWindow ui;
|
||||
if (ui.Init() == false) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
double angular_velocity_rad = FLAGS_angular_velocity * sad::math::kDEG2RAD; // 弧度制角速度
|
||||
SE3 pose; // TWB表示的位姿
|
||||
Vec3d omega(0, 0, angular_velocity_rad); // 角速度矢量
|
||||
Vec3d v_body(FLAGS_linear_velocity, 0, 0); // 本体系速度
|
||||
const double dt = 0.05; // 每次更新的时间
|
||||
|
||||
while (ui.ShouldQuit() == false) {
|
||||
// 更新自身位置
|
||||
Vec3d v_world = pose.so3() * v_body;
|
||||
pose.translation() += v_world * dt;
|
||||
|
||||
// 更新自身旋转
|
||||
if (FLAGS_use_quaternion) {
|
||||
Quatd q = pose.unit_quaternion() * Quatd(1, 0.5 * omega[0] * dt, 0.5 * omega[1] * dt, 0.5 * omega[2] * dt);
|
||||
q.normalize();
|
||||
pose.so3() = SO3(q);
|
||||
} else {
|
||||
pose.so3() = pose.so3() * SO3::exp(omega * dt);
|
||||
}
|
||||
|
||||
LOG(INFO) << "pose: " << pose.translation().transpose();
|
||||
ui.UpdateNavState(sad::NavStated(0, pose, v_world));
|
||||
|
||||
usleep(dt * 1e6);
|
||||
}
|
||||
|
||||
ui.Quit();
|
||||
return 0;
|
||||
}
|
||||
32
workspace/slam_in_autonomous_driving/src/ch3/CMakeLists.txt
Normal file
32
workspace/slam_in_autonomous_driving/src/ch3/CMakeLists.txt
Normal file
@@ -0,0 +1,32 @@
|
||||
add_executable(run_imu_integration
|
||||
run_imu_integration.cc
|
||||
)
|
||||
|
||||
target_link_libraries(run_imu_integration
|
||||
glog gflags ${PROJECT_NAME}.common
|
||||
)
|
||||
|
||||
add_executable(run_eskf_gins run_eskf_gins.cc)
|
||||
target_link_libraries(run_eskf_gins
|
||||
glog gflags ${PROJECT_NAME}.common ${PROJECT_NAME}.ch3
|
||||
)
|
||||
|
||||
add_executable(process_gnss process_gnss.cc)
|
||||
target_link_libraries(process_gnss
|
||||
glog gflags ${PROJECT_NAME}.common ${PROJECT_NAME}.ch3
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}.ch3
|
||||
static_imu_init.cc
|
||||
utm_convert.cc
|
||||
# ieskf/nav_state_manifold.cc
|
||||
# ieskf/ieskf.cc
|
||||
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/utm_convert/utm.cc
|
||||
${PROJECT_SOURCE_DIR}/thirdparty/utm_convert/tranmerc.cc
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}.ch3
|
||||
glog gflags ${PROJECT_NAME}.common
|
||||
)
|
||||
|
||||
336
workspace/slam_in_autonomous_driving/src/ch3/eskf.hpp
Normal file
336
workspace/slam_in_autonomous_driving/src/ch3/eskf.hpp
Normal file
@@ -0,0 +1,336 @@
|
||||
//
|
||||
// Created by xiang on 2021/11/11.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_ESKF_HPP
|
||||
#define SLAM_IN_AUTO_DRIVING_ESKF_HPP
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/gnss.h"
|
||||
#include "common/imu.h"
|
||||
#include "common/math_utils.h"
|
||||
#include "common/nav_state.h"
|
||||
#include "common/odom.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <iomanip>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 书本第3章介绍的误差卡尔曼滤波器
|
||||
* 可以指定观测GNSS的读数，GNSS应该事先转换到车体坐标系
|
||||
*
|
||||
* 本书使用18维的ESKF，标量类型可以由S指定，默认取double
|
||||
* 变量顺序：p, v, R, bg, ba, grav，与书本对应
|
||||
* @tparam S 状态变量的精度，取float或double
|
||||
*/
|
||||
template <typename S = double>
|
||||
class ESKF {
|
||||
public:
|
||||
/// 类型定义
|
||||
using SO3 = Sophus::SO3<S>; // 旋转变量类型
|
||||
using VecT = Eigen::Matrix<S, 3, 1>; // 向量类型
|
||||
using Vec18T = Eigen::Matrix<S, 18, 1>; // 18维向量类型
|
||||
using Mat3T = Eigen::Matrix<S, 3, 3>; // 3x3矩阵类型
|
||||
using MotionNoiseT = Eigen::Matrix<S, 18, 18>; // 运动噪声类型
|
||||
using OdomNoiseT = Eigen::Matrix<S, 3, 3>; // 里程计噪声类型
|
||||
using GnssNoiseT = Eigen::Matrix<S, 6, 6>; // GNSS噪声类型
|
||||
using Mat18T = Eigen::Matrix<S, 18, 18>; // 18维方差类型
|
||||
using NavStateT = NavState<S>; // 整体名义状态变量类型
|
||||
|
||||
struct Options {
|
||||
Options() = default;
|
||||
|
||||
/// IMU 测量与零偏参数
|
||||
double imu_dt_ = 0.01; // IMU测量间隔
|
||||
// NOTE IMU噪声项都为离散时间，不需要再乘dt，可以由初始化器指定IMU噪声
|
||||
double gyro_var_ = 1e-5; // 陀螺测量标准差
|
||||
double acce_var_ = 1e-2; // 加计测量标准差
|
||||
double bias_gyro_var_ = 1e-6; // 陀螺零偏游走标准差
|
||||
double bias_acce_var_ = 1e-4; // 加计零偏游走标准差
|
||||
|
||||
/// 里程计参数
|
||||
double odom_var_ = 0.5;
|
||||
double odom_span_ = 0.1; // 里程计测量间隔
|
||||
double wheel_radius_ = 0.155; // 轮子半径
|
||||
double circle_pulse_ = 1024.0; // 编码器每圈脉冲数
|
||||
|
||||
/// RTK 观测参数
|
||||
double gnss_pos_noise_ = 0.1; // GNSS位置噪声
|
||||
double gnss_height_noise_ = 0.1; // GNSS高度噪声
|
||||
double gnss_ang_noise_ = 1.0 * math::kDEG2RAD; // GNSS旋转噪声
|
||||
|
||||
/// 其他配置
|
||||
bool update_bias_gyro_ = true; // 是否更新陀螺bias
|
||||
bool update_bias_acce_ = true; // 是否更新加计bias
|
||||
};
|
||||
|
||||
/**
|
||||
* 初始零偏取零
|
||||
*/
|
||||
ESKF(Options option = Options()) : options_(option) { BuildNoise(option); }
|
||||
|
||||
/**
|
||||
* 设置初始条件
|
||||
* @param options 噪声项配置
|
||||
* @param init_bg 初始零偏 陀螺
|
||||
* @param init_ba 初始零偏 加计
|
||||
* @param gravity 重力
|
||||
*/
|
||||
void SetInitialConditions(Options options, const VecT& init_bg, const VecT& init_ba,
|
||||
const VecT& gravity = VecT(0, 0, -9.8)) {
|
||||
BuildNoise(options);
|
||||
options_ = options;
|
||||
bg_ = init_bg;
|
||||
ba_ = init_ba;
|
||||
g_ = gravity;
|
||||
cov_ = Mat18T::Identity() * 1e-4;
|
||||
}
|
||||
|
||||
/// 使用IMU递推
|
||||
bool Predict(const IMU& imu);
|
||||
|
||||
/// 使用轮速计观测
|
||||
bool ObserveWheelSpeed(const Odom& odom);
|
||||
|
||||
/// 使用GPS观测
|
||||
bool ObserveGps(const GNSS& gnss);
|
||||
|
||||
/**
|
||||
* 使用SE3进行观测
|
||||
* @param pose 观测位姿
|
||||
* @param trans_noise 平移噪声
|
||||
* @param ang_noise 角度噪声
|
||||
* @return
|
||||
*/
|
||||
bool ObserveSE3(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
|
||||
|
||||
/// accessors
|
||||
/// 获取全量状态
|
||||
NavStateT GetNominalState() const { return NavStateT(current_time_, R_, p_, v_, bg_, ba_); }
|
||||
|
||||
/// 获取SE3 状态
|
||||
SE3 GetNominalSE3() const { return SE3(R_, p_); }
|
||||
|
||||
/// 设置状态X
|
||||
void SetX(const NavStated& x, const Vec3d& grav) {
|
||||
current_time_ = x.timestamp_;
|
||||
R_ = x.R_;
|
||||
p_ = x.p_;
|
||||
v_ = x.v_;
|
||||
bg_ = x.bg_;
|
||||
ba_ = x.ba_;
|
||||
g_ = grav;
|
||||
}
|
||||
|
||||
/// 设置协方差
|
||||
void SetCov(const Mat18T& cov) { cov_ = cov; }
|
||||
|
||||
/// 获取重力
|
||||
Vec3d GetGravity() const { return g_; }
|
||||
|
||||
private:
|
||||
void BuildNoise(const Options& options) {
|
||||
double ev = options.acce_var_;
|
||||
double et = options.gyro_var_;
|
||||
double eg = options.bias_gyro_var_;
|
||||
double ea = options.bias_acce_var_;
|
||||
|
||||
double ev2 = ev; // * ev;
|
||||
double et2 = et; // * et;
|
||||
double eg2 = eg; // * eg;
|
||||
double ea2 = ea; // * ea;
|
||||
|
||||
// 设置过程噪声
|
||||
Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;
|
||||
|
||||
// 设置里程计噪声
|
||||
double o2 = options_.odom_var_ * options_.odom_var_;
|
||||
odom_noise_.diagonal() << o2, o2, o2;
|
||||
|
||||
// 设置GNSS状态
|
||||
double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;
|
||||
double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;
|
||||
double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;
|
||||
gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
|
||||
}
|
||||
|
||||
/// 更新名义状态变量，重置error state
|
||||
void UpdateAndReset() {
|
||||
p_ += dx_.template block<3, 1>(0, 0);
|
||||
v_ += dx_.template block<3, 1>(3, 0);
|
||||
R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));
|
||||
|
||||
if (options_.update_bias_gyro_) {
|
||||
bg_ += dx_.template block<3, 1>(9, 0);
|
||||
}
|
||||
|
||||
if (options_.update_bias_acce_) {
|
||||
ba_ += dx_.template block<3, 1>(12, 0);
|
||||
}
|
||||
|
||||
g_ += dx_.template block<3, 1>(15, 0);
|
||||
|
||||
ProjectCov();
|
||||
dx_.setZero();
|
||||
}
|
||||
|
||||
/// 对P阵进行投影，参考式(3.63)
|
||||
void ProjectCov() {
|
||||
Mat18T J = Mat18T::Identity();
|
||||
J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
|
||||
cov_ = J * cov_ * J.transpose();
|
||||
}
|
||||
|
||||
/// 成员变量
|
||||
double current_time_ = 0.0; // 当前时间
|
||||
|
||||
/// 名义状态
|
||||
VecT p_ = VecT::Zero();
|
||||
VecT v_ = VecT::Zero();
|
||||
SO3 R_;
|
||||
VecT bg_ = VecT::Zero();
|
||||
VecT ba_ = VecT::Zero();
|
||||
VecT g_{0, 0, -9.8};
|
||||
|
||||
/// 误差状态
|
||||
Vec18T dx_ = Vec18T::Zero();
|
||||
|
||||
/// 协方差阵
|
||||
Mat18T cov_ = Mat18T::Identity();
|
||||
|
||||
/// 噪声阵
|
||||
MotionNoiseT Q_ = MotionNoiseT::Zero();
|
||||
OdomNoiseT odom_noise_ = OdomNoiseT::Zero();
|
||||
GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();
|
||||
|
||||
/// 标志位
|
||||
bool first_gnss_ = true; // 是否为第一个gnss数据
|
||||
|
||||
/// 配置项
|
||||
Options options_;
|
||||
};
|
||||
|
||||
using ESKFD = ESKF<double>;
|
||||
using ESKFF = ESKF<float>;
|
||||
|
||||
template <typename S>
|
||||
bool ESKF<S>::Predict(const IMU& imu) {
|
||||
assert(imu.timestamp_ >= current_time_);
|
||||
|
||||
double dt = imu.timestamp_ - current_time_;
|
||||
if (dt > (5 * options_.imu_dt_) || dt < 0) {
|
||||
// 时间间隔不对，可能是第一个IMU数据，没有历史信息
|
||||
LOG(INFO) << "skip this imu because dt_ = " << dt;
|
||||
current_time_ = imu.timestamp_;
|
||||
return false;
|
||||
}
|
||||
|
||||
// nominal state 递推
|
||||
VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
|
||||
VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
|
||||
SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);
|
||||
|
||||
R_ = new_R;
|
||||
v_ = new_v;
|
||||
p_ = new_p;
|
||||
// 其余状态维度不变
|
||||
|
||||
// error state 递推
|
||||
// 计算运动过程雅可比矩阵 F，见(3.47)
|
||||
// F实际上是稀疏矩阵，也可以不用矩阵形式进行相乘而是写成散装形式，这里为了教学方便，使用矩阵形式
|
||||
Mat18T F = Mat18T::Identity(); // 主对角线
|
||||
F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt; // p 对 v
|
||||
F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt; // v对theta
|
||||
F.template block<3, 3>(3, 12) = -R_.matrix() * dt; // v 对 ba
|
||||
F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt; // v 对 g
|
||||
F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix(); // theta 对 theta
|
||||
F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt; // theta 对 bg
|
||||
|
||||
// mean and cov prediction
|
||||
dx_ = F * dx_; // 这行其实没必要算，dx_在重置之后应该为零，因此这步可以跳过，但F需要参与Cov部分计算，所以保留
|
||||
cov_ = F * cov_.eval() * F.transpose() + Q_;
|
||||
current_time_ = imu.timestamp_;
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename S>
|
||||
bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {
|
||||
assert(odom.timestamp_ >= current_time_);
|
||||
// odom 修正以及雅可比
|
||||
// 使用三维的轮速观测，H为3x18，大部分为零
|
||||
Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
|
||||
H.template block<3, 3>(0, 3) = Mat3T::Identity();
|
||||
|
||||
// 卡尔曼增益
|
||||
Eigen::Matrix<S, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();
|
||||
|
||||
// velocity obs
|
||||
double velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
|
||||
double velo_r =
|
||||
options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
|
||||
double average_vel = 0.5 * (velo_l + velo_r);
|
||||
|
||||
VecT vel_odom(average_vel, 0.0, 0.0);
|
||||
VecT vel_world = R_ * vel_odom;
|
||||
|
||||
dx_ = K * (vel_world - v_);
|
||||
|
||||
// update cov
|
||||
cov_ = (Mat18T::Identity() - K * H) * cov_;
|
||||
|
||||
UpdateAndReset();
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename S>
|
||||
bool ESKF<S>::ObserveGps(const GNSS& gnss) {
|
||||
/// GNSS 观测的修正
|
||||
assert(gnss.unix_time_ >= current_time_);
|
||||
|
||||
if (first_gnss_) {
|
||||
R_ = gnss.utm_pose_.so3();
|
||||
p_ = gnss.utm_pose_.translation();
|
||||
first_gnss_ = false;
|
||||
current_time_ = gnss.unix_time_;
|
||||
return true;
|
||||
}
|
||||
|
||||
assert(gnss.heading_valid_);
|
||||
ObserveSE3(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
|
||||
current_time_ = gnss.unix_time_;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename S>
|
||||
bool ESKF<S>::ObserveSE3(const SE3& pose, double trans_noise, double ang_noise) {
|
||||
/// 既有旋转，也有平移
|
||||
/// 观测状态变量中的p, R，H为6x18，其余为零
|
||||
Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();
|
||||
H.template block<3, 3>(0, 0) = Mat3T::Identity(); // P部分
|
||||
H.template block<3, 3>(3, 6) = Mat3T::Identity(); // R部分（3.66)
|
||||
|
||||
// 卡尔曼增益和更新过程
|
||||
Vec6d noise_vec;
|
||||
noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;
|
||||
|
||||
Mat6d V = noise_vec.asDiagonal();
|
||||
Eigen::Matrix<S, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();
|
||||
|
||||
// 更新x和cov
|
||||
Vec6d innov = Vec6d::Zero();
|
||||
innov.template head<3>() = (pose.translation() - p_); // 平移部分
|
||||
innov.template tail<3>() = (R_.inverse() * pose.so3()).log(); // 旋转部分(3.67)
|
||||
|
||||
dx_ = K * innov;
|
||||
cov_ = (Mat18T::Identity() - K * H) * cov_;
|
||||
|
||||
UpdateAndReset();
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_ESKF_HPP
|
||||
@@ -0,0 +1,60 @@
|
||||
//
|
||||
// Created by xiang on 2021/11/5.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
|
||||
#define SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/imu.h"
|
||||
#include "common/nav_state.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 本程序演示单纯靠IMU的积分
|
||||
*/
|
||||
class IMUIntegration {
|
||||
public:
|
||||
IMUIntegration(const Vec3d& gravity, const Vec3d& init_bg, const Vec3d& init_ba)
|
||||
: gravity_(gravity), bg_(init_bg), ba_(init_ba) {}
|
||||
|
||||
// 增加imu读数
|
||||
void AddIMU(const IMU& imu) {
|
||||
double dt = imu.timestamp_ - timestamp_;
|
||||
if (dt > 0 && dt < 0.1) {
|
||||
// 假设IMU时间间隔在0至0.1以内
|
||||
p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;
|
||||
v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
|
||||
R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
|
||||
}
|
||||
|
||||
// 更新时间
|
||||
timestamp_ = imu.timestamp_;
|
||||
}
|
||||
|
||||
/// 组成NavState
|
||||
NavStated GetNavState() const { return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }
|
||||
|
||||
SO3 GetR() const { return R_; }
|
||||
Vec3d GetV() const { return v_; }
|
||||
Vec3d GetP() const { return p_; }
|
||||
|
||||
private:
|
||||
// 累计量
|
||||
SO3 R_;
|
||||
Vec3d v_ = Vec3d::Zero();
|
||||
Vec3d p_ = Vec3d::Zero();
|
||||
|
||||
double timestamp_ = 0.0;
|
||||
|
||||
// 零偏，由外部设定
|
||||
Vec3d bg_ = Vec3d::Zero();
|
||||
Vec3d ba_ = Vec3d::Zero();
|
||||
|
||||
Vec3d gravity_ = Vec3d(0, 0, -9.8); // 重力
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
|
||||
94
workspace/slam_in_autonomous_driving/src/ch3/process_gnss.cc
Normal file
94
workspace/slam_in_autonomous_driving/src/ch3/process_gnss.cc
Normal file
@@ -0,0 +1,94 @@
|
||||
//
|
||||
// Created by xiang on 2022/1/4.
|
||||
//
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <iomanip>
|
||||
#include <memory>
|
||||
|
||||
#include "common/gnss.h"
|
||||
#include "common/io_utils.h"
|
||||
#include "tools/ui/pangolin_window.h"
|
||||
#include "utm_convert.h"
|
||||
|
||||
DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
|
||||
|
||||
// 以下参数仅针对本书提供的数据
|
||||
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
|
||||
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
|
||||
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
|
||||
DEFINE_bool(with_ui, true, "是否显示图形界面");
|
||||
|
||||
/**
|
||||
* 本程序演示如何处理GNSS数据
|
||||
* 我们将GNSS原始读数处理成能够进行后续处理的6自由度Pose
|
||||
* 需要处理UTM转换、RTK天线外参、坐标系转换三个步骤
|
||||
*
|
||||
* 我们将结果保存在文件中，然后用python脚本进行可视化
|
||||
*/
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (fLS::FLAGS_txt_path.empty()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
sad::TxtIO io(fLS::FLAGS_txt_path);
|
||||
|
||||
std::ofstream fout("./data/ch3/gnss_output.txt");
|
||||
Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
|
||||
|
||||
auto save_result = [](std::ofstream& fout, double timestamp, const SE3& pose) {
|
||||
auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
|
||||
auto save_quat = [](std::ofstream& fout, const Quatd& q) {
|
||||
fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
|
||||
};
|
||||
|
||||
fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
|
||||
save_vec3(fout, pose.translation());
|
||||
save_quat(fout, pose.unit_quaternion());
|
||||
fout << std::endl;
|
||||
};
|
||||
|
||||
std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
|
||||
if (FLAGS_with_ui) {
|
||||
ui = std::make_shared<sad::ui::PangolinWindow>();
|
||||
ui->Init();
|
||||
}
|
||||
|
||||
bool first_gnss_set = false;
|
||||
Vec3d origin = Vec3d::Zero();
|
||||
io.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
|
||||
sad::GNSS gnss_out = gnss;
|
||||
if (sad::ConvertGps2UTM(gnss_out, antenna_pos, FLAGS_antenna_angle)) {
|
||||
if (!first_gnss_set) {
|
||||
origin = gnss_out.utm_pose_.translation();
|
||||
first_gnss_set = true;
|
||||
}
|
||||
|
||||
/// 减掉一个原点
|
||||
gnss_out.utm_pose_.translation() -= origin;
|
||||
|
||||
save_result(fout, gnss_out.unix_time_, gnss_out.utm_pose_);
|
||||
|
||||
if (ui) {
|
||||
ui->UpdateNavState(
|
||||
sad::NavStated(gnss_out.unix_time_, gnss_out.utm_pose_.so3(), gnss_out.utm_pose_.translation()));
|
||||
usleep(1e3);
|
||||
}
|
||||
}
|
||||
}).Go();
|
||||
|
||||
if (ui) {
|
||||
while (!ui->ShouldQuit()) {
|
||||
usleep(1e5);
|
||||
}
|
||||
ui->Quit();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
154
workspace/slam_in_autonomous_driving/src/ch3/run_eskf_gins.cc
Normal file
154
workspace/slam_in_autonomous_driving/src/ch3/run_eskf_gins.cc
Normal file
@@ -0,0 +1,154 @@
|
||||
//
|
||||
// Created by xiang on 2021/11/11.
|
||||
//
|
||||
|
||||
#include "ch3/eskf.hpp"
|
||||
#include "ch3/static_imu_init.h"
|
||||
#include "common/io_utils.h"
|
||||
#include "tools/ui/pangolin_window.h"
|
||||
#include "utm_convert.h"
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
|
||||
DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
|
||||
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
|
||||
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
|
||||
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
|
||||
DEFINE_bool(with_ui, true, "是否显示图形界面");
|
||||
DEFINE_bool(with_odom, false, "是否加入轮速计信息");
|
||||
|
||||
/**
|
||||
* 本程序演示使用RTK+IMU进行组合导航
|
||||
*/
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (fLS::FLAGS_txt_path.empty()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 初始化器
|
||||
sad::StaticIMUInit imu_init; // 使用默认配置
|
||||
sad::ESKFD eskf;
|
||||
|
||||
sad::TxtIO io(FLAGS_txt_path);
|
||||
Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
|
||||
|
||||
auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
|
||||
auto save_quat = [](std::ofstream& fout, const Quatd& q) {
|
||||
fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
|
||||
};
|
||||
|
||||
auto save_result = [&save_vec3, &save_quat](std::ofstream& fout, const sad::NavStated& save_state) {
|
||||
fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);
|
||||
save_vec3(fout, save_state.p_);
|
||||
save_quat(fout, save_state.R_.unit_quaternion());
|
||||
save_vec3(fout, save_state.v_);
|
||||
save_vec3(fout, save_state.bg_);
|
||||
save_vec3(fout, save_state.ba_);
|
||||
fout << std::endl;
|
||||
};
|
||||
|
||||
std::ofstream fout("./data/ch3/gins.txt");
|
||||
bool imu_inited = false, gnss_inited = false;
|
||||
|
||||
std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
|
||||
if (FLAGS_with_ui) {
|
||||
ui = std::make_shared<sad::ui::PangolinWindow>();
|
||||
ui->Init();
|
||||
}
|
||||
|
||||
/// 设置各类回调函数
|
||||
bool first_gnss_set = false;
|
||||
Vec3d origin = Vec3d::Zero();
|
||||
|
||||
io.SetIMUProcessFunc([&](const sad::IMU& imu) {
|
||||
/// IMU 处理函数
|
||||
if (!imu_init.InitSuccess()) {
|
||||
imu_init.AddIMU(imu);
|
||||
return;
|
||||
}
|
||||
|
||||
/// 需要IMU初始化
|
||||
if (!imu_inited) {
|
||||
// 读取初始零偏，设置ESKF
|
||||
sad::ESKFD::Options options;
|
||||
// 噪声由初始化器估计
|
||||
options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);
|
||||
options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);
|
||||
eskf.SetInitialConditions(options, imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity());
|
||||
imu_inited = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!gnss_inited) {
|
||||
/// 等待有效的RTK数据
|
||||
return;
|
||||
}
|
||||
|
||||
/// GNSS 也接收到之后，再开始进行预测
|
||||
eskf.Predict(imu);
|
||||
|
||||
/// predict就会更新ESKF，所以此时就可以发送数据
|
||||
auto state = eskf.GetNominalState();
|
||||
if (ui) {
|
||||
ui->UpdateNavState(state);
|
||||
}
|
||||
|
||||
/// 记录数据以供绘图
|
||||
save_result(fout, state);
|
||||
|
||||
usleep(1e3);
|
||||
})
|
||||
.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
|
||||
/// GNSS 处理函数
|
||||
if (!imu_inited) {
|
||||
return;
|
||||
}
|
||||
|
||||
sad::GNSS gnss_convert = gnss;
|
||||
if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
|
||||
return;
|
||||
}
|
||||
|
||||
/// 去掉原点
|
||||
if (!first_gnss_set) {
|
||||
origin = gnss_convert.utm_pose_.translation();
|
||||
first_gnss_set = true;
|
||||
}
|
||||
gnss_convert.utm_pose_.translation() -= origin;
|
||||
|
||||
// 要求RTK heading有效，才能合入ESKF
|
||||
eskf.ObserveGps(gnss_convert);
|
||||
|
||||
auto state = eskf.GetNominalState();
|
||||
if (ui) {
|
||||
ui->UpdateNavState(state);
|
||||
}
|
||||
save_result(fout, state);
|
||||
|
||||
gnss_inited = true;
|
||||
})
|
||||
.SetOdomProcessFunc([&](const sad::Odom& odom) {
|
||||
/// Odom 处理函数，本章Odom只给初始化使用
|
||||
imu_init.AddOdom(odom);
|
||||
if (FLAGS_with_odom && imu_inited && gnss_inited) {
|
||||
eskf.ObserveWheelSpeed(odom);
|
||||
}
|
||||
})
|
||||
.Go();
|
||||
|
||||
while (ui && !ui->ShouldQuit()) {
|
||||
usleep(1e5);
|
||||
}
|
||||
if (ui) {
|
||||
ui->Quit();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,77 @@
|
||||
//
|
||||
// Created by xiang on 2021/11/5.
|
||||
//
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <iomanip>
|
||||
|
||||
#include "ch3/imu_integration.h"
|
||||
#include "common/io_utils.h"
|
||||
#include "tools/ui/pangolin_window.h"
|
||||
|
||||
DEFINE_string(imu_txt_path, "./data/ch3/10.txt", "数据文件路径");
|
||||
DEFINE_bool(with_ui, true, "是否显示图形界面");
|
||||
|
||||
/// 本程序演示如何对IMU进行直接积分
|
||||
/// 该程序需要输入data/ch3/下的文本文件，同时它将状态输出到data/ch3/state.txt中，在UI中也可以观察到车辆运动
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (FLAGS_imu_txt_path.empty()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
sad::TxtIO io(FLAGS_imu_txt_path);
|
||||
|
||||
// 该实验中，我们假设零偏已知
|
||||
Vec3d gravity(0, 0, -9.8); // 重力方向
|
||||
Vec3d init_bg(00.000224886, -7.61038e-05, -0.000742259);
|
||||
Vec3d init_ba(-0.165205, 0.0926887, 0.0058049);
|
||||
|
||||
sad::IMUIntegration imu_integ(gravity, init_bg, init_ba);
|
||||
|
||||
std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
|
||||
if (FLAGS_with_ui) {
|
||||
ui = std::make_shared<sad::ui::PangolinWindow>();
|
||||
ui->Init();
|
||||
}
|
||||
|
||||
/// 记录结果
|
||||
auto save_result = [](std::ofstream& fout, double timestamp, const Sophus::SO3d& R, const Vec3d& v,
|
||||
const Vec3d& p) {
|
||||
auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
|
||||
auto save_quat = [](std::ofstream& fout, const Quatd& q) {
|
||||
fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
|
||||
};
|
||||
|
||||
fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
|
||||
save_vec3(fout, p);
|
||||
save_quat(fout, R.unit_quaternion());
|
||||
save_vec3(fout, v);
|
||||
fout << std::endl;
|
||||
};
|
||||
|
||||
std::ofstream fout("./data/ch3/state.txt");
|
||||
io.SetIMUProcessFunc([&imu_integ, &save_result, &fout, &ui](const sad::IMU& imu) {
|
||||
imu_integ.AddIMU(imu);
|
||||
save_result(fout, imu.timestamp_, imu_integ.GetR(), imu_integ.GetV(), imu_integ.GetP());
|
||||
if (ui) {
|
||||
ui->UpdateNavState(imu_integ.GetNavState());
|
||||
usleep(1e2);
|
||||
}
|
||||
}).Go();
|
||||
|
||||
// 打开了可视化的话，等待界面退出
|
||||
while (ui && !ui->ShouldQuit()) {
|
||||
usleep(1e4);
|
||||
}
|
||||
|
||||
if (ui) {
|
||||
ui->Quit();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
104
workspace/slam_in_autonomous_driving/src/ch3/static_imu_init.cc
Normal file
104
workspace/slam_in_autonomous_driving/src/ch3/static_imu_init.cc
Normal file
@@ -0,0 +1,104 @@
|
||||
//
|
||||
// Created by xiang on 2021/11/11.
|
||||
//
|
||||
|
||||
#include "ch3/static_imu_init.h"
|
||||
#include "common/math_utils.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
bool StaticIMUInit::AddIMU(const IMU& imu) {
|
||||
if (init_success_) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (options_.use_speed_for_static_checking_ && !is_static_) {
|
||||
LOG(WARNING) << "等待车辆静止";
|
||||
init_imu_deque_.clear();
|
||||
return false;
|
||||
}
|
||||
|
||||
if (init_imu_deque_.empty()) {
|
||||
// 记录初始静止时间
|
||||
init_start_time_ = imu.timestamp_;
|
||||
}
|
||||
|
||||
// 记入初始化队列
|
||||
init_imu_deque_.push_back(imu);
|
||||
|
||||
double init_time = imu.timestamp_ - init_start_time_; // 初始化经过时间
|
||||
if (init_time > options_.init_time_seconds_) {
|
||||
// 尝试初始化逻辑
|
||||
TryInit();
|
||||
}
|
||||
|
||||
// 维持初始化队列长度
|
||||
while (init_imu_deque_.size() > options_.init_imu_queue_max_size_) {
|
||||
init_imu_deque_.pop_front();
|
||||
}
|
||||
|
||||
current_time_ = imu.timestamp_;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool StaticIMUInit::AddOdom(const Odom& odom) {
|
||||
// 判断车辆是否静止
|
||||
if (init_success_) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (odom.left_pulse_ < options_.static_odom_pulse_ && odom.right_pulse_ < options_.static_odom_pulse_) {
|
||||
is_static_ = true;
|
||||
} else {
|
||||
is_static_ = false;
|
||||
}
|
||||
|
||||
current_time_ = odom.timestamp_;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool StaticIMUInit::TryInit() {
|
||||
if (init_imu_deque_.size() < 10) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 计算均值和方差
|
||||
Vec3d mean_gyro, mean_acce;
|
||||
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });
|
||||
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });
|
||||
|
||||
// 以acce均值为方向，取9.8长度为重力
|
||||
LOG(INFO) << "mean acce: " << mean_acce.transpose();
|
||||
gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;
|
||||
|
||||
// 重新计算加计的协方差
|
||||
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
|
||||
[this](const IMU& imu) { return imu.acce_ + gravity_; });
|
||||
|
||||
// 检查IMU噪声
|
||||
if (cov_gyro_.norm() > options_.max_static_gyro_var) {
|
||||
LOG(ERROR) << "陀螺仪测量噪声太大" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (cov_acce_.norm() > options_.max_static_acce_var) {
|
||||
LOG(ERROR) << "加计测量噪声太大" << cov_acce_.norm() << " > " << options_.max_static_acce_var;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 估计测量噪声和零偏
|
||||
init_bg_ = mean_gyro;
|
||||
init_ba_ = mean_acce;
|
||||
|
||||
LOG(INFO) << "IMU 初始化成功，初始化时间= " << current_time_ - init_start_time_ << ", bg = " << init_bg_.transpose()
|
||||
<< ", ba = " << init_ba_.transpose() << ", gyro sq = " << cov_gyro_.transpose()
|
||||
<< ", acce sq = " << cov_acce_.transpose() << ", grav = " << gravity_.transpose()
|
||||
<< ", norm: " << gravity_.norm();
|
||||
LOG(INFO) << "mean gyro: " << mean_gyro.transpose() << " acce: " << mean_acce.transpose();
|
||||
init_success_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
@@ -0,0 +1,74 @@
|
||||
//
|
||||
// Created by xiang on 2021/11/11.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
|
||||
#define SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/imu.h"
|
||||
#include "common/odom.h"
|
||||
|
||||
#include <deque>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* IMU水平静止状态下初始化器
|
||||
* 使用方法：调用AddIMU, AddOdom添加数据，使用InitSuccess获取初始化是否成功
|
||||
* 成功后，使用各Get函数获取内部参数
|
||||
*
|
||||
* 初始化器在每次调用AddIMU时尝试对系统进行初始化。在有odom的场合，初始化要求odom轮速读数接近零；没有时，假设车辆初期静止。
|
||||
* 初始化器收集一段时间内的IMU读数，按照书本3.5.4节估计初始零偏和噪声参数，提供给ESKF或者其他滤波器
|
||||
*/
|
||||
class StaticIMUInit {
|
||||
public:
|
||||
struct Options {
|
||||
Options() {}
|
||||
double init_time_seconds_ = 10.0; // 静止时间
|
||||
int init_imu_queue_max_size_ = 2000; // 初始化IMU队列最大长度
|
||||
int static_odom_pulse_ = 5; // 静止时轮速计输出噪声
|
||||
double max_static_gyro_var = 0.5; // 静态下陀螺测量方差
|
||||
double max_static_acce_var = 0.05; // 静态下加计测量方差
|
||||
double gravity_norm_ = 9.81; // 重力大小
|
||||
bool use_speed_for_static_checking_ = true; // 是否使用odom来判断车辆静止（部分数据集没有odom选项）
|
||||
};
|
||||
|
||||
/// 构造函数
|
||||
StaticIMUInit(Options options = Options()) : options_(options) {}
|
||||
|
||||
/// 添加IMU数据
|
||||
bool AddIMU(const IMU& imu);
|
||||
/// 添加轮速数据
|
||||
bool AddOdom(const Odom& odom);
|
||||
|
||||
/// 判定初始化是否成功
|
||||
bool InitSuccess() const { return init_success_; }
|
||||
|
||||
/// 获取各Cov, bias, gravity
|
||||
Vec3d GetCovGyro() const { return cov_gyro_; }
|
||||
Vec3d GetCovAcce() const { return cov_acce_; }
|
||||
Vec3d GetInitBg() const { return init_bg_; }
|
||||
Vec3d GetInitBa() const { return init_ba_; }
|
||||
Vec3d GetGravity() const { return gravity_; }
|
||||
|
||||
private:
|
||||
/// 尝试对系统初始化
|
||||
bool TryInit();
|
||||
|
||||
Options options_; // 选项信息
|
||||
bool init_success_ = false; // 初始化是否成功
|
||||
Vec3d cov_gyro_ = Vec3d::Zero(); // 陀螺测量噪声协方差（初始化时评估）
|
||||
Vec3d cov_acce_ = Vec3d::Zero(); // 加计测量噪声协方差（初始化时评估）
|
||||
Vec3d init_bg_ = Vec3d::Zero(); // 陀螺初始零偏
|
||||
Vec3d init_ba_ = Vec3d::Zero(); // 加计初始零偏
|
||||
Vec3d gravity_ = Vec3d::Zero(); // 重力
|
||||
bool is_static_ = false; // 标志车辆是否静止
|
||||
std::deque<IMU> init_imu_deque_; // 初始化用的数据
|
||||
double current_time_ = 0.0; // 当前时间
|
||||
double init_start_time_ = 0.0; // 静止的初始时间
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
|
||||
84
workspace/slam_in_autonomous_driving/src/ch3/utm_convert.cc
Normal file
84
workspace/slam_in_autonomous_driving/src/ch3/utm_convert.cc
Normal file
@@ -0,0 +1,84 @@
|
||||
//
|
||||
// Created by xiang on 2022/1/4.
|
||||
//
|
||||
|
||||
#include "ch3/utm_convert.h"
|
||||
#include "common/math_utils.h"
|
||||
#include "utm_convert/utm.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
bool LatLon2UTM(const Vec2d& latlon, UTMCoordinate& utm_coor) {
|
||||
long zone = 0;
|
||||
char char_north = 0;
|
||||
long ret = Convert_Geodetic_To_UTM(latlon[0] * math::kDEG2RAD, latlon[1] * math::kDEG2RAD, &zone, &char_north,
|
||||
&utm_coor.xy_[0], &utm_coor.xy_[1]);
|
||||
utm_coor.zone_ = (int)zone;
|
||||
utm_coor.north_ = char_north == 'N';
|
||||
|
||||
return ret == 0;
|
||||
}
|
||||
|
||||
bool UTM2LatLon(const UTMCoordinate& utm_coor, Vec2d& latlon) {
|
||||
bool ret = Convert_UTM_To_Geodetic((long)utm_coor.zone_, utm_coor.north_ ? 'N' : 'S', utm_coor.xy_[0],
|
||||
utm_coor.xy_[1], &latlon[0], &latlon[1]);
|
||||
latlon *= math::kRAD2DEG;
|
||||
return ret == 0;
|
||||
}
|
||||
|
||||
bool ConvertGps2UTM(GNSS& gps_msg, const Vec2d& antenna_pos, const double& antenna_angle, const Vec3d& map_origin) {
|
||||
/// 经纬高转换为UTM
|
||||
UTMCoordinate utm_rtk;
|
||||
if (!LatLon2UTM(gps_msg.lat_lon_alt_.head<2>(), utm_rtk)) {
|
||||
return false;
|
||||
}
|
||||
utm_rtk.z_ = gps_msg.lat_lon_alt_[2];
|
||||
|
||||
/// GPS heading 转成弧度
|
||||
double heading = 0;
|
||||
if (gps_msg.heading_valid_) {
|
||||
heading = (90 - gps_msg.heading_) * math::kDEG2RAD; // 北东地转到东北天
|
||||
}
|
||||
|
||||
/// TWG 转到 TWB
|
||||
SE3 TBG(SO3::rotZ(antenna_angle * math::kDEG2RAD), Vec3d(antenna_pos[0], antenna_pos[1], 0));
|
||||
SE3 TGB = TBG.inverse();
|
||||
|
||||
/// 若指明地图原点，则减去地图原点
|
||||
double x = utm_rtk.xy_[0] - map_origin[0];
|
||||
double y = utm_rtk.xy_[1] - map_origin[1];
|
||||
double z = utm_rtk.z_ - map_origin[2];
|
||||
SE3 TWG(SO3::rotZ(heading), Vec3d(x, y, z));
|
||||
SE3 TWB = TWG * TGB;
|
||||
|
||||
gps_msg.utm_valid_ = true;
|
||||
gps_msg.utm_.xy_[0] = TWB.translation().x();
|
||||
gps_msg.utm_.xy_[1] = TWB.translation().y();
|
||||
gps_msg.utm_.z_ = TWB.translation().z();
|
||||
|
||||
if (gps_msg.heading_valid_) {
|
||||
// 组装为带旋转的位姿
|
||||
gps_msg.utm_pose_ = TWB;
|
||||
} else {
|
||||
// 组装为仅有平移的SE3
|
||||
// 注意当安装偏移存在时，并不能实际推出车辆位姿
|
||||
gps_msg.utm_pose_ = SE3(SO3(), TWB.translation());
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ConvertGps2UTMOnlyTrans(GNSS& gps_msg) {
|
||||
/// 经纬高转换为UTM
|
||||
UTMCoordinate utm_rtk;
|
||||
LatLon2UTM(gps_msg.lat_lon_alt_.head<2>(), utm_rtk);
|
||||
gps_msg.utm_valid_ = true;
|
||||
gps_msg.utm_.xy_ = utm_rtk.xy_;
|
||||
gps_msg.utm_.z_ = gps_msg.lat_lon_alt_[2];
|
||||
gps_msg.utm_pose_ = SE3(SO3(), Vec3d(gps_msg.utm_.xy_[0], gps_msg.utm_.xy_[1], gps_msg.utm_.z_));
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
49
workspace/slam_in_autonomous_driving/src/ch3/utm_convert.h
Normal file
49
workspace/slam_in_autonomous_driving/src/ch3/utm_convert.h
Normal file
@@ -0,0 +1,49 @@
|
||||
//
|
||||
// Created by xiang on 2022/1/4.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_UTM_CONVERT_H
|
||||
#define SLAM_IN_AUTO_DRIVING_UTM_CONVERT_H
|
||||
|
||||
#include "common/gnss.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 计算本书的GNSS读数对应的UTM pose和六自由度Pose
|
||||
* @param gnss_reading 输入gnss读数
|
||||
* @param antenna_pos 安装位置
|
||||
* @param antenna_angle 安装偏角
|
||||
* @param map_origin 地图原点，指定时，将从UTM位置中减掉坐标原点
|
||||
* @return
|
||||
*/
|
||||
bool ConvertGps2UTM(GNSS& gnss_reading, const Vec2d& antenna_pos, const double& antenna_angle,
|
||||
const Vec3d& map_origin = Vec3d::Zero());
|
||||
|
||||
/**
|
||||
* 仅转换平移部分的经纬度，不作外参和角度处理
|
||||
* @param gnss_reading
|
||||
* @return
|
||||
*/
|
||||
bool ConvertGps2UTMOnlyTrans(GNSS& gnss_reading);
|
||||
|
||||
/**
|
||||
* 经纬度转UTM
|
||||
* NOTE 经纬度单位为度数
|
||||
* @param latlon
|
||||
* @param utm_coor
|
||||
* @return
|
||||
*/
|
||||
bool LatLon2UTM(const Vec2d& latlon, UTMCoordinate& utm_coor);
|
||||
|
||||
/**
|
||||
* UTM转经纬度
|
||||
* @param utm_coor
|
||||
* @param latlon
|
||||
* @return
|
||||
*/
|
||||
bool UTM2LatLon(const UTMCoordinate& utm_coor, Vec2d& latlon);
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_UTM_CONVERT_H
|
||||
19
workspace/slam_in_autonomous_driving/src/ch4/CMakeLists.txt
Normal file
19
workspace/slam_in_autonomous_driving/src/ch4/CMakeLists.txt
Normal file
@@ -0,0 +1,19 @@
|
||||
add_library(${PROJECT_NAME}.ch4
|
||||
gins_pre_integ.cc
|
||||
imu_preintegration.cc
|
||||
g2o_types.cc
|
||||
)
|
||||
|
||||
ADD_EXECUTABLE(test_preintegration test_preintegration.cc)
|
||||
ADD_TEST(test_preintegration test_preintegration)
|
||||
|
||||
target_link_libraries(test_preintegration
|
||||
gtest pthread glog gflags ${PROJECT_NAME}.ch4 ${PROJECT_NAME}.ch3 ${PROJECT_NAME}.common
|
||||
)
|
||||
|
||||
add_executable(run_gins_pre_integ run_gins_pre_integ.cc)
|
||||
target_link_libraries(run_gins_pre_integ
|
||||
${PROJECT_NAME}.ch3
|
||||
${PROJECT_NAME}.ch4
|
||||
${g2o_libs}
|
||||
)
|
||||
138
workspace/slam_in_autonomous_driving/src/ch4/g2o_types.cc
Normal file
138
workspace/slam_in_autonomous_driving/src/ch4/g2o_types.cc
Normal file
@@ -0,0 +1,138 @@
|
||||
//
|
||||
// Created by xiang on 23-1-19.
|
||||
//
|
||||
|
||||
#include "ch4/g2o_types.h"
|
||||
#include "common/g2o_types.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
EdgeInertial::EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight)
|
||||
: preint_(preinteg), dt_(preinteg->dt_) {
|
||||
resize(6); // 6个关联顶点
|
||||
grav_ = gravity;
|
||||
setInformation(preinteg->cov_.inverse() * weight);
|
||||
}
|
||||
|
||||
void EdgeInertial::computeError() {
|
||||
auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
|
||||
auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
|
||||
auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
|
||||
auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
|
||||
auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
|
||||
auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);
|
||||
|
||||
Vec3d bg = bg1->estimate();
|
||||
Vec3d ba = ba1->estimate();
|
||||
|
||||
const SO3 dR = preint_->GetDeltaRotation(bg);
|
||||
const Vec3d dv = preint_->GetDeltaVelocity(bg, ba);
|
||||
const Vec3d dp = preint_->GetDeltaPosition(bg, ba);
|
||||
|
||||
/// 预积分误差项（4.41）
|
||||
const Vec3d er = (dR.inverse() * p1->estimate().so3().inverse() * p2->estimate().so3()).log();
|
||||
Mat3d RiT = p1->estimate().so3().inverse().matrix();
|
||||
const Vec3d ev = RiT * (v2->estimate() - v1->estimate() - grav_ * dt_) - dv;
|
||||
const Vec3d ep = RiT * (p2->estimate().translation() - p1->estimate().translation() - v1->estimate() * dt_ -
|
||||
grav_ * dt_ * dt_ / 2) -
|
||||
dp;
|
||||
_error << er, ev, ep;
|
||||
}
|
||||
|
||||
void EdgeInertial::linearizeOplus() {
|
||||
auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
|
||||
auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
|
||||
auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
|
||||
auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
|
||||
auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
|
||||
auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);
|
||||
|
||||
Vec3d bg = bg1->estimate();
|
||||
Vec3d ba = ba1->estimate();
|
||||
Vec3d dbg = bg - preint_->bg_;
|
||||
|
||||
// 一些中间符号
|
||||
const SO3 R1 = p1->estimate().so3();
|
||||
const SO3 R1T = R1.inverse();
|
||||
const SO3 R2 = p2->estimate().so3();
|
||||
|
||||
auto dR_dbg = preint_->dR_dbg_;
|
||||
auto dv_dbg = preint_->dV_dbg_;
|
||||
auto dp_dbg = preint_->dP_dbg_;
|
||||
auto dv_dba = preint_->dV_dba_;
|
||||
auto dp_dba = preint_->dP_dba_;
|
||||
|
||||
// 估计值
|
||||
Vec3d vi = v1->estimate();
|
||||
Vec3d vj = v2->estimate();
|
||||
Vec3d pi = p1->estimate().translation();
|
||||
Vec3d pj = p2->estimate().translation();
|
||||
|
||||
const SO3 dR = preint_->GetDeltaRotation(bg);
|
||||
const SO3 eR = SO3(dR).inverse() * R1T * R2;
|
||||
const Vec3d er = eR.log();
|
||||
const Mat3d invJr = SO3::jr_inv(eR);
|
||||
|
||||
/// 雅可比矩阵
|
||||
/// 注意有3个index, 顶点的，自己误差的，顶点内部变量的
|
||||
/// 变量顺序：pose1(R1,p1), v1, bg1, ba1, pose2(R2,p2), v2
|
||||
/// 残差顺序：eR, ev, ep，残差顺序为行，变量顺序为列
|
||||
|
||||
// | R1 | p1 | v1 | bg1 | ba1 | R2 | p2 | v2 |
|
||||
// vert | 0 | 1 | 2 | 3 | 4 | 5 |
|
||||
// col | 0 3 | 0 | 0 | 0 | 0 3 | 0 |
|
||||
// row
|
||||
// eR 0 |
|
||||
// ev 3 |
|
||||
// ep 6 |
|
||||
|
||||
/// 残差对R1, 9x3
|
||||
_jacobianOplus[0].setZero();
|
||||
// dR/dR1, 4.42
|
||||
_jacobianOplus[0].block<3, 3>(0, 0) = -invJr * (R2.inverse() * R1).matrix();
|
||||
// dv/dR1, 4.47
|
||||
_jacobianOplus[0].block<3, 3>(3, 0) = SO3::hat(R1T * (vj - vi - grav_ * dt_));
|
||||
// dp/dR1, 4.48d
|
||||
_jacobianOplus[0].block<3, 3>(6, 0) = SO3::hat(R1T * (pj - pi - v1->estimate() * dt_ - 0.5 * grav_ * dt_ * dt_));
|
||||
|
||||
/// 残差对p1, 9x3
|
||||
// dp/dp1, 4.48a
|
||||
_jacobianOplus[0].block<3, 3>(6, 3) = -R1T.matrix();
|
||||
|
||||
/// 残差对v1, 9x3
|
||||
_jacobianOplus[1].setZero();
|
||||
// dv/dv1, 4.46a
|
||||
_jacobianOplus[1].block<3, 3>(3, 0) = -R1T.matrix();
|
||||
// dp/dv1, 4.48c
|
||||
_jacobianOplus[1].block<3, 3>(6, 0) = -R1T.matrix() * dt_;
|
||||
|
||||
/// 残差对bg1
|
||||
_jacobianOplus[2].setZero();
|
||||
// dR/dbg1, 4.45
|
||||
_jacobianOplus[2].block<3, 3>(0, 0) = -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;
|
||||
// dv/dbg1
|
||||
_jacobianOplus[2].block<3, 3>(3, 0) = -dv_dbg;
|
||||
// dp/dbg1
|
||||
_jacobianOplus[2].block<3, 3>(6, 0) = -dp_dbg;
|
||||
|
||||
/// 残差对ba1
|
||||
_jacobianOplus[3].setZero();
|
||||
// dv/dba1
|
||||
_jacobianOplus[3].block<3, 3>(3, 0) = -dv_dba;
|
||||
// dp/dba1
|
||||
_jacobianOplus[3].block<3, 3>(6, 0) = -dp_dba;
|
||||
|
||||
/// 残差对pose2
|
||||
_jacobianOplus[4].setZero();
|
||||
// dr/dr2, 4.43
|
||||
_jacobianOplus[4].block<3, 3>(0, 0) = invJr;
|
||||
// dp/dp2, 4.48b
|
||||
_jacobianOplus[4].block<3, 3>(6, 3) = R1T.matrix();
|
||||
|
||||
/// 残差对v2
|
||||
_jacobianOplus[5].setZero();
|
||||
// dv/dv2, 4,46b
|
||||
_jacobianOplus[5].block<3, 3>(3, 0) = R1T.matrix(); // OK
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
64
workspace/slam_in_autonomous_driving/src/ch4/g2o_types.h
Normal file
64
workspace/slam_in_autonomous_driving/src/ch4/g2o_types.h
Normal file
@@ -0,0 +1,64 @@
|
||||
//
|
||||
// Created by xiang on 23-1-19.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_CH4_G2O_TYPES_H
|
||||
#define SLAM_IN_AUTO_DRIVING_CH4_G2O_TYPES_H
|
||||
|
||||
#include <g2o/core/base_binary_edge.h>
|
||||
#include <g2o/core/base_multi_edge.h>
|
||||
#include <g2o/core/base_unary_edge.h>
|
||||
#include <g2o/core/base_vertex.h>
|
||||
#include <g2o/core/robust_kernel.h>
|
||||
|
||||
#include "ch4/imu_preintegration.h"
|
||||
#include "common/eigen_types.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
/// 与预积分相关的vertex, edge
|
||||
/**
|
||||
* 预积分边
|
||||
* 连接6个顶点：上一帧的pose, v, bg, ba，下一帧的pose, v
|
||||
* 观测量为9维，即预积分残差, 顺序：R, v, p
|
||||
* information从预积分类中获取，构造函数中计算
|
||||
*/
|
||||
class EdgeInertial : public g2o::BaseMultiEdge<9, Vec9d> {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
/**
|
||||
* 构造函数中需要指定预积分类对象
|
||||
* @param preinteg 预积分对象指针
|
||||
* @param gravity 重力矢量
|
||||
* @param weight 权重
|
||||
*/
|
||||
EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight = 1.0);
|
||||
|
||||
bool read(std::istream& is) override { return false; }
|
||||
bool write(std::ostream& os) const override { return false; }
|
||||
|
||||
void computeError() override;
|
||||
void linearizeOplus() override;
|
||||
|
||||
Eigen::Matrix<double, 24, 24> GetHessian() {
|
||||
linearizeOplus();
|
||||
Eigen::Matrix<double, 9, 24> J;
|
||||
J.block<9, 6>(0, 0) = _jacobianOplus[0];
|
||||
J.block<9, 3>(0, 6) = _jacobianOplus[1];
|
||||
J.block<9, 3>(0, 9) = _jacobianOplus[2];
|
||||
J.block<9, 3>(0, 12) = _jacobianOplus[3];
|
||||
J.block<9, 6>(0, 15) = _jacobianOplus[4];
|
||||
J.block<9, 3>(0, 21) = _jacobianOplus[5];
|
||||
return J.transpose() * information() * J;
|
||||
}
|
||||
|
||||
private:
|
||||
const double dt_;
|
||||
std::shared_ptr<IMUPreintegration> preint_ = nullptr;
|
||||
Vec3d grav_;
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_G2O_TYPES_H
|
||||
266
workspace/slam_in_autonomous_driving/src/ch4/gins_pre_integ.cc
Normal file
266
workspace/slam_in_autonomous_driving/src/ch4/gins_pre_integ.cc
Normal file
@@ -0,0 +1,266 @@
|
||||
//
|
||||
// Created by xiang on 2021/7/19.
|
||||
//
|
||||
|
||||
#include "ch4/gins_pre_integ.h"
|
||||
#include "ch4/g2o_types.h"
|
||||
#include "common/g2o_types.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include <g2o/core/block_solver.h>
|
||||
#include <g2o/core/optimization_algorithm_levenberg.h>
|
||||
#include <g2o/core/robust_kernel.h>
|
||||
#include <g2o/solvers/eigen/linear_solver_eigen.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
void GinsPreInteg::AddImu(const IMU& imu) {
|
||||
if (first_gnss_received_ && first_imu_received_) {
|
||||
pre_integ_->Integrate(imu, imu.timestamp_ - last_imu_.timestamp_);
|
||||
}
|
||||
|
||||
first_imu_received_ = true;
|
||||
last_imu_ = imu;
|
||||
current_time_ = imu.timestamp_;
|
||||
}
|
||||
|
||||
void GinsPreInteg::SetOptions(sad::GinsPreInteg::Options options) {
|
||||
double bg_rw2 = 1.0 / (options_.bias_gyro_var_ * options_.bias_gyro_var_);
|
||||
options_.bg_rw_info_.diagonal() << bg_rw2, bg_rw2, bg_rw2;
|
||||
double ba_rw2 = 1.0 / (options_.bias_acce_var_ * options_.bias_acce_var_);
|
||||
options_.ba_rw_info_.diagonal() << ba_rw2, ba_rw2, ba_rw2;
|
||||
|
||||
double gp2 = options_.gnss_pos_noise_ * options_.gnss_pos_noise_;
|
||||
double gh2 = options_.gnss_height_noise_ * options_.gnss_height_noise_;
|
||||
double ga2 = options_.gnss_ang_noise_ * options_.gnss_ang_noise_;
|
||||
|
||||
options_.gnss_info_.diagonal() << 1.0 / ga2, 1.0 / ga2, 1.0 / ga2, 1.0 / gp2, 1.0 / gp2, 1.0 / gh2;
|
||||
pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
|
||||
|
||||
double o2 = 1.0 / (options_.odom_var_ * options_.odom_var_);
|
||||
options_.odom_info_.diagonal() << o2, o2, o2;
|
||||
|
||||
prior_info_.block<6, 6>(9, 9) = Mat6d ::Identity() * 1e6;
|
||||
|
||||
if (this_frame_) {
|
||||
this_frame_->bg_ = options_.preinteg_options_.init_bg_;
|
||||
this_frame_->ba_ = options_.preinteg_options_.init_ba_;
|
||||
}
|
||||
}
|
||||
|
||||
void GinsPreInteg::AddGnss(const GNSS& gnss) {
|
||||
this_frame_ = std::make_shared<NavStated>(current_time_);
|
||||
this_gnss_ = gnss;
|
||||
|
||||
if (!first_gnss_received_) {
|
||||
if (!gnss.heading_valid_) {
|
||||
// 要求首个GNSS必须有航向
|
||||
return;
|
||||
}
|
||||
|
||||
// 首个gnss信号，将初始pose设置为该gnss信号
|
||||
this_frame_->timestamp_ = gnss.unix_time_;
|
||||
this_frame_->p_ = gnss.utm_pose_.translation();
|
||||
this_frame_->R_ = gnss.utm_pose_.so3();
|
||||
this_frame_->v_.setZero();
|
||||
this_frame_->bg_ = options_.preinteg_options_.init_bg_;
|
||||
this_frame_->ba_ = options_.preinteg_options_.init_ba_;
|
||||
|
||||
pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
|
||||
|
||||
last_frame_ = this_frame_;
|
||||
last_gnss_ = this_gnss_;
|
||||
first_gnss_received_ = true;
|
||||
current_time_ = gnss.unix_time_;
|
||||
return;
|
||||
}
|
||||
|
||||
// 积分到GNSS时刻
|
||||
pre_integ_->Integrate(last_imu_, gnss.unix_time_ - current_time_);
|
||||
|
||||
current_time_ = gnss.unix_time_;
|
||||
*this_frame_ = pre_integ_->Predict(*last_frame_, options_.gravity_);
|
||||
|
||||
Optimize();
|
||||
|
||||
last_frame_ = this_frame_;
|
||||
last_gnss_ = this_gnss_;
|
||||
}
|
||||
|
||||
void GinsPreInteg::AddOdom(const sad::Odom& odom) {
|
||||
last_odom_ = odom;
|
||||
last_odom_set_ = true;
|
||||
}
|
||||
|
||||
void GinsPreInteg::Optimize() {
|
||||
if (pre_integ_->dt_ < 1e-3) {
|
||||
// 未得到积分
|
||||
return;
|
||||
}
|
||||
|
||||
using BlockSolverType = g2o::BlockSolverX;
|
||||
using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;
|
||||
|
||||
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
|
||||
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
|
||||
g2o::SparseOptimizer optimizer;
|
||||
optimizer.setAlgorithm(solver);
|
||||
|
||||
// 上时刻顶点， pose, v, bg, ba
|
||||
auto v0_pose = new VertexPose();
|
||||
v0_pose->setId(0);
|
||||
v0_pose->setEstimate(last_frame_->GetSE3());
|
||||
optimizer.addVertex(v0_pose);
|
||||
|
||||
auto v0_vel = new VertexVelocity();
|
||||
v0_vel->setId(1);
|
||||
v0_vel->setEstimate(last_frame_->v_);
|
||||
optimizer.addVertex(v0_vel);
|
||||
|
||||
auto v0_bg = new VertexGyroBias();
|
||||
v0_bg->setId(2);
|
||||
v0_bg->setEstimate(last_frame_->bg_);
|
||||
optimizer.addVertex(v0_bg);
|
||||
|
||||
auto v0_ba = new VertexAccBias();
|
||||
v0_ba->setId(3);
|
||||
v0_ba->setEstimate(last_frame_->ba_);
|
||||
optimizer.addVertex(v0_ba);
|
||||
|
||||
// 本时刻顶点，pose, v, bg, ba
|
||||
auto v1_pose = new VertexPose();
|
||||
v1_pose->setId(4);
|
||||
v1_pose->setEstimate(this_frame_->GetSE3());
|
||||
optimizer.addVertex(v1_pose);
|
||||
|
||||
auto v1_vel = new VertexVelocity();
|
||||
v1_vel->setId(5);
|
||||
v1_vel->setEstimate(this_frame_->v_);
|
||||
optimizer.addVertex(v1_vel);
|
||||
|
||||
auto v1_bg = new VertexGyroBias();
|
||||
v1_bg->setId(6);
|
||||
v1_bg->setEstimate(this_frame_->bg_);
|
||||
optimizer.addVertex(v1_bg);
|
||||
|
||||
auto v1_ba = new VertexAccBias();
|
||||
v1_ba->setId(7);
|
||||
v1_ba->setEstimate(this_frame_->ba_);
|
||||
optimizer.addVertex(v1_ba);
|
||||
|
||||
// 预积分边
|
||||
auto edge_inertial = new EdgeInertial(pre_integ_, options_.gravity_);
|
||||
edge_inertial->setVertex(0, v0_pose);
|
||||
edge_inertial->setVertex(1, v0_vel);
|
||||
edge_inertial->setVertex(2, v0_bg);
|
||||
edge_inertial->setVertex(3, v0_ba);
|
||||
edge_inertial->setVertex(4, v1_pose);
|
||||
edge_inertial->setVertex(5, v1_vel);
|
||||
auto* rk = new g2o::RobustKernelHuber();
|
||||
rk->setDelta(200.0);
|
||||
edge_inertial->setRobustKernel(rk);
|
||||
optimizer.addEdge(edge_inertial);
|
||||
|
||||
// 两个零偏随机游走
|
||||
auto* edge_gyro_rw = new EdgeGyroRW();
|
||||
edge_gyro_rw->setVertex(0, v0_bg);
|
||||
edge_gyro_rw->setVertex(1, v1_bg);
|
||||
edge_gyro_rw->setInformation(options_.bg_rw_info_);
|
||||
optimizer.addEdge(edge_gyro_rw);
|
||||
|
||||
auto* edge_acc_rw = new EdgeAccRW();
|
||||
edge_acc_rw->setVertex(0, v0_ba);
|
||||
edge_acc_rw->setVertex(1, v1_ba);
|
||||
edge_acc_rw->setInformation(options_.ba_rw_info_);
|
||||
optimizer.addEdge(edge_acc_rw);
|
||||
|
||||
// 上时刻先验
|
||||
auto* edge_prior = new EdgePriorPoseNavState(*last_frame_, prior_info_);
|
||||
edge_prior->setVertex(0, v0_pose);
|
||||
edge_prior->setVertex(1, v0_vel);
|
||||
edge_prior->setVertex(2, v0_bg);
|
||||
edge_prior->setVertex(3, v0_ba);
|
||||
optimizer.addEdge(edge_prior);
|
||||
|
||||
// GNSS边
|
||||
auto edge_gnss0 = new EdgeGNSS(v0_pose, last_gnss_.utm_pose_);
|
||||
edge_gnss0->setInformation(options_.gnss_info_);
|
||||
optimizer.addEdge(edge_gnss0);
|
||||
|
||||
auto edge_gnss1 = new EdgeGNSS(v1_pose, this_gnss_.utm_pose_);
|
||||
edge_gnss1->setInformation(options_.gnss_info_);
|
||||
optimizer.addEdge(edge_gnss1);
|
||||
|
||||
// Odom边
|
||||
EdgeEncoder3D* edge_odom = nullptr;
|
||||
Vec3d vel_world = Vec3d::Zero();
|
||||
Vec3d vel_odom = Vec3d::Zero();
|
||||
if (last_odom_set_) {
|
||||
// velocity obs
|
||||
double velo_l =
|
||||
options_.wheel_radius_ * last_odom_.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
|
||||
double velo_r =
|
||||
options_.wheel_radius_ * last_odom_.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
|
||||
double average_vel = 0.5 * (velo_l + velo_r);
|
||||
vel_odom = Vec3d(average_vel, 0.0, 0.0);
|
||||
vel_world = this_frame_->R_ * vel_odom;
|
||||
|
||||
edge_odom = new EdgeEncoder3D(v1_vel, vel_world);
|
||||
edge_odom->setInformation(options_.odom_info_);
|
||||
optimizer.addEdge(edge_odom);
|
||||
|
||||
// 重置odom数据到达标志位，等待最新的odom数据
|
||||
last_odom_set_ = false;
|
||||
}
|
||||
|
||||
optimizer.setVerbose(options_.verbose_);
|
||||
optimizer.initializeOptimization();
|
||||
optimizer.optimize(20);
|
||||
|
||||
if (options_.verbose_) {
|
||||
// 获取结果，统计各类误差
|
||||
LOG(INFO) << "chi2/error: ";
|
||||
LOG(INFO) << "preintegration: " << edge_inertial->chi2() << "/" << edge_inertial->error().transpose();
|
||||
// LOG(INFO) << "gnss0: " << edge_gnss0->chi2() << ", " << edge_gnss0->error().transpose();
|
||||
LOG(INFO) << "gnss1: " << edge_gnss1->chi2() << ", " << edge_gnss1->error().transpose();
|
||||
LOG(INFO) << "bias: " << edge_gyro_rw->chi2() << "/" << edge_acc_rw->error().transpose();
|
||||
LOG(INFO) << "prior: " << edge_prior->chi2() << "/" << edge_prior->error().transpose();
|
||||
if (edge_odom) {
|
||||
LOG(INFO) << "body vel: " << (v1_pose->estimate().so3().inverse() * v1_vel->estimate()).transpose();
|
||||
LOG(INFO) << "meas: " << vel_odom.transpose();
|
||||
LOG(INFO) << "odom: " << edge_odom->chi2() << "/" << edge_odom->error().transpose();
|
||||
}
|
||||
}
|
||||
|
||||
last_frame_->R_ = v0_pose->estimate().so3();
|
||||
last_frame_->p_ = v0_pose->estimate().translation();
|
||||
last_frame_->v_ = v0_vel->estimate();
|
||||
last_frame_->bg_ = v0_bg->estimate();
|
||||
last_frame_->ba_ = v0_ba->estimate();
|
||||
|
||||
this_frame_->R_ = v1_pose->estimate().so3();
|
||||
this_frame_->p_ = v1_pose->estimate().translation();
|
||||
this_frame_->v_ = v1_vel->estimate();
|
||||
this_frame_->bg_ = v1_bg->estimate();
|
||||
this_frame_->ba_ = v1_ba->estimate();
|
||||
|
||||
// 重置integ
|
||||
options_.preinteg_options_.init_bg_ = this_frame_->bg_;
|
||||
options_.preinteg_options_.init_ba_ = this_frame_->ba_;
|
||||
pre_integ_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
|
||||
}
|
||||
|
||||
NavStated GinsPreInteg::GetState() const {
|
||||
if (this_frame_ == nullptr) {
|
||||
return {};
|
||||
}
|
||||
|
||||
if (pre_integ_ == nullptr) {
|
||||
return *this_frame_;
|
||||
}
|
||||
|
||||
return pre_integ_->Predict(*this_frame_, options_.gravity_);
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
119
workspace/slam_in_autonomous_driving/src/ch4/gins_pre_integ.h
Normal file
119
workspace/slam_in_autonomous_driving/src/ch4/gins_pre_integ.h
Normal file
@@ -0,0 +1,119 @@
|
||||
//
|
||||
// Created by xiang on 2021/7/19.
|
||||
//
|
||||
|
||||
#ifndef MAPPING_DR_PRE_INTEG_H
|
||||
#define MAPPING_DR_PRE_INTEG_H
|
||||
|
||||
#include <deque>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/gnss.h"
|
||||
#include "common/imu.h"
|
||||
#include "common/math_utils.h"
|
||||
#include "common/odom.h"
|
||||
|
||||
#include "ch4/imu_preintegration.h"
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 使用预积分优化的GINS
|
||||
*
|
||||
* 本章仍使用两帧模型，不会去做全量优化（尽管也可以做），两帧模型和ESKF更像一些
|
||||
* IMU到达时，累计到预积分器中
|
||||
* 每次RTK到达时，触发一次优化，并做边缘化。边缘化结果写入下一帧先验中。
|
||||
* 里程计方面，使用最近时间的里程计信息作为速度观测量。
|
||||
*/
|
||||
class GinsPreInteg {
|
||||
public:
|
||||
/// GINS 配置项
|
||||
struct Options {
|
||||
Options() {}
|
||||
|
||||
Vec3d gravity_ = Vec3d(0, 0, -9.8); // 重力方向
|
||||
|
||||
/// IMU相关噪声参数在preinteg内部配置
|
||||
IMUPreintegration::Options preinteg_options_;
|
||||
|
||||
// 噪声
|
||||
double bias_gyro_var_ = 1e-6; // 陀螺零偏游走标准差
|
||||
double bias_acce_var_ = 1e-4; // 加计零偏游走标准差
|
||||
Mat3d bg_rw_info_ = Mat3d::Identity(); // 陀螺随机游走信息阵
|
||||
Mat3d ba_rw_info_ = Mat3d::Identity(); // 加计随机游走信息阵
|
||||
|
||||
double gnss_pos_noise_ = 0.1; // GNSS位置方差
|
||||
double gnss_height_noise_ = 0.1; // GNSS高度方差
|
||||
double gnss_ang_noise_ = 1.0 * math::kDEG2RAD; // GNSS角度方差
|
||||
Mat6d gnss_info_ = Mat6d::Identity(); // 6D gnss信息矩阵
|
||||
|
||||
/// 轮速计相关
|
||||
double odom_var_ = 0.05;
|
||||
Mat3d odom_info_ = Mat3d::Identity();
|
||||
double odom_span_ = 0.1; // 里程计测量间隔
|
||||
double wheel_radius_ = 0.155; // 轮子半径
|
||||
double circle_pulse_ = 1024.0; // 编码器每圈脉冲数
|
||||
|
||||
bool verbose_ = true; // 是否输出调试信息
|
||||
};
|
||||
|
||||
/// Option 可以在构造时设置，也可以在后续设置
|
||||
GinsPreInteg(Options options = Options()) : options_(options) { SetOptions(options_); }
|
||||
|
||||
/**
|
||||
* IMU 处理函数，要求初始零偏已经设置过，再调用此函数
|
||||
* @param imu imu读数
|
||||
*/
|
||||
void AddImu(const IMU& imu);
|
||||
|
||||
/**
|
||||
* GNSS 处理函数
|
||||
* @param gnss
|
||||
*/
|
||||
void AddGnss(const GNSS& gnss);
|
||||
|
||||
/**
|
||||
* 轮速计处理函数
|
||||
* @param odom
|
||||
*/
|
||||
void AddOdom(const Odom& odom);
|
||||
|
||||
/// 设置gins的各种配置项，可以在构建时调用，也可以构造完成后，静止初始化结束时调用
|
||||
void SetOptions(Options options);
|
||||
|
||||
/**
|
||||
* 获取当前状态
|
||||
* 如果IMU没有积分，就返回最后优化的状态
|
||||
* 如果有，利用IMU积分，预测自己的状态
|
||||
* @return
|
||||
*/
|
||||
NavStated GetState() const;
|
||||
|
||||
private:
|
||||
// 优化
|
||||
void Optimize();
|
||||
|
||||
Options options_;
|
||||
double current_time_ = 0.0; // 当前时间
|
||||
|
||||
std::shared_ptr<IMUPreintegration> pre_integ_ = nullptr;
|
||||
std::shared_ptr<NavStated> last_frame_ = nullptr; // 上一个时刻状态
|
||||
std::shared_ptr<NavStated> this_frame_ = nullptr; // 当前时刻状态
|
||||
Mat15d prior_info_ = Mat15d::Identity() * 1e2; // 当前时刻先验
|
||||
|
||||
/// 两帧GNSS观测
|
||||
GNSS last_gnss_;
|
||||
GNSS this_gnss_;
|
||||
|
||||
IMU last_imu_; // 上时刻IMU
|
||||
Odom last_odom_; // 上时刻odom
|
||||
bool last_odom_set_ = false;
|
||||
|
||||
/// 标志位
|
||||
bool first_gnss_received_ = false; // 是否已收到第一个gnss信号
|
||||
bool first_imu_received_ = false; // 是否已收到第一个imu信号
|
||||
};
|
||||
} // namespace sad
|
||||
|
||||
#endif // MAPPING_DR_PRE_INTEG_H
|
||||
@@ -0,0 +1,89 @@
|
||||
#include "imu_preintegration.h"
|
||||
#include <glog/logging.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
IMUPreintegration::IMUPreintegration(Options options) {
|
||||
bg_ = options.init_bg_;
|
||||
ba_ = options.init_ba_;
|
||||
const float ng2 = options.noise_gyro_ * options.noise_gyro_;
|
||||
const float na2 = options.noise_acce_ * options.noise_acce_;
|
||||
noise_gyro_acce_.diagonal() << ng2, ng2, ng2, na2, na2, na2;
|
||||
}
|
||||
|
||||
void IMUPreintegration::Integrate(const IMU &imu, double dt) {
|
||||
// 去掉零偏的测量
|
||||
Vec3d gyr = imu.gyro_ - bg_; // 陀螺
|
||||
Vec3d acc = imu.acce_ - ba_; // 加计
|
||||
|
||||
// 更新dv, dp, 见(4.13), (4.16)
|
||||
dp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt;
|
||||
dv_ = dv_ + dR_ * acc * dt;
|
||||
|
||||
// dR先不更新，因为A, B阵还需要现在的dR
|
||||
|
||||
// 运动方程雅可比矩阵系数，A,B阵，见(4.29)
|
||||
// 另外两项在后面
|
||||
Eigen::Matrix<double, 9, 9> A;
|
||||
A.setIdentity();
|
||||
Eigen::Matrix<double, 9, 6> B;
|
||||
B.setZero();
|
||||
|
||||
Mat3d acc_hat = SO3::hat(acc);
|
||||
double dt2 = dt * dt;
|
||||
|
||||
// NOTE A, B左上角块与公式稍有不同
|
||||
A.block<3, 3>(3, 0) = -dR_.matrix() * dt * acc_hat;
|
||||
A.block<3, 3>(6, 0) = -0.5f * dR_.matrix() * acc_hat * dt2;
|
||||
A.block<3, 3>(6, 3) = dt * Mat3d::Identity();
|
||||
|
||||
B.block<3, 3>(3, 3) = dR_.matrix() * dt;
|
||||
B.block<3, 3>(6, 3) = 0.5f * dR_.matrix() * dt2;
|
||||
|
||||
// 更新各雅可比，见式(4.39)
|
||||
dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2; // (4.39d)
|
||||
dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_; // (4.39e)
|
||||
dV_dba_ = dV_dba_ - dR_.matrix() * dt; // (4.39b)
|
||||
dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_; // (4.39c)
|
||||
|
||||
// 旋转部分
|
||||
Vec3d omega = gyr * dt; // 转动量
|
||||
Mat3d rightJ = SO3::jr(omega); // 右雅可比
|
||||
SO3 deltaR = SO3::exp(omega); // exp后
|
||||
dR_ = dR_ * deltaR; // (4.9)
|
||||
|
||||
A.block<3, 3>(0, 0) = deltaR.matrix().transpose();
|
||||
B.block<3, 3>(0, 0) = rightJ * dt;
|
||||
|
||||
// 更新噪声项
|
||||
cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();
|
||||
|
||||
// 更新dR_dbg
|
||||
dR_dbg_ = deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt; // (4.39a)
|
||||
|
||||
// 增量积分时间
|
||||
dt_ += dt;
|
||||
}
|
||||
|
||||
SO3 IMUPreintegration::GetDeltaRotation(const Vec3d &bg) { return dR_ * SO3::exp(dR_dbg_ * (bg - bg_)); }
|
||||
|
||||
Vec3d IMUPreintegration::GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba) {
|
||||
return dv_ + dV_dbg_ * (bg - bg_) + dV_dba_ * (ba - ba_);
|
||||
}
|
||||
|
||||
Vec3d IMUPreintegration::GetDeltaPosition(const Vec3d &bg, const Vec3d &ba) {
|
||||
return dp_ + dP_dbg_ * (bg - bg_) + dP_dba_ * (ba - ba_);
|
||||
}
|
||||
|
||||
NavStated IMUPreintegration::Predict(const sad::NavStated &start, const Vec3d &grav) const {
|
||||
SO3 Rj = start.R_ * dR_;
|
||||
Vec3d vj = start.R_ * dv_ + start.v_ + grav * dt_;
|
||||
Vec3d pj = start.R_ * dp_ + start.p_ + start.v_ * dt_ + 0.5f * grav * dt_ * dt_;
|
||||
|
||||
auto state = NavStated(start.timestamp_ + dt_, Rj, pj, vj);
|
||||
state.bg_ = bg_;
|
||||
state.ba_ = ba_;
|
||||
return state;
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
@@ -0,0 +1,79 @@
|
||||
#ifndef IMUTYPES_H
|
||||
#define IMUTYPES_H
|
||||
|
||||
#include <mutex>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/imu.h"
|
||||
#include "common/nav_state.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* IMU 预积分器
|
||||
*
|
||||
* 调用Integrate来插入新的IMU读数，然后通过Get函数得到预积分的值
|
||||
* 雅可比也可以通过本类获得，可用于构建g2o的边类
|
||||
*/
|
||||
class IMUPreintegration {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
/// 参数配置项
|
||||
/// 初始的零偏需要设置，其他可以不改
|
||||
struct Options {
|
||||
Options() {}
|
||||
Vec3d init_bg_ = Vec3d::Zero(); // 初始零偏
|
||||
Vec3d init_ba_ = Vec3d::Zero(); // 初始零偏
|
||||
double noise_gyro_ = 1e-2; // 陀螺噪声，标准差
|
||||
double noise_acce_ = 1e-1; // 加计噪声，标准差
|
||||
};
|
||||
|
||||
IMUPreintegration(Options options = Options());
|
||||
|
||||
/**
|
||||
* 插入新的IMU数据
|
||||
* @param imu imu 读数
|
||||
* @param dt 时间差
|
||||
*/
|
||||
void Integrate(const IMU &imu, double dt);
|
||||
|
||||
/**
|
||||
* 从某个起始点开始预测积分之后的状态
|
||||
* @param start 起始时时刻状态
|
||||
* @return 预测的状态
|
||||
*/
|
||||
NavStated Predict(const NavStated &start, const Vec3d &grav = Vec3d(0, 0, -9.81)) const;
|
||||
|
||||
/// 获取修正之后的观测量，bias可以与预积分时期的不同，会有一阶修正
|
||||
SO3 GetDeltaRotation(const Vec3d &bg);
|
||||
Vec3d GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba);
|
||||
Vec3d GetDeltaPosition(const Vec3d &bg, const Vec3d &ba);
|
||||
|
||||
public:
|
||||
double dt_ = 0; // 整体预积分时间
|
||||
Mat9d cov_ = Mat9d::Zero(); // 累计噪声矩阵
|
||||
Mat6d noise_gyro_acce_ = Mat6d::Zero(); // 测量噪声矩阵
|
||||
|
||||
// 零偏
|
||||
Vec3d bg_ = Vec3d::Zero();
|
||||
Vec3d ba_ = Vec3d::Zero();
|
||||
|
||||
// 预积分观测量
|
||||
SO3 dR_;
|
||||
Vec3d dv_ = Vec3d::Zero();
|
||||
Vec3d dp_ = Vec3d::Zero();
|
||||
|
||||
// 雅可比矩阵
|
||||
Mat3d dR_dbg_ = Mat3d::Zero();
|
||||
Mat3d dV_dbg_ = Mat3d::Zero();
|
||||
Mat3d dV_dba_ = Mat3d::Zero();
|
||||
Mat3d dP_dbg_ = Mat3d::Zero();
|
||||
Mat3d dP_dba_ = Mat3d::Zero();
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // IMUTYPES_H
|
||||
@@ -0,0 +1,152 @@
|
||||
//
|
||||
// Created by xiang on 2022/1/21.
|
||||
//
|
||||
|
||||
#include "ch3/static_imu_init.h"
|
||||
#include "ch3/utm_convert.h"
|
||||
#include "ch4/gins_pre_integ.h"
|
||||
#include "common/io_utils.h"
|
||||
#include "tools/ui/pangolin_window.h"
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
|
||||
/**
|
||||
* 运行由预积分构成的GINS系统
|
||||
*/
|
||||
DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
|
||||
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
|
||||
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
|
||||
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
|
||||
DEFINE_bool(with_ui, true, "是否显示图形界面");
|
||||
DEFINE_bool(debug, false, "是否打印调试信息");
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (fLS::FLAGS_txt_path.empty()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 初始化器
|
||||
sad::StaticIMUInit imu_init; // 使用默认配置
|
||||
|
||||
sad::TxtIO io(fLS::FLAGS_txt_path);
|
||||
Vec2d antenna_pos(fLD::FLAGS_antenna_pox_x, fLD::FLAGS_antenna_pox_y);
|
||||
|
||||
auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
|
||||
auto save_quat = [](std::ofstream& fout, const Quatd& q) {
|
||||
fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
|
||||
};
|
||||
|
||||
auto save_result = [&save_vec3, &save_quat](std::ofstream& fout, const sad::NavStated& save_state) {
|
||||
fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);
|
||||
save_vec3(fout, save_state.p_);
|
||||
save_quat(fout, save_state.R_.unit_quaternion());
|
||||
save_vec3(fout, save_state.v_);
|
||||
save_vec3(fout, save_state.bg_);
|
||||
save_vec3(fout, save_state.ba_);
|
||||
fout << std::endl;
|
||||
};
|
||||
|
||||
std::ofstream fout("./data/ch4/gins_preintg.txt");
|
||||
bool imu_inited = false, gnss_inited = false;
|
||||
|
||||
sad::GinsPreInteg::Options gins_options;
|
||||
gins_options.verbose_ = FLAGS_debug;
|
||||
sad::GinsPreInteg gins(gins_options);
|
||||
|
||||
bool first_gnss_set = false;
|
||||
Vec3d origin = Vec3d::Zero();
|
||||
|
||||
std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
|
||||
if (FLAGS_with_ui) {
|
||||
ui = std::make_shared<sad::ui::PangolinWindow>();
|
||||
ui->Init();
|
||||
}
|
||||
|
||||
/// 设置各类回调函数
|
||||
io.SetIMUProcessFunc([&](const sad::IMU& imu) {
|
||||
/// IMU 处理函数
|
||||
if (!imu_init.InitSuccess()) {
|
||||
imu_init.AddIMU(imu);
|
||||
return;
|
||||
}
|
||||
|
||||
/// 需要IMU初始化
|
||||
if (!imu_inited) {
|
||||
// 读取初始零偏，设置GINS
|
||||
sad::GinsPreInteg::Options options;
|
||||
options.preinteg_options_.init_bg_ = imu_init.GetInitBg();
|
||||
options.preinteg_options_.init_ba_ = imu_init.GetInitBa();
|
||||
options.gravity_ = imu_init.GetGravity();
|
||||
gins.SetOptions(options);
|
||||
imu_inited = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!gnss_inited) {
|
||||
/// 等待有效的RTK数据
|
||||
return;
|
||||
}
|
||||
|
||||
/// GNSS 也接收到之后，再开始进行预测
|
||||
gins.AddImu(imu);
|
||||
|
||||
auto state = gins.GetState();
|
||||
save_result(fout, state);
|
||||
if (ui) {
|
||||
ui->UpdateNavState(state);
|
||||
usleep(5e2);
|
||||
}
|
||||
})
|
||||
.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
|
||||
/// GNSS 处理函数
|
||||
if (!imu_inited) {
|
||||
return;
|
||||
}
|
||||
|
||||
sad::GNSS gnss_convert = gnss;
|
||||
if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
|
||||
return;
|
||||
}
|
||||
|
||||
/// 去掉原点
|
||||
if (!first_gnss_set) {
|
||||
origin = gnss_convert.utm_pose_.translation();
|
||||
first_gnss_set = true;
|
||||
}
|
||||
gnss_convert.utm_pose_.translation() -= origin;
|
||||
|
||||
gins.AddGnss(gnss_convert);
|
||||
|
||||
auto state = gins.GetState();
|
||||
save_result(fout, state);
|
||||
if (ui) {
|
||||
ui->UpdateNavState(state);
|
||||
usleep(1e3);
|
||||
}
|
||||
gnss_inited = true;
|
||||
})
|
||||
.SetOdomProcessFunc([&](const sad::Odom& odom) {
|
||||
imu_init.AddOdom(odom);
|
||||
|
||||
if (imu_inited && gnss_inited) {
|
||||
gins.AddOdom(odom);
|
||||
}
|
||||
})
|
||||
.Go();
|
||||
|
||||
while (ui && !ui->ShouldQuit()) {
|
||||
usleep(1e5);
|
||||
}
|
||||
if (ui) {
|
||||
ui->Quit();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,397 @@
|
||||
//
|
||||
// Created by xiang on 2021/7/16.
|
||||
//
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "ch3/eskf.hpp"
|
||||
#include "ch3/static_imu_init.h"
|
||||
#include "ch4/imu_preintegration.h"
|
||||
#include "ch4/g2o_types.h"
|
||||
#include "common/g2o_types.h"
|
||||
#include "common/io_utils.h"
|
||||
|
||||
#include <g2o/core/block_solver.h>
|
||||
#include <g2o/core/optimization_algorithm_gauss_newton.h>
|
||||
#include <g2o/core/optimization_algorithm_levenberg.h>
|
||||
#include <g2o/core/robust_kernel.h>
|
||||
#include <g2o/solvers/eigen/linear_solver_eigen.h>
|
||||
|
||||
DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");
|
||||
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
|
||||
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
|
||||
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
|
||||
DEFINE_bool(with_ui, true, "是否显示图形界面");
|
||||
|
||||
TEST(PREINTEGRATION_TEST, ROTATION_TEST) {
|
||||
// 测试在恒定角速度运转下的预积分情况
|
||||
double imu_time_span = 0.01; // IMU测量间隔
|
||||
Vec3d constant_omega(0, 0, M_PI); // 角速度为180度/s，转1秒应该等于转180度
|
||||
Vec3d gravity(0, 0, -9.8); // Z 向上，重力方向为负
|
||||
|
||||
sad::NavStated start_status(0), end_status(1.0);
|
||||
sad::IMUPreintegration pre_integ;
|
||||
|
||||
// 对比直接积分
|
||||
Sophus::SO3d R;
|
||||
Vec3d t = Vec3d::Zero();
|
||||
Vec3d v = Vec3d::Zero();
|
||||
|
||||
for (int i = 1; i <= 100; ++i) {
|
||||
double time = imu_time_span * i;
|
||||
Vec3d acce = -gravity; // 加速度计应该测量到一个向上的力
|
||||
pre_integ.Integrate(sad::IMU(time, constant_omega, acce), imu_time_span);
|
||||
|
||||
sad::NavStated this_status = pre_integ.Predict(start_status, gravity);
|
||||
|
||||
t = t + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +
|
||||
0.5 * (R * acce) * imu_time_span * imu_time_span;
|
||||
v = v + gravity * imu_time_span + (R * acce) * imu_time_span;
|
||||
R = R * Sophus::SO3d::exp(constant_omega * imu_time_span);
|
||||
|
||||
// 验证在简单情况下，直接积分和预积分结果相等
|
||||
EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);
|
||||
EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);
|
||||
EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);
|
||||
|
||||
EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);
|
||||
EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);
|
||||
EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);
|
||||
|
||||
EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);
|
||||
EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);
|
||||
EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);
|
||||
EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);
|
||||
}
|
||||
|
||||
end_status = pre_integ.Predict(start_status);
|
||||
|
||||
LOG(INFO) << "preinteg result: ";
|
||||
LOG(INFO) << "end rotation: \n" << end_status.R_.matrix();
|
||||
LOG(INFO) << "end trans: \n" << end_status.p_.transpose();
|
||||
LOG(INFO) << "end v: \n" << end_status.v_.transpose();
|
||||
|
||||
LOG(INFO) << "direct integ result: ";
|
||||
LOG(INFO) << "end rotation: \n" << R.matrix();
|
||||
LOG(INFO) << "end trans: \n" << t.transpose();
|
||||
LOG(INFO) << "end v: \n" << v.transpose();
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(PREINTEGRATION_TEST, ACCELERATION_TEST) {
|
||||
// 测试在恒定加速度运行下的预积分情况
|
||||
double imu_time_span = 0.01; // IMU测量间隔
|
||||
Vec3d gravity(0, 0, -9.8); // Z 向上，重力方向为负
|
||||
Vec3d constant_acce(0.1, 0, 0); // x 方向上的恒定加速度
|
||||
|
||||
sad::NavStated start_status(0), end_status(1.0);
|
||||
sad::IMUPreintegration pre_integ;
|
||||
|
||||
// 对比直接积分
|
||||
Sophus::SO3d R;
|
||||
Vec3d t = Vec3d::Zero();
|
||||
Vec3d v = Vec3d::Zero();
|
||||
|
||||
for (int i = 1; i <= 100; ++i) {
|
||||
double time = imu_time_span * i;
|
||||
Vec3d acce = constant_acce - gravity;
|
||||
pre_integ.Integrate(sad::IMU(time, Vec3d::Zero(), acce), imu_time_span);
|
||||
sad::NavStated this_status = pre_integ.Predict(start_status, gravity);
|
||||
|
||||
t = t + v * imu_time_span + 0.5 * gravity * imu_time_span * imu_time_span +
|
||||
0.5 * (R * acce) * imu_time_span * imu_time_span;
|
||||
v = v + gravity * imu_time_span + (R * acce) * imu_time_span;
|
||||
|
||||
// 验证在简单情况下，直接积分和预积分结果相等
|
||||
EXPECT_NEAR(t[0], this_status.p_[0], 1e-2);
|
||||
EXPECT_NEAR(t[1], this_status.p_[1], 1e-2);
|
||||
EXPECT_NEAR(t[2], this_status.p_[2], 1e-2);
|
||||
|
||||
EXPECT_NEAR(v[0], this_status.v_[0], 1e-2);
|
||||
EXPECT_NEAR(v[1], this_status.v_[1], 1e-2);
|
||||
EXPECT_NEAR(v[2], this_status.v_[2], 1e-2);
|
||||
|
||||
EXPECT_NEAR(R.unit_quaternion().x(), this_status.R_.unit_quaternion().x(), 1e-4);
|
||||
EXPECT_NEAR(R.unit_quaternion().y(), this_status.R_.unit_quaternion().y(), 1e-4);
|
||||
EXPECT_NEAR(R.unit_quaternion().z(), this_status.R_.unit_quaternion().z(), 1e-4);
|
||||
EXPECT_NEAR(R.unit_quaternion().w(), this_status.R_.unit_quaternion().w(), 1e-4);
|
||||
}
|
||||
|
||||
end_status = pre_integ.Predict(start_status);
|
||||
LOG(INFO) << "preinteg result: ";
|
||||
LOG(INFO) << "end rotation: \n" << end_status.R_.matrix();
|
||||
LOG(INFO) << "end trans: \n" << end_status.p_.transpose();
|
||||
LOG(INFO) << "end v: \n" << end_status.v_.transpose();
|
||||
|
||||
LOG(INFO) << "direct integ result: ";
|
||||
LOG(INFO) << "end rotation: \n" << R.matrix();
|
||||
LOG(INFO) << "end trans: \n" << t.transpose();
|
||||
LOG(INFO) << "end v: \n" << v.transpose();
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
void Optimize(sad::NavStated& last_state, sad::NavStated& this_state, sad::GNSS& last_gnss, sad::GNSS& this_gnss,
|
||||
std::shared_ptr<sad::IMUPreintegration>& preinteg, const Vec3d& grav);
|
||||
|
||||
/// 使用ESKF的Predict, Update来验证预积分的优化过程
|
||||
TEST(PREINTEGRATION_TEST, ESKF_TEST) {
|
||||
if (fLS::FLAGS_txt_path.empty()) {
|
||||
FAIL();
|
||||
}
|
||||
|
||||
// 初始化器
|
||||
sad::StaticIMUInit imu_init; // 使用默认配置
|
||||
sad::ESKFD eskf;
|
||||
|
||||
sad::TxtIO io(FLAGS_txt_path);
|
||||
Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
|
||||
|
||||
std::ofstream fout("./data/ch3/gins.txt");
|
||||
bool imu_inited = false, gnss_inited = false;
|
||||
|
||||
/// 设置各类回调函数
|
||||
bool first_gnss_set = false;
|
||||
Vec3d origin = Vec3d::Zero();
|
||||
|
||||
std::shared_ptr<sad::IMUPreintegration> preinteg = nullptr;
|
||||
|
||||
sad::NavStated last_state;
|
||||
bool last_state_set = false;
|
||||
|
||||
sad::GNSS last_gnss;
|
||||
bool last_gnss_set = false;
|
||||
|
||||
io.SetIMUProcessFunc([&](const sad::IMU& imu) {
|
||||
/// IMU 处理函数
|
||||
if (!imu_init.InitSuccess()) {
|
||||
imu_init.AddIMU(imu);
|
||||
return;
|
||||
}
|
||||
|
||||
/// 需要IMU初始化
|
||||
if (!imu_inited) {
|
||||
// 读取初始零偏，设置ESKF
|
||||
sad::ESKFD::Options options;
|
||||
// 噪声由初始化器估计
|
||||
options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);
|
||||
options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);
|
||||
eskf.SetInitialConditions(options, imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity());
|
||||
|
||||
imu_inited = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!gnss_inited) {
|
||||
/// 等待有效的RTK数据
|
||||
return;
|
||||
}
|
||||
|
||||
/// GNSS 也接收到之后，再开始进行预测
|
||||
double current_time = eskf.GetNominalState().timestamp_;
|
||||
eskf.Predict(imu);
|
||||
|
||||
if (preinteg) {
|
||||
preinteg->Integrate(imu, imu.timestamp_ - current_time);
|
||||
|
||||
if (last_state_set) {
|
||||
auto pred_of_preinteg = preinteg->Predict(last_state, eskf.GetGravity());
|
||||
auto pred_of_eskf = eskf.GetNominalState();
|
||||
|
||||
/// 这两个预测值的误差应该非常接近
|
||||
EXPECT_NEAR((pred_of_preinteg.p_ - pred_of_eskf.p_).norm(), 0, 1e-2);
|
||||
EXPECT_NEAR((pred_of_preinteg.R_.inverse() * pred_of_eskf.R_).log().norm(), 0, 1e-2);
|
||||
EXPECT_NEAR((pred_of_preinteg.v_ - pred_of_eskf.v_).norm(), 0, 1e-2);
|
||||
}
|
||||
}
|
||||
})
|
||||
.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
|
||||
/// GNSS 处理函数
|
||||
if (!imu_inited) {
|
||||
return;
|
||||
}
|
||||
|
||||
sad::GNSS gnss_convert = gnss;
|
||||
if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
|
||||
return;
|
||||
}
|
||||
|
||||
/// 去掉原点
|
||||
if (!first_gnss_set) {
|
||||
origin = gnss_convert.utm_pose_.translation();
|
||||
first_gnss_set = true;
|
||||
}
|
||||
gnss_convert.utm_pose_.translation() -= origin;
|
||||
|
||||
// 要求RTK heading有效，才能合入EKF
|
||||
auto state_bef_update = eskf.GetNominalState();
|
||||
|
||||
eskf.ObserveGps(gnss_convert);
|
||||
|
||||
// 验证优化过程是否正确
|
||||
if (last_state_set && last_gnss_set) {
|
||||
auto update_state = eskf.GetNominalState();
|
||||
|
||||
LOG(INFO) << "state before eskf update: " << state_bef_update;
|
||||
LOG(INFO) << "state after eskf update: " << update_state;
|
||||
|
||||
auto state_pred = preinteg->Predict(last_state, eskf.GetGravity());
|
||||
LOG(INFO) << "state in pred: " << state_pred;
|
||||
|
||||
Optimize(last_state, update_state, last_gnss, gnss_convert, preinteg, eskf.GetGravity());
|
||||
}
|
||||
|
||||
last_state = eskf.GetNominalState();
|
||||
last_state_set = true;
|
||||
|
||||
// 重置预积分
|
||||
sad::IMUPreintegration::Options options;
|
||||
options.init_bg_ = last_state.bg_;
|
||||
options.init_ba_ = last_state.ba_;
|
||||
preinteg = std::make_shared<sad::IMUPreintegration>(options);
|
||||
|
||||
gnss_inited = true;
|
||||
last_gnss = gnss_convert;
|
||||
last_gnss_set = true;
|
||||
})
|
||||
.SetOdomProcessFunc([&](const sad::Odom& odom) { imu_init.AddOdom(odom); })
|
||||
.Go();
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
void Optimize(sad::NavStated& last_state, sad::NavStated& this_state, sad::GNSS& last_gnss, sad::GNSS& this_gnss,
|
||||
std::shared_ptr<sad::IMUPreintegration>& pre_integ, const Vec3d& grav) {
|
||||
assert(pre_integ != nullptr);
|
||||
|
||||
if (pre_integ->dt_ < 1e-3) {
|
||||
// 未得到积分
|
||||
return;
|
||||
}
|
||||
|
||||
using BlockSolverType = g2o::BlockSolverX;
|
||||
using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;
|
||||
|
||||
auto* solver = new g2o::OptimizationAlgorithmGaussNewton(
|
||||
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
|
||||
g2o::SparseOptimizer optimizer;
|
||||
optimizer.setAlgorithm(solver);
|
||||
|
||||
// 上时刻顶点， pose, v, bg, ba
|
||||
auto v0_pose = new sad::VertexPose();
|
||||
v0_pose->setId(0);
|
||||
v0_pose->setEstimate(last_state.GetSE3());
|
||||
optimizer.addVertex(v0_pose);
|
||||
|
||||
auto v0_vel = new sad::VertexVelocity();
|
||||
v0_vel->setId(1);
|
||||
v0_vel->setEstimate(last_state.v_);
|
||||
optimizer.addVertex(v0_vel);
|
||||
|
||||
auto v0_bg = new sad::VertexGyroBias();
|
||||
v0_bg->setId(2);
|
||||
v0_bg->setEstimate(last_state.bg_);
|
||||
optimizer.addVertex(v0_bg);
|
||||
|
||||
auto v0_ba = new sad::VertexAccBias();
|
||||
v0_ba->setId(3);
|
||||
v0_ba->setEstimate(last_state.ba_);
|
||||
optimizer.addVertex(v0_ba);
|
||||
|
||||
// 本时刻顶点，pose, v, bg, ba
|
||||
auto v1_pose = new sad::VertexPose();
|
||||
v1_pose->setId(4);
|
||||
v1_pose->setEstimate(this_state.GetSE3());
|
||||
optimizer.addVertex(v1_pose);
|
||||
|
||||
auto v1_vel = new sad::VertexVelocity();
|
||||
v1_vel->setId(5);
|
||||
v1_vel->setEstimate(this_state.v_);
|
||||
optimizer.addVertex(v1_vel);
|
||||
|
||||
auto v1_bg = new sad::VertexGyroBias();
|
||||
v1_bg->setId(6);
|
||||
v1_bg->setEstimate(this_state.bg_);
|
||||
optimizer.addVertex(v1_bg);
|
||||
|
||||
auto v1_ba = new sad::VertexAccBias();
|
||||
v1_ba->setId(7);
|
||||
v1_ba->setEstimate(this_state.ba_);
|
||||
optimizer.addVertex(v1_ba);
|
||||
|
||||
// 预积分边
|
||||
auto edge_inertial = new sad::EdgeInertial(pre_integ, grav);
|
||||
edge_inertial->setVertex(0, v0_pose);
|
||||
edge_inertial->setVertex(1, v0_vel);
|
||||
edge_inertial->setVertex(2, v0_bg);
|
||||
edge_inertial->setVertex(3, v0_ba);
|
||||
edge_inertial->setVertex(4, v1_pose);
|
||||
edge_inertial->setVertex(5, v1_vel);
|
||||
|
||||
auto* rk = new g2o::RobustKernelHuber();
|
||||
rk->setDelta(200.0);
|
||||
edge_inertial->setRobustKernel(rk);
|
||||
|
||||
optimizer.addEdge(edge_inertial);
|
||||
edge_inertial->computeError();
|
||||
LOG(INFO) << "inertial init err: " << edge_inertial->chi2();
|
||||
|
||||
auto* edge_gyro_rw = new sad::EdgeGyroRW();
|
||||
edge_gyro_rw->setVertex(0, v0_bg);
|
||||
edge_gyro_rw->setVertex(1, v1_bg);
|
||||
edge_gyro_rw->setInformation(Mat3d::Identity() * 1e6);
|
||||
optimizer.addEdge(edge_gyro_rw);
|
||||
|
||||
edge_gyro_rw->computeError();
|
||||
LOG(INFO) << "inertial bg rw: " << edge_gyro_rw->chi2();
|
||||
|
||||
auto* edge_acc_rw = new sad::EdgeAccRW();
|
||||
edge_acc_rw->setVertex(0, v0_ba);
|
||||
edge_acc_rw->setVertex(1, v1_ba);
|
||||
edge_acc_rw->setInformation(Mat3d::Identity() * 1e6);
|
||||
optimizer.addEdge(edge_acc_rw);
|
||||
|
||||
edge_acc_rw->computeError();
|
||||
LOG(INFO) << "inertial ba rw: " << edge_acc_rw->chi2();
|
||||
|
||||
// GNSS边
|
||||
auto edge_gnss0 = new sad::EdgeGNSS(v0_pose, last_gnss.utm_pose_);
|
||||
edge_gnss0->setInformation(Mat6d::Identity() * 1e2);
|
||||
optimizer.addEdge(edge_gnss0);
|
||||
|
||||
edge_gnss0->computeError();
|
||||
LOG(INFO) << "gnss0 init err: " << edge_gnss0->chi2();
|
||||
|
||||
auto edge_gnss1 = new sad::EdgeGNSS(v1_pose, this_gnss.utm_pose_);
|
||||
edge_gnss1->setInformation(Mat6d::Identity() * 1e2);
|
||||
optimizer.addEdge(edge_gnss1);
|
||||
|
||||
edge_gnss1->computeError();
|
||||
LOG(INFO) << "gnss1 init err: " << edge_gnss1->chi2();
|
||||
|
||||
optimizer.setVerbose(true);
|
||||
optimizer.initializeOptimization();
|
||||
optimizer.optimize(10);
|
||||
|
||||
sad::NavStated corr_state(this_state.timestamp_, v1_pose->estimate().so3(), v1_pose->estimate().translation(),
|
||||
v1_vel->estimate(), v1_bg->estimate(), v1_ba->estimate());
|
||||
LOG(INFO) << "corr state in opt: " << corr_state;
|
||||
|
||||
// 获取结果，统计各类误差
|
||||
LOG(INFO) << "chi2/error: ";
|
||||
LOG(INFO) << "preintegration: " << edge_inertial->chi2() << "/" << edge_inertial->error().transpose();
|
||||
LOG(INFO) << "gnss0: " << edge_gnss0->chi2() << ", " << edge_gnss0->error().transpose();
|
||||
LOG(INFO) << "gnss1: " << edge_gnss1->chi2() << ", " << edge_gnss1->error().transpose();
|
||||
LOG(INFO) << "bias: " << edge_gyro_rw->chi2() << "/" << edge_acc_rw->error().transpose();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
46
workspace/slam_in_autonomous_driving/src/ch5/CMakeLists.txt
Normal file
46
workspace/slam_in_autonomous_driving/src/ch5/CMakeLists.txt
Normal file
@@ -0,0 +1,46 @@
|
||||
add_executable(point_cloud_load_and_vis point_cloud_load_and_vis.cc)
|
||||
target_link_libraries(point_cloud_load_and_vis
|
||||
${PCL_LIBRARIES}
|
||||
${GLOG_LIBRARIES}
|
||||
gflags
|
||||
)
|
||||
|
||||
add_executable(pcd_to_bird_eye pcd_to_bird_eye.cc)
|
||||
target_link_libraries(pcd_to_bird_eye
|
||||
${PCL_LIBRARIES}
|
||||
${GLOG_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
gflags
|
||||
)
|
||||
|
||||
add_executable(scan_to_range_image scan_to_range_image.cc)
|
||||
target_link_libraries(scan_to_range_image
|
||||
${PCL_LIBRARIES}
|
||||
${GLOG_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
gflags
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}.ch5
|
||||
bfnn.cc
|
||||
kdtree.cc
|
||||
octo_tree.cc
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}.ch5
|
||||
tbb
|
||||
)
|
||||
|
||||
add_executable(linear_fitting linear_fitting.cc)
|
||||
target_link_libraries(linear_fitting
|
||||
${PCL_LIBRARIES}
|
||||
${GLOG_LIBRARIES}
|
||||
${OpenCV_LIBS}
|
||||
gflags
|
||||
)
|
||||
|
||||
ADD_EXECUTABLE(test_nn test_nn.cc)
|
||||
ADD_TEST(test_nn test_bfnn)
|
||||
|
||||
target_link_libraries(test_nn
|
||||
gtest pthread glog gflags ${PROJECT_NAME}.ch5 ${PROJECT_NAME}.common ${PCL_LIBRARIES} tbb
|
||||
)
|
||||
80
workspace/slam_in_autonomous_driving/src/ch5/bfnn.cc
Normal file
80
workspace/slam_in_autonomous_driving/src/ch5/bfnn.cc
Normal file
@@ -0,0 +1,80 @@
|
||||
//
|
||||
// Created by xiang on 2021/8/18.
|
||||
//
|
||||
|
||||
#include "ch5/bfnn.h"
|
||||
#include <execution>
|
||||
|
||||
namespace sad {
|
||||
|
||||
int bfnn_point(CloudPtr cloud, const Vec3f& point) {
|
||||
return std::min_element(cloud->points.begin(), cloud->points.end(),
|
||||
[&point](const PointType& pt1, const PointType& pt2) -> bool {
|
||||
return (pt1.getVector3fMap() - point).squaredNorm() <
|
||||
(pt2.getVector3fMap() - point).squaredNorm();
|
||||
}) -
|
||||
cloud->points.begin();
|
||||
}
|
||||
|
||||
std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k) {
|
||||
struct IndexAndDis {
|
||||
IndexAndDis() {}
|
||||
IndexAndDis(int index, double dis2) : index_(index), dis2_(dis2) {}
|
||||
int index_ = 0;
|
||||
double dis2_ = 0;
|
||||
};
|
||||
|
||||
std::vector<IndexAndDis> index_and_dis(cloud->size());
|
||||
for (int i = 0; i < cloud->size(); ++i) {
|
||||
index_and_dis[i] = {i, (cloud->points[i].getVector3fMap() - point).squaredNorm()};
|
||||
}
|
||||
std::sort(index_and_dis.begin(), index_and_dis.end(),
|
||||
[](const auto& d1, const auto& d2) { return d1.dis2_ < d2.dis2_; });
|
||||
std::vector<int> ret;
|
||||
std::transform(index_and_dis.begin(), index_and_dis.begin() + k, std::back_inserter(ret),
|
||||
[](const auto& d1) { return d1.index_; });
|
||||
return ret;
|
||||
}
|
||||
|
||||
void bfnn_cloud_mt(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches) {
|
||||
// 先生成索引
|
||||
std::vector<size_t> index(cloud2->size());
|
||||
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
|
||||
|
||||
// 并行化for_each
|
||||
matches.resize(index.size());
|
||||
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](auto idx) {
|
||||
matches[idx].second = idx;
|
||||
matches[idx].first = bfnn_point(cloud1, ToVec3f(cloud2->points[idx]));
|
||||
});
|
||||
}
|
||||
|
||||
void bfnn_cloud(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches) {
|
||||
// 单线程版本
|
||||
std::vector<size_t> index(cloud2->size());
|
||||
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
|
||||
|
||||
matches.resize(index.size());
|
||||
std::for_each(std::execution::seq, index.begin(), index.end(), [&](auto idx) {
|
||||
matches[idx].second = idx;
|
||||
matches[idx].first = bfnn_point(cloud1, ToVec3f(cloud2->points[idx]));
|
||||
});
|
||||
}
|
||||
|
||||
void bfnn_cloud_mt_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k) {
|
||||
// 先生成索引
|
||||
std::vector<size_t> index(cloud2->size());
|
||||
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
|
||||
|
||||
// 并行化for_each
|
||||
matches.resize(index.size() * k);
|
||||
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](auto idx) {
|
||||
auto v = bfnn_point_k(cloud1, ToVec3f(cloud2->points[idx]), k);
|
||||
for (int i = 0; i < v.size(); ++i) {
|
||||
matches[idx * k + i].first = v[i];
|
||||
matches[idx * k + i].second = idx;
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
58
workspace/slam_in_autonomous_driving/src/ch5/bfnn.h
Normal file
58
workspace/slam_in_autonomous_driving/src/ch5/bfnn.h
Normal file
@@ -0,0 +1,58 @@
|
||||
//
|
||||
// Created by xiang on 2021/8/18.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_BFNN_H
|
||||
#define SLAM_IN_AUTO_DRIVING_BFNN_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/point_types.h"
|
||||
|
||||
#include <thread>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* Brute-force Nearest Neighbour
|
||||
* @param cloud 点云
|
||||
* @param point 待查找点
|
||||
* @return 找到的最近点索引
|
||||
*/
|
||||
int bfnn_point(CloudPtr cloud, const Vec3f& point);
|
||||
|
||||
/**
|
||||
* Brute-force Nearest Neighbour, k近邻
|
||||
* @param cloud 点云
|
||||
* @param point 待查找点
|
||||
* @param k 近邻数
|
||||
* @return 找到的最近点索引
|
||||
*/
|
||||
std::vector<int> bfnn_point_k(CloudPtr cloud, const Vec3f& point, int k = 5);
|
||||
|
||||
/**
|
||||
* 对点云进行BF最近邻
|
||||
* @param cloud1 目标点云
|
||||
* @param cloud2 被查找点云
|
||||
* @param matches 两个点云内的匹配关系
|
||||
* @return
|
||||
*/
|
||||
void bfnn_cloud(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches);
|
||||
|
||||
/**
|
||||
* 对点云进行BF最近邻 多线程版本
|
||||
* @param cloud1
|
||||
* @param cloud2
|
||||
* @param matches
|
||||
*/
|
||||
void bfnn_cloud_mt(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches);
|
||||
|
||||
/**
|
||||
* 对点云进行BF最近邻 多线程版本，k近邻
|
||||
* @param cloud1
|
||||
* @param cloud2
|
||||
* @param matches
|
||||
*/
|
||||
void bfnn_cloud_mt_k(CloudPtr cloud1, CloudPtr cloud2, std::vector<std::pair<size_t, size_t>>& matches, int k = 5);
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_BFNN_H
|
||||
215
workspace/slam_in_autonomous_driving/src/ch5/gridnn.hpp
Normal file
215
workspace/slam_in_autonomous_driving/src/ch5/gridnn.hpp
Normal file
@@ -0,0 +1,215 @@
|
||||
//
|
||||
// Created by xiang on 2021/8/25.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_GRID2D_HPP
|
||||
#define SLAM_IN_AUTO_DRIVING_GRID2D_HPP
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/math_utils.h"
|
||||
#include "common/point_types.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <execution>
|
||||
#include <map>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 栅格法最近邻
|
||||
* @tparam dim 模板参数，使用2D或3D栅格
|
||||
*/
|
||||
template <int dim>
|
||||
class GridNN {
|
||||
public:
|
||||
using KeyType = Eigen::Matrix<int, dim, 1>;
|
||||
using PtType = Eigen::Matrix<float, dim, 1>;
|
||||
|
||||
enum class NearbyType {
|
||||
CENTER, // 只考虑中心
|
||||
// for 2D
|
||||
NEARBY4, // 上下左右
|
||||
NEARBY8, // 上下左右+四角
|
||||
|
||||
// for 3D
|
||||
NEARBY6, // 上下左右前后
|
||||
};
|
||||
|
||||
/**
|
||||
* 构造函数
|
||||
* @param resolution 分辨率
|
||||
* @param nearby_type 近邻判定方法
|
||||
*/
|
||||
explicit GridNN(float resolution = 0.1, NearbyType nearby_type = NearbyType::NEARBY4)
|
||||
: resolution_(resolution), nearby_type_(nearby_type) {
|
||||
inv_resolution_ = 1.0 / resolution_;
|
||||
|
||||
// check dim and nearby
|
||||
if (dim == 2 && nearby_type_ == NearbyType::NEARBY6) {
|
||||
LOG(INFO) << "2D grid does not support nearby6, using nearby4 instead.";
|
||||
nearby_type_ = NearbyType::NEARBY4;
|
||||
} else if (dim == 3 && (nearby_type_ != NearbyType::NEARBY6 && nearby_type_ != NearbyType::CENTER)) {
|
||||
LOG(INFO) << "3D grid does not support nearby4/8, using nearby6 instead.";
|
||||
nearby_type_ = NearbyType::NEARBY6;
|
||||
}
|
||||
|
||||
GenerateNearbyGrids();
|
||||
}
|
||||
|
||||
/// 设置点云，建立栅格
|
||||
bool SetPointCloud(CloudPtr cloud);
|
||||
|
||||
/// 获取最近邻
|
||||
bool GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx);
|
||||
|
||||
/// 对比两个点云
|
||||
bool GetClosestPointForCloud(CloudPtr ref, CloudPtr query, std::vector<std::pair<size_t, size_t>>& matches);
|
||||
bool GetClosestPointForCloudMT(CloudPtr ref, CloudPtr query, std::vector<std::pair<size_t, size_t>>& matches);
|
||||
|
||||
private:
|
||||
/// 根据最近邻的类型，生成附近网格
|
||||
void GenerateNearbyGrids();
|
||||
|
||||
/// 空间坐标转到grid
|
||||
KeyType Pos2Grid(const PtType& pt);
|
||||
|
||||
float resolution_ = 0.1; // 分辨率
|
||||
float inv_resolution_ = 10.0; // 分辨率倒数
|
||||
|
||||
NearbyType nearby_type_ = NearbyType::NEARBY4;
|
||||
std::unordered_map<KeyType, std::vector<size_t>, hash_vec<dim>> grids_; // 栅格数据
|
||||
CloudPtr cloud_;
|
||||
|
||||
std::vector<KeyType> nearby_grids_; // 附近的栅格
|
||||
};
|
||||
|
||||
// 实现
|
||||
template <int dim>
|
||||
bool GridNN<dim>::SetPointCloud(CloudPtr cloud) {
|
||||
std::vector<size_t> index(cloud->size());
|
||||
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
|
||||
|
||||
std::for_each(index.begin(), index.end(), [&cloud, this](const size_t& idx) {
|
||||
auto pt = cloud->points[idx];
|
||||
auto key = Pos2Grid(ToEigen<float, dim>(pt));
|
||||
if (grids_.find(key) == grids_.end()) {
|
||||
grids_.insert({key, {idx}});
|
||||
} else {
|
||||
grids_[key].emplace_back(idx);
|
||||
}
|
||||
});
|
||||
|
||||
cloud_ = cloud;
|
||||
LOG(INFO) << "grids: " << grids_.size();
|
||||
return true;
|
||||
}
|
||||
|
||||
template <int dim>
|
||||
Eigen::Matrix<int, dim, 1> GridNN<dim>::Pos2Grid(const Eigen::Matrix<float, dim, 1>& pt) {
|
||||
return pt.array().template round().template cast<int>();
|
||||
// Eigen::Matrix<int, dim, 1> ret;
|
||||
// for (int i = 0; i < dim; ++i) {
|
||||
// ret(i, 0) = round(pt[i] * inv_resolution_);
|
||||
// }
|
||||
// return ret;
|
||||
}
|
||||
|
||||
template <>
|
||||
void GridNN<2>::GenerateNearbyGrids() {
|
||||
if (nearby_type_ == NearbyType::CENTER) {
|
||||
nearby_grids_.emplace_back(KeyType::Zero());
|
||||
} else if (nearby_type_ == NearbyType::NEARBY4) {
|
||||
nearby_grids_ = {Vec2i(0, 0), Vec2i(-1, 0), Vec2i(1, 0), Vec2i(0, 1), Vec2i(0, -1)};
|
||||
} else if (nearby_type_ == NearbyType::NEARBY8) {
|
||||
nearby_grids_ = {
|
||||
Vec2i(0, 0), Vec2i(-1, 0), Vec2i(1, 0), Vec2i(0, 1), Vec2i(0, -1),
|
||||
Vec2i(-1, -1), Vec2i(-1, 1), Vec2i(1, -1), Vec2i(1, 1),
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
template <>
|
||||
void GridNN<3>::GenerateNearbyGrids() {
|
||||
if (nearby_type_ == NearbyType::CENTER) {
|
||||
nearby_grids_.emplace_back(KeyType::Zero());
|
||||
} else if (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)};
|
||||
}
|
||||
}
|
||||
|
||||
template <int dim>
|
||||
bool GridNN<dim>::GetClosestPoint(const PointType& pt, PointType& closest_pt, size_t& idx) {
|
||||
// 在pt栅格周边寻找最近邻
|
||||
std::vector<size_t> idx_to_check;
|
||||
auto key = Pos2Grid(ToEigen<float, dim>(pt));
|
||||
|
||||
std::for_each(nearby_grids_.begin(), nearby_grids_.end(), [&key, &idx_to_check, this](const KeyType& delta) {
|
||||
auto dkey = key + delta;
|
||||
auto iter = grids_.find(dkey);
|
||||
if (iter != grids_.end()) {
|
||||
idx_to_check.insert(idx_to_check.end(), iter->second.begin(), iter->second.end());
|
||||
}
|
||||
});
|
||||
|
||||
if (idx_to_check.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// brute force nn in cloud_[idx]
|
||||
CloudPtr nearby_cloud(new PointCloudType);
|
||||
std::vector<size_t> nearby_idx;
|
||||
for (auto& idx : idx_to_check) {
|
||||
nearby_cloud->points.template emplace_back(cloud_->points[idx]);
|
||||
nearby_idx.emplace_back(idx);
|
||||
}
|
||||
|
||||
size_t closest_point_idx = bfnn_point(nearby_cloud, ToVec3f(pt));
|
||||
idx = nearby_idx.at(closest_point_idx);
|
||||
closest_pt = cloud_->points[idx];
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template <int dim>
|
||||
bool GridNN<dim>::GetClosestPointForCloud(CloudPtr ref, CloudPtr query,
|
||||
std::vector<std::pair<size_t, size_t>>& matches) {
|
||||
matches.clear();
|
||||
std::vector<size_t> index(query->size());
|
||||
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
|
||||
std::for_each(index.begin(), index.end(), [this, &matches, &query](const size_t& idx) {
|
||||
PointType cp;
|
||||
size_t cp_idx;
|
||||
if (GetClosestPoint(query->points[idx], cp, cp_idx)) {
|
||||
matches.emplace_back(cp_idx, idx);
|
||||
}
|
||||
});
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template <int dim>
|
||||
bool GridNN<dim>::GetClosestPointForCloudMT(CloudPtr ref, CloudPtr query,
|
||||
std::vector<std::pair<size_t, size_t>>& matches) {
|
||||
matches.clear();
|
||||
// 与串行版本基本一样，但matches需要预先生成，匹配失败时填入非法匹配
|
||||
std::vector<size_t> index(query->size());
|
||||
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
|
||||
matches.resize(index.size());
|
||||
|
||||
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &matches, &query](const size_t& idx) {
|
||||
PointType cp;
|
||||
size_t cp_idx;
|
||||
if (GetClosestPoint(query->points[idx], cp, cp_idx)) {
|
||||
matches[idx] = {cp_idx, idx};
|
||||
} else {
|
||||
matches[idx] = {math::kINVALID_ID, math::kINVALID_ID};
|
||||
}
|
||||
});
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_GRID2D_HPP
|
||||
236
workspace/slam_in_autonomous_driving/src/ch5/kdtree.cc
Normal file
236
workspace/slam_in_autonomous_driving/src/ch5/kdtree.cc
Normal file
@@ -0,0 +1,236 @@
|
||||
//
|
||||
// Created by xiang on 2021/9/22.
|
||||
//
|
||||
|
||||
#include "ch5/kdtree.h"
|
||||
#include "common/math_utils.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <execution>
|
||||
#include <set>
|
||||
|
||||
namespace sad {
|
||||
|
||||
bool KdTree::BuildTree(const CloudPtr &cloud) {
|
||||
if (cloud->empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
cloud_.clear();
|
||||
cloud_.resize(cloud->size());
|
||||
for (size_t i = 0; i < cloud->points.size(); ++i) {
|
||||
cloud_[i] = ToVec3f(cloud->points[i]);
|
||||
}
|
||||
|
||||
Clear();
|
||||
Reset();
|
||||
|
||||
IndexVec idx(cloud->size());
|
||||
for (int i = 0; i < cloud->points.size(); ++i) {
|
||||
idx[i] = i;
|
||||
}
|
||||
|
||||
Insert(idx, root_.get());
|
||||
return true;
|
||||
}
|
||||
|
||||
void KdTree::Insert(const IndexVec &points, KdTreeNode *node) {
|
||||
nodes_.insert({node->id_, node});
|
||||
|
||||
if (points.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (points.size() == 1) {
|
||||
size_++;
|
||||
node->point_idx_ = points[0];
|
||||
return;
|
||||
}
|
||||
|
||||
IndexVec left, right;
|
||||
if (!FindSplitAxisAndThresh(points, node->axis_index_, node->split_thresh_, left, right)) {
|
||||
size_++;
|
||||
node->point_idx_ = points[0];
|
||||
return;
|
||||
}
|
||||
|
||||
const auto create_if_not_empty = [&node, this](KdTreeNode *&new_node, const IndexVec &index) {
|
||||
if (!index.empty()) {
|
||||
new_node = new KdTreeNode;
|
||||
new_node->id_ = tree_node_id_++;
|
||||
Insert(index, new_node);
|
||||
}
|
||||
};
|
||||
|
||||
create_if_not_empty(node->left_, left);
|
||||
create_if_not_empty(node->right_, right);
|
||||
}
|
||||
|
||||
bool KdTree::GetClosestPoint(const PointType &pt, std::vector<int> &closest_idx, int k) {
|
||||
if (k > size_) {
|
||||
LOG(ERROR) << "cannot set k larger than cloud size: " << k << ", " << size_;
|
||||
return false;
|
||||
}
|
||||
k_ = k;
|
||||
|
||||
std::priority_queue<NodeAndDistance> knn_result;
|
||||
Knn(ToVec3f(pt), root_.get(), knn_result);
|
||||
|
||||
// 排序并返回结果
|
||||
closest_idx.resize(knn_result.size());
|
||||
for (int i = closest_idx.size() - 1; i >= 0; --i) {
|
||||
// 倒序插入
|
||||
closest_idx[i] = knn_result.top().node_->point_idx_;
|
||||
knn_result.pop();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool KdTree::GetClosestPointMT(const CloudPtr &cloud, std::vector<std::pair<size_t, size_t>> &matches, int k) {
|
||||
matches.resize(cloud->size() * k);
|
||||
|
||||
// 索引
|
||||
std::vector<int> index(cloud->size());
|
||||
for (int i = 0; i < cloud->points.size(); ++i) {
|
||||
index[i] = i;
|
||||
}
|
||||
|
||||
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &cloud, &matches, &k](int idx) {
|
||||
std::vector<int> closest_idx;
|
||||
GetClosestPoint(cloud->points[idx], closest_idx, k);
|
||||
for (int i = 0; i < k; ++i) {
|
||||
matches[idx * k + i].second = idx;
|
||||
if (i < closest_idx.size()) {
|
||||
matches[idx * k + i].first = closest_idx[i];
|
||||
} else {
|
||||
matches[idx * k + i].first = math::kINVALID_ID;
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void KdTree::Knn(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
|
||||
if (node->IsLeaf()) {
|
||||
// 如果是叶子，检查叶子是否能插入
|
||||
ComputeDisForLeaf(pt, node, knn_result);
|
||||
return;
|
||||
}
|
||||
|
||||
// 看pt落在左还是右，优先搜索pt所在的子树
|
||||
// 然后再看另一侧子树是否需要搜索
|
||||
KdTreeNode *this_side, *that_side;
|
||||
if (pt[node->axis_index_] < node->split_thresh_) {
|
||||
this_side = node->left_;
|
||||
that_side = node->right_;
|
||||
} else {
|
||||
this_side = node->right_;
|
||||
that_side = node->left_;
|
||||
}
|
||||
|
||||
Knn(pt, this_side, knn_result);
|
||||
if (NeedExpand(pt, node, knn_result)) { // 注意这里是跟自己比
|
||||
Knn(pt, that_side, knn_result);
|
||||
}
|
||||
}
|
||||
|
||||
bool KdTree::NeedExpand(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
|
||||
if (knn_result.size() < k_) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (approximate_) {
|
||||
float d = pt[node->axis_index_] - node->split_thresh_;
|
||||
if ((d * d) < knn_result.top().distance2_ * alpha_) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
// 检测切面距离，看是否有比现在更小的
|
||||
float d = pt[node->axis_index_] - node->split_thresh_;
|
||||
if ((d * d) < knn_result.top().distance2_) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void KdTree::ComputeDisForLeaf(const Vec3f &pt, KdTreeNode *node,
|
||||
std::priority_queue<NodeAndDistance> &knn_result) const {
|
||||
// 比较与结果队列的差异，如果优于最远距离，则插入
|
||||
float dis2 = Dis2(pt, cloud_[node->point_idx_]);
|
||||
if (knn_result.size() < k_) {
|
||||
// results 不足k
|
||||
knn_result.emplace(node, dis2);
|
||||
} else {
|
||||
// results等于k，比较current与max_dis_iter之间的差异
|
||||
if (dis2 < knn_result.top().distance2_) {
|
||||
knn_result.emplace(node, dis2);
|
||||
knn_result.pop();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool KdTree::FindSplitAxisAndThresh(const IndexVec &point_idx, int &axis, float &th, IndexVec &left, IndexVec &right) {
|
||||
// 计算三个轴上的散布情况，我们使用math_utils.h里的函数
|
||||
Vec3f var;
|
||||
Vec3f mean;
|
||||
math::ComputeMeanAndCovDiag(point_idx, mean, var, [this](int idx) { return cloud_[idx]; });
|
||||
int max_i, max_j;
|
||||
var.maxCoeff(&max_i, &max_j);
|
||||
axis = max_i;
|
||||
th = mean[axis];
|
||||
|
||||
for (const auto &idx : point_idx) {
|
||||
if (cloud_[idx][axis] < th) {
|
||||
// 中位数可能向左取整
|
||||
left.emplace_back(idx);
|
||||
} else {
|
||||
right.emplace_back(idx);
|
||||
}
|
||||
}
|
||||
|
||||
// 边界情况检查：输入的points等于同一个值，上面的判定是>=号，所以都进了右侧
|
||||
// 这种情况不需要继续展开，直接将当前节点设为叶子就行
|
||||
// if (point_idx.size() > 1 && (left.empty() || right.empty())) {
|
||||
// return false;
|
||||
// }
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void KdTree::Reset() {
|
||||
tree_node_id_ = 0;
|
||||
root_.reset(new KdTreeNode());
|
||||
root_->id_ = tree_node_id_++;
|
||||
size_ = 0;
|
||||
}
|
||||
|
||||
void KdTree::Clear() {
|
||||
for (const auto &np : nodes_) {
|
||||
if (np.second != root_.get()) {
|
||||
delete np.second;
|
||||
}
|
||||
}
|
||||
|
||||
nodes_.clear();
|
||||
root_ = nullptr;
|
||||
size_ = 0;
|
||||
tree_node_id_ = 0;
|
||||
}
|
||||
|
||||
void KdTree::PrintAll() {
|
||||
for (const auto &np : nodes_) {
|
||||
auto node = np.second;
|
||||
if (node->left_ == nullptr && node->right_ == nullptr) {
|
||||
LOG(INFO) << "leaf node: " << node->id_ << ", idx: " << node->point_idx_;
|
||||
} else {
|
||||
LOG(INFO) << "node: " << node->id_ << ", axis: " << node->axis_index_ << ", th: " << node->split_thresh_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
133
workspace/slam_in_autonomous_driving/src/ch5/kdtree.h
Normal file
133
workspace/slam_in_autonomous_driving/src/ch5/kdtree.h
Normal file
@@ -0,0 +1,133 @@
|
||||
//
|
||||
// Created by xiang on 2021/9/22.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_KDTREE_H
|
||||
#define SLAM_IN_AUTO_DRIVING_KDTREE_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/point_types.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/// Kd树节点，二叉树结构，内部用祼指针，对外一个root的shared_ptr
|
||||
struct KdTreeNode {
|
||||
int id_ = -1;
|
||||
int point_idx_ = 0; // 点的索引
|
||||
int axis_index_ = 0; // 分割轴
|
||||
float split_thresh_ = 0.0; // 分割位置
|
||||
KdTreeNode* left_ = nullptr; // 左子树
|
||||
KdTreeNode* right_ = nullptr; // 右子树
|
||||
|
||||
bool IsLeaf() const { return left_ == nullptr && right_ == nullptr; } // 是否为叶子
|
||||
};
|
||||
|
||||
/// 用于记录knn结果
|
||||
struct NodeAndDistance {
|
||||
NodeAndDistance(KdTreeNode* node, float dis2) : node_(node), distance2_(dis2) {}
|
||||
KdTreeNode* node_ = nullptr;
|
||||
float distance2_ = 0; // 平方距离，用于比较
|
||||
|
||||
bool operator<(const NodeAndDistance& other) const { return distance2_ < other.distance2_; }
|
||||
};
|
||||
|
||||
/**
|
||||
* 手写kd树
|
||||
* 测试这个kd树的召回!
|
||||
*/
|
||||
class KdTree {
|
||||
public:
|
||||
explicit KdTree() = default;
|
||||
~KdTree() { Clear(); }
|
||||
|
||||
bool BuildTree(const CloudPtr& cloud);
|
||||
|
||||
/// 获取k最近邻
|
||||
bool GetClosestPoint(const PointType& pt, std::vector<int>& closest_idx, int k = 5);
|
||||
|
||||
/// 并行为点云寻找最近邻
|
||||
bool GetClosestPointMT(const CloudPtr& cloud, std::vector<std::pair<size_t, size_t>>& matches, int k = 5);
|
||||
|
||||
/// 这个被用于计算最近邻的倍数
|
||||
void SetEnableANN(bool use_ann = true, float alpha = 0.1) {
|
||||
approximate_ = use_ann;
|
||||
alpha_ = alpha;
|
||||
}
|
||||
|
||||
/// 返回节点数量
|
||||
size_t size() const { return size_; }
|
||||
|
||||
/// 清理数据
|
||||
void Clear();
|
||||
|
||||
/// 打印所有节点信息
|
||||
void PrintAll();
|
||||
|
||||
private:
|
||||
/// kdtree 构建相关
|
||||
/**
|
||||
* 在node处插入点
|
||||
* @param points
|
||||
* @param node
|
||||
*/
|
||||
void Insert(const IndexVec& points, KdTreeNode* node);
|
||||
|
||||
/**
|
||||
* 计算点集的分割面
|
||||
* @param points 输入点云
|
||||
* @param axis 轴
|
||||
* @param th 阈值
|
||||
* @param left 左子树
|
||||
* @param right 右子树
|
||||
* @return
|
||||
*/
|
||||
bool FindSplitAxisAndThresh(const IndexVec& point_idx, int& axis, float& th, IndexVec& left, IndexVec& right);
|
||||
|
||||
void Reset();
|
||||
|
||||
/// 两个点的平方距离
|
||||
static inline float Dis2(const Vec3f& p1, const Vec3f& p2) { return (p1 - p2).squaredNorm(); }
|
||||
|
||||
// Knn 相关
|
||||
/**
|
||||
* 检查给定点在kdtree node上的knn，可以递归调用
|
||||
* @param pt 查询点
|
||||
* @param node kdtree 节点
|
||||
*/
|
||||
void Knn(const Vec3f& pt, KdTreeNode* node, std::priority_queue<NodeAndDistance>& result) const;
|
||||
|
||||
/**
|
||||
* 对叶子节点，计算它和查询点的距离，尝试放入结果中
|
||||
* @param pt 查询点
|
||||
* @param node Kdtree 节点
|
||||
*/
|
||||
void ComputeDisForLeaf(const Vec3f& pt, KdTreeNode* node, std::priority_queue<NodeAndDistance>& result) const;
|
||||
|
||||
/**
|
||||
* 检查node下是否需要展开
|
||||
* @param pt 查询点
|
||||
* @param node Kdtree 节点
|
||||
* @return true if 需要展开
|
||||
*/
|
||||
bool NeedExpand(const Vec3f& pt, KdTreeNode* node, std::priority_queue<NodeAndDistance>& knn_result) const;
|
||||
|
||||
int k_ = 5; // knn最近邻数量
|
||||
std::shared_ptr<KdTreeNode> root_ = nullptr; // 根节点
|
||||
std::vector<Vec3f> cloud_; // 输入点云
|
||||
std::unordered_map<int, KdTreeNode*> nodes_; // for bookkeeping
|
||||
|
||||
size_t size_ = 0; // 叶子节点数量
|
||||
int tree_node_id_ = 0; // 为kdtree node 分配id
|
||||
|
||||
// 近似最近邻
|
||||
bool approximate_ = true;
|
||||
float alpha_ = 0.1;
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_KDTREE_H
|
||||
@@ -0,0 +1,82 @@
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/math_utils.h"
|
||||
|
||||
DEFINE_int32(num_tested_points_plane, 10, "number of tested points in plane fitting");
|
||||
DEFINE_int32(num_tested_points_line, 100, "number of tested points in line fitting");
|
||||
DEFINE_double(noise_sigma, 0.01, "noise of generated samples");
|
||||
|
||||
void PlaneFittingTest();
|
||||
void LineFittingTest();
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
LOG(INFO) << "testing plane fitting";
|
||||
PlaneFittingTest();
|
||||
|
||||
LOG(INFO) << "testing line fitting";
|
||||
LineFittingTest();
|
||||
}
|
||||
|
||||
void PlaneFittingTest() {
|
||||
Vec4d true_plane_coeffs(0.1, 0.2, 0.3, 0.4);
|
||||
true_plane_coeffs.normalize();
|
||||
|
||||
std::vector<Vec3d> points;
|
||||
|
||||
// 随机生成仿真平面点
|
||||
cv::RNG rng;
|
||||
for (int i = 0; i < FLAGS_num_tested_points_plane; ++i) {
|
||||
// 先生成一个随机点，计算第四维，增加噪声，再归一化
|
||||
Vec3d p(rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0), rng.uniform(0.0, 1.0));
|
||||
double n4 = -p.dot(true_plane_coeffs.head<3>()) / true_plane_coeffs[3];
|
||||
p = p / (n4 + std::numeric_limits<double>::min()); // 防止除零
|
||||
p += Vec3d(rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));
|
||||
|
||||
points.emplace_back(p);
|
||||
|
||||
// 验证在平面上
|
||||
LOG(INFO) << "res of p: " << p.dot(true_plane_coeffs.head<3>()) + true_plane_coeffs[3];
|
||||
}
|
||||
|
||||
Vec4d estimated_plane_coeffs;
|
||||
if (sad::math::FitPlane(points, estimated_plane_coeffs)) {
|
||||
LOG(INFO) << "estimated coeffs: " << estimated_plane_coeffs.transpose()
|
||||
<< ", true: " << true_plane_coeffs.transpose();
|
||||
} else {
|
||||
LOG(INFO) << "plane fitting failed";
|
||||
}
|
||||
}
|
||||
|
||||
void LineFittingTest() {
|
||||
// 直线拟合参数真值
|
||||
Vec3d true_line_origin(0.1, 0.2, 0.3);
|
||||
Vec3d true_line_dir(0.4, 0.5, 0.6);
|
||||
true_line_dir.normalize();
|
||||
|
||||
// 随机生成直线点，利用参数方程
|
||||
std::vector<Vec3d> points;
|
||||
cv::RNG rng;
|
||||
for (int i = 0; i < fLI::FLAGS_num_tested_points_line; ++i) {
|
||||
double t = rng.uniform(-1.0, 1.0);
|
||||
Vec3d p = true_line_origin + true_line_dir * t;
|
||||
p += Vec3d(rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma), rng.gaussian(FLAGS_noise_sigma));
|
||||
|
||||
points.emplace_back(p);
|
||||
}
|
||||
|
||||
Vec3d esti_origin, esti_dir;
|
||||
if (sad::math::FitLine(points, esti_origin, esti_dir)) {
|
||||
LOG(INFO) << "estimated origin: " << esti_origin.transpose() << ", true: " << true_line_origin.transpose();
|
||||
LOG(INFO) << "estimated dir: " << esti_dir.transpose() << ", true: " << true_line_dir.transpose();
|
||||
} else {
|
||||
LOG(INFO) << "line fitting failed";
|
||||
}
|
||||
}
|
||||
269
workspace/slam_in_autonomous_driving/src/ch5/octo_tree.cc
Normal file
269
workspace/slam_in_autonomous_driving/src/ch5/octo_tree.cc
Normal file
@@ -0,0 +1,269 @@
|
||||
//
|
||||
// Created by xiang on 2021/9/27.
|
||||
//
|
||||
|
||||
#include "octo_tree.h"
|
||||
#include "common/math_utils.h"
|
||||
|
||||
#include <execution>
|
||||
|
||||
namespace sad {
|
||||
|
||||
bool OctoTree::BuildTree(const CloudPtr &cloud) {
|
||||
if (cloud->empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
cloud_.clear();
|
||||
cloud_.resize(cloud->size());
|
||||
for (size_t i = 0; i < cloud->points.size(); ++i) {
|
||||
cloud_[i] = ToVec3f(cloud->points[i]);
|
||||
}
|
||||
|
||||
Clear();
|
||||
Reset();
|
||||
|
||||
IndexVec idx(cloud->size());
|
||||
for (int i = 0; i < cloud->points.size(); ++i) {
|
||||
idx[i] = i;
|
||||
}
|
||||
|
||||
// 生成根节点的边界框
|
||||
root_->box_ = ComputeBoundingBox();
|
||||
Insert(idx, root_.get());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void OctoTree::Insert(const IndexVec &points, OctoTreeNode *node) {
|
||||
nodes_.insert({node->id_, node});
|
||||
|
||||
if (points.empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (points.size() == 1) {
|
||||
size_++;
|
||||
node->point_idx_ = points[0];
|
||||
return;
|
||||
}
|
||||
|
||||
/// 只要点数不为1,就继续展开这个节点
|
||||
std::vector<IndexVec> children_points;
|
||||
ExpandNode(node, points, children_points);
|
||||
|
||||
/// 对子节点进行插入操作
|
||||
for (size_t i = 0; i < 8; ++i) {
|
||||
Insert(children_points[i], node->children[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void OctoTree::ExpandNode(OctoTreeNode *node, const IndexVec &parent_idx, std::vector<IndexVec> &children_idx) {
|
||||
children_idx.resize(8);
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
node->children[i] = new OctoTreeNode();
|
||||
node->children[i]->id_ = tree_node_id_++;
|
||||
}
|
||||
|
||||
const Box3D &b = node->box_; // 本节点的box
|
||||
// 中心点
|
||||
float c_x = 0.5 * (node->box_.min_[0] + node->box_.max_[0]);
|
||||
float c_y = 0.5 * (node->box_.min_[1] + node->box_.max_[1]);
|
||||
float c_z = 0.5 * (node->box_.min_[2] + node->box_.max_[2]);
|
||||
|
||||
// 8个外框示意图
|
||||
// clang-format off
|
||||
// 第一层：左上1 右上2 左下3 右下4
|
||||
// 第二层：左上5 右上6 左下7 右下8
|
||||
// ---> x /-------/-------/|
|
||||
// /| /-------/-------/||
|
||||
// / | /-------/-------/ ||
|
||||
// y |z | | | /|
|
||||
// |_______|_______|/|/
|
||||
// | | | /
|
||||
// |_______|_______|/
|
||||
// clang-format on
|
||||
node->children[0]->box_ = {b.min_[0], c_x, b.min_[1], c_y, b.min_[2], c_z};
|
||||
node->children[1]->box_ = {c_x, b.max_[0], b.min_[1], c_y, b.min_[2], c_z};
|
||||
node->children[2]->box_ = {b.min_[0], c_x, c_y, b.max_[1], b.min_[2], c_z};
|
||||
node->children[3]->box_ = {c_x, b.max_[0], c_y, b.max_[1], b.min_[2], c_z};
|
||||
|
||||
node->children[4]->box_ = {b.min_[0], c_x, b.min_[1], c_y, c_z, b.max_[2]};
|
||||
node->children[5]->box_ = {c_x, b.max_[0], b.min_[1], c_y, c_z, b.max_[2]};
|
||||
node->children[6]->box_ = {b.min_[0], c_x, c_y, b.max_[1], c_z, b.max_[2]};
|
||||
node->children[7]->box_ = {c_x, b.max_[0], c_y, b.max_[1], c_z, b.max_[2]};
|
||||
|
||||
// 把点云归到子节点中
|
||||
for (int idx : parent_idx) {
|
||||
const auto pt = cloud_[idx];
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (node->children[i]->box_.Inside(pt)) {
|
||||
children_idx[i].emplace_back(idx);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Box3D OctoTree::ComputeBoundingBox() {
|
||||
float min_values[3] = {std::numeric_limits<float>::max(),std::numeric_limits<float>::max(),std::numeric_limits<float>::max()};
|
||||
float max_values[3] = {-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max()};
|
||||
|
||||
for (const auto &p : cloud_) {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
max_values[i] = p[i] > max_values[i] ? p[i] : max_values[i];
|
||||
min_values[i] = p[i] < min_values[i] ? p[i] : min_values[i];
|
||||
}
|
||||
}
|
||||
|
||||
return {min_values[0], max_values[0], min_values[1], max_values[1], min_values[2], max_values[2]};
|
||||
}
|
||||
|
||||
bool OctoTree::GetClosestPoint(const PointType &pt, std::vector<int> &closest_idx, int k) const {
|
||||
if (k > size_) {
|
||||
LOG(ERROR) << "cannot set k larger than cloud size: " << k << ", " << size_;
|
||||
return false;
|
||||
}
|
||||
|
||||
std::priority_queue<NodeAndDistanceOcto> knn_result;
|
||||
Knn(ToVec3f(pt), root_.get(), knn_result);
|
||||
|
||||
// 排序并返回结果
|
||||
closest_idx.resize(knn_result.size());
|
||||
for (int i = closest_idx.size() - 1; i >= 0; --i) {
|
||||
// 倒序插入
|
||||
closest_idx[i] = knn_result.top().node_->point_idx_;
|
||||
knn_result.pop();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OctoTree::GetClosestPointMT(const CloudPtr &cloud, std::vector<std::pair<size_t, size_t>> &matches, int k) {
|
||||
k_ = k;
|
||||
matches.resize(cloud->size() * k);
|
||||
// 索引
|
||||
std::vector<int> index(cloud->size());
|
||||
for (int i = 0; i < cloud->points.size(); ++i) {
|
||||
index[i] = i;
|
||||
}
|
||||
|
||||
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &cloud, &matches, &k](int idx) {
|
||||
std::vector<int> closest_idx;
|
||||
GetClosestPoint(cloud->points[idx], closest_idx, k);
|
||||
|
||||
for (int i = 0; i < k; ++i) {
|
||||
matches[idx * k + i].second = idx;
|
||||
if (i < closest_idx.size()) {
|
||||
matches[idx * k + i].first = closest_idx[i];
|
||||
} else {
|
||||
matches[idx * k + i].first = math::kINVALID_ID;
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void OctoTree::Clear() {
|
||||
for (const auto &np : nodes_) {
|
||||
if (np.second != root_.get()) {
|
||||
delete np.second;
|
||||
}
|
||||
}
|
||||
|
||||
root_ = nullptr;
|
||||
size_ = 0;
|
||||
tree_node_id_ = 0;
|
||||
}
|
||||
|
||||
void OctoTree::Reset() {
|
||||
tree_node_id_ = 0;
|
||||
root_.reset(new OctoTreeNode());
|
||||
root_->id_ = tree_node_id_++;
|
||||
size_ = 0;
|
||||
}
|
||||
|
||||
void OctoTree::Knn(const Vec3f &pt, OctoTreeNode *node, std::priority_queue<NodeAndDistanceOcto> &knn_result) const {
|
||||
if (node->IsLeaf()) {
|
||||
if (node->point_idx_ != -1) {
|
||||
// 如果是叶子，看该点是否为最近邻
|
||||
ComputeDisForLeaf(pt, node, knn_result);
|
||||
return;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// 看pt落在哪一格，优先搜索pt所在的子树
|
||||
// 然后再看其他子树是否需要搜索
|
||||
// 如果pt在外边，优先搜索最近的子树
|
||||
int idx_child = -1;
|
||||
float min_dis = std::numeric_limits<float>::max();
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (node->children[i]->box_.Inside(pt)) {
|
||||
idx_child = i;
|
||||
break;
|
||||
} else {
|
||||
float d = node->children[i]->box_.Dis(pt);
|
||||
if (d < min_dis) {
|
||||
idx_child = i;
|
||||
min_dis = d;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 先检查idx_child
|
||||
Knn(pt, node->children[idx_child], knn_result);
|
||||
|
||||
// 再检查其他的
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
if (i == idx_child) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (NeedExpand(pt, node->children[i], knn_result)) {
|
||||
Knn(pt, node->children[i], knn_result);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void OctoTree::ComputeDisForLeaf(const Vec3f &pt, OctoTreeNode *node,
|
||||
std::priority_queue<NodeAndDistanceOcto> &knn_result) const {
|
||||
// 比较与结果队列的差异，如果优于最远距离，则插入
|
||||
float dis2 = Dis2(pt, cloud_[node->point_idx_]);
|
||||
if (knn_result.size() < k_) {
|
||||
// results 不足k
|
||||
knn_result.push({node, dis2});
|
||||
} else {
|
||||
// results等于k，比较current与max_dis_iter之间的差异
|
||||
if (dis2 < knn_result.top().distance_) {
|
||||
knn_result.push({node, dis2});
|
||||
knn_result.pop();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool OctoTree::NeedExpand(const Vec3f &pt, OctoTreeNode *node,
|
||||
std::priority_queue<NodeAndDistanceOcto> &knn_result) const {
|
||||
if (knn_result.size() < k_) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (approximate_) {
|
||||
float d = node->box_.Dis(pt);
|
||||
if ((d * d) < knn_result.top().distance_ * alpha_) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
// 不用flann时，按通常情况查找
|
||||
float d = node->box_.Dis(pt);
|
||||
if ((d * d) < knn_result.top().distance_) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
164
workspace/slam_in_autonomous_driving/src/ch5/octo_tree.h
Normal file
164
workspace/slam_in_autonomous_driving/src/ch5/octo_tree.h
Normal file
@@ -0,0 +1,164 @@
|
||||
//
|
||||
// Created by xiang on 2021/9/27.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_OCTO_TREE_H
|
||||
#define SLAM_IN_AUTO_DRIVING_OCTO_TREE_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/point_types.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
|
||||
namespace sad {
|
||||
|
||||
// 3D Box 记录各轴上的最大最小值
|
||||
struct Box3D {
|
||||
Box3D() = default;
|
||||
Box3D(float min_x, float max_x, float min_y, float max_y, float min_z, float max_z)
|
||||
: min_{min_x, min_y, min_z}, max_{max_x, max_y, max_z} {}
|
||||
|
||||
/// 判断pt是否在内部
|
||||
bool Inside(const Vec3f& pt) const {
|
||||
return pt[0] <= max_[0] && pt[0] >= min_[0] && pt[1] <= max_[1] && pt[1] >= min_[1] && pt[2] <= max_[2] &&
|
||||
pt[2] >= min_[2];
|
||||
}
|
||||
|
||||
/// 点到3D Box距离
|
||||
/// 我们取外侧点到边界的最大值
|
||||
float Dis(const Vec3f& pt) const {
|
||||
float ret = 0;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (pt[i] < min_[i]) {
|
||||
float d = min_[i] - pt[i];
|
||||
ret = d > ret ? d : ret;
|
||||
} else if (pt[i] > max_[i]) {
|
||||
float d = pt[i] - max_[i];
|
||||
ret = d > ret ? d : ret;
|
||||
}
|
||||
}
|
||||
|
||||
assert(ret >= 0);
|
||||
return ret;
|
||||
}
|
||||
|
||||
float min_[3] = {0};
|
||||
float max_[3] = {0};
|
||||
};
|
||||
|
||||
/// octo tree 节点
|
||||
struct OctoTreeNode {
|
||||
int id_ = -1;
|
||||
int point_idx_ = -1; // 点的索引，-1为无效
|
||||
Box3D box_; // 边界框
|
||||
OctoTreeNode* children[8] = {nullptr}; // 子节点
|
||||
|
||||
bool IsLeaf() const {
|
||||
for (const OctoTreeNode* n : children) {
|
||||
if (n != nullptr) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
/// 用于记录knn结果
|
||||
struct NodeAndDistanceOcto {
|
||||
NodeAndDistanceOcto(OctoTreeNode* node, float dis2) : node_(node), distance_(dis2) {}
|
||||
OctoTreeNode* node_ = nullptr;
|
||||
float distance_ = 0; // 平方距离，用于比较
|
||||
|
||||
bool operator<(const NodeAndDistanceOcto& other) const { return distance_ < other.distance_; }
|
||||
};
|
||||
|
||||
class OctoTree {
|
||||
public:
|
||||
explicit OctoTree() = default;
|
||||
~OctoTree() { Clear(); }
|
||||
|
||||
bool BuildTree(const CloudPtr& cloud);
|
||||
|
||||
/// 获取k最近邻
|
||||
bool GetClosestPoint(const PointType& pt, std::vector<int>& closest_idx, int k = 5) const;
|
||||
|
||||
/// 并行为点云寻找最近邻
|
||||
bool GetClosestPointMT(const CloudPtr& cloud, std::vector<std::pair<size_t, size_t>>& match, int k = 5);
|
||||
|
||||
/// 设置近似最近邻参数
|
||||
void SetApproximate(bool use_ann = true, float alpha = 0.1) {
|
||||
approximate_ = use_ann;
|
||||
alpha_ = alpha;
|
||||
}
|
||||
|
||||
/// 返回节点数量
|
||||
size_t size() const { return size_; }
|
||||
|
||||
/// 清理数据
|
||||
void Clear();
|
||||
|
||||
private:
|
||||
/// kdtree 构建相关
|
||||
/**
|
||||
* 在node处插入点
|
||||
* @param points
|
||||
* @param node
|
||||
*/
|
||||
void Insert(const IndexVec& points, OctoTreeNode* node);
|
||||
|
||||
/// 为全局点云生成边界框
|
||||
Box3D ComputeBoundingBox();
|
||||
|
||||
/**
|
||||
* 展开一个节点
|
||||
* @param [in] node 被展开的节点
|
||||
* @param [in] parent_idx 父节点的点云索引
|
||||
* @param [out] children_idx 子节点的点云索引
|
||||
*/
|
||||
void ExpandNode(OctoTreeNode* node, const IndexVec& parent_idx, std::vector<IndexVec>& children_idx);
|
||||
|
||||
void Reset();
|
||||
|
||||
/// 两个点的平方距离
|
||||
static inline float Dis2(const Vec3f& p1, const Vec3f& p2) { return (p1 - p2).squaredNorm(); }
|
||||
|
||||
// Knn 相关
|
||||
/**
|
||||
* 检查给定点在kdtree node上的knn，可以递归调用
|
||||
* @param pt 查询点
|
||||
* @param node kdtree 节点
|
||||
*/
|
||||
void Knn(const Vec3f& pt, OctoTreeNode* node, std::priority_queue<NodeAndDistanceOcto>& result) const;
|
||||
|
||||
/**
|
||||
* 对叶子节点，计算它和查询点的距离，尝试放入结果中
|
||||
* @param pt 查询点
|
||||
* @param node Kdtree 节点
|
||||
*/
|
||||
void ComputeDisForLeaf(const Vec3f& pt, OctoTreeNode* node, std::priority_queue<NodeAndDistanceOcto>& result) const;
|
||||
|
||||
/**
|
||||
* 检查node下是否需要展开
|
||||
* @param pt 查询点
|
||||
* @param node Kdtree 节点
|
||||
* @return true if 需要展开
|
||||
*/
|
||||
bool NeedExpand(const Vec3f& pt, OctoTreeNode* node, std::priority_queue<NodeAndDistanceOcto>& knn_result) const;
|
||||
|
||||
int k_ = 5; // knn最近邻数量
|
||||
std::shared_ptr<OctoTreeNode> root_ = nullptr; // 叶子节点
|
||||
std::vector<Vec3f> cloud_; // 输入点云
|
||||
std::map<int, OctoTreeNode*> nodes_; // for bookkeeping
|
||||
size_t size_ = 0; // 叶子节点数量
|
||||
int tree_node_id_ = 0; // 为kdtree node 分配id
|
||||
|
||||
// flann
|
||||
bool approximate_ = false;
|
||||
float alpha_ = 1.0; // flann的距离倍数
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_OCTO_TREE_H
|
||||
@@ -0,0 +1,85 @@
|
||||
//
|
||||
// Created by xiang on 2021/8/9.
|
||||
//
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
using PointType = pcl::PointXYZI;
|
||||
using PointCloudType = pcl::PointCloud<PointType>;
|
||||
|
||||
DEFINE_string(pcd_path, "./data/ch5/map_example.pcd", "点云文件路径");
|
||||
DEFINE_double(image_resolution, 0.1, "俯视图分辨率");
|
||||
DEFINE_double(min_z, 0.2, "俯视图最低高度");
|
||||
DEFINE_double(max_z, 2.5, "俯视图最高高度");
|
||||
|
||||
/// 本节演示如何将一个点云转换为俯视图像
|
||||
void GenerateBEVImage(PointCloudType::Ptr cloud) {
|
||||
// 计算点云边界
|
||||
auto minmax_x = std::minmax_element(cloud->points.begin(), cloud->points.end(),
|
||||
[](const PointType& p1, const PointType& p2) { return p1.x < p2.x; });
|
||||
auto minmax_y = std::minmax_element(cloud->points.begin(), cloud->points.end(),
|
||||
[](const PointType& p1, const PointType& p2) { return p1.y < p2.y; });
|
||||
double min_x = minmax_x.first->x;
|
||||
double max_x = minmax_x.second->x;
|
||||
double min_y = minmax_y.first->y;
|
||||
double max_y = minmax_y.second->y;
|
||||
|
||||
const double inv_r = 1.0 / FLAGS_image_resolution;
|
||||
|
||||
const int image_rows = int((max_y - min_y) * inv_r);
|
||||
const int image_cols = int((max_x - min_x) * inv_r);
|
||||
|
||||
float x_center = 0.5 * (max_x + min_x);
|
||||
float y_center = 0.5 * (max_y + min_y);
|
||||
float x_center_image = image_cols / 2;
|
||||
float y_center_image = image_rows / 2;
|
||||
|
||||
// 生成图像
|
||||
cv::Mat image(image_rows, image_cols, CV_8UC3, cv::Scalar(255, 255, 255));
|
||||
|
||||
for (const auto& pt : cloud->points) {
|
||||
int x = int((pt.x - x_center) * inv_r + x_center_image);
|
||||
int y = int((pt.y - y_center) * inv_r + y_center_image);
|
||||
if (x < 0 || x >= image_cols || y < 0 || y >= image_rows || pt.z < FLAGS_min_z || pt.z > FLAGS_max_z) {
|
||||
continue;
|
||||
}
|
||||
|
||||
image.at<cv::Vec3b>(y, x) = cv::Vec3b(227, 143, 79);
|
||||
}
|
||||
|
||||
cv::imwrite("./bev.png", image);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (FLAGS_pcd_path.empty()) {
|
||||
LOG(ERROR) << "pcd path is empty";
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 读取点云
|
||||
PointCloudType::Ptr cloud(new PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
|
||||
|
||||
if (cloud->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud file";
|
||||
return -1;
|
||||
}
|
||||
|
||||
LOG(INFO) << "cloud points: " << cloud->size();
|
||||
GenerateBEVImage(cloud);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
//
|
||||
// Created by xiang on 2021/8/6.
|
||||
//
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl/visualization/pcl_visualizer.h>
|
||||
|
||||
using PointType = pcl::PointXYZI;
|
||||
using PointCloudType = pcl::PointCloud<PointType>;
|
||||
|
||||
DEFINE_string(pcd_path, "./data/ch5/map_example.pcd", "点云文件路径");
|
||||
|
||||
/// 本程序可用于显示单个点云，演示PCL的基本用法
|
||||
/// 实际上就是调用了pcl的可视化库，类似于pcl_viewer
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (FLAGS_pcd_path.empty()) {
|
||||
LOG(ERROR) << "pcd path is empty";
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 读取点云
|
||||
PointCloudType::Ptr cloud(new PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
|
||||
|
||||
if (cloud->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud file";
|
||||
return -1;
|
||||
}
|
||||
|
||||
LOG(INFO) << "cloud points: " << cloud->size();
|
||||
|
||||
// visualize
|
||||
pcl::visualization::PCLVisualizer viewer("cloud viewer");
|
||||
pcl::visualization::PointCloudColorHandlerGenericField<PointType> handle(cloud, "z"); // 使用高度来着色
|
||||
viewer.addPointCloud<PointType>(cloud, handle);
|
||||
viewer.spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
// // Created by xiang on 2021/8/9.
|
||||
//
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
using PointType = pcl::PointXYZI;
|
||||
using PointCloudType = pcl::PointCloud<PointType>;
|
||||
|
||||
DEFINE_string(pcd_path, "./data/ch5/scan_example.pcd", "点云文件路径");
|
||||
DEFINE_double(azimuth_resolution_deg, 0.3, "方位角分辨率（度）");
|
||||
DEFINE_int32(elevation_rows, 16, "俯仰角对应的行数");
|
||||
DEFINE_double(elevation_range, 15.0, "俯仰角范围"); // VLP-16 上下各15度范围
|
||||
DEFINE_double(lidar_height, 1.128, "雷达安装高度");
|
||||
|
||||
void GenerateRangeImage(PointCloudType::Ptr cloud) {
|
||||
int image_cols = int(360 / FLAGS_azimuth_resolution_deg); // 水平为360度，按分辨率切分即可
|
||||
int image_rows = FLAGS_elevation_rows; // 图像行数固定
|
||||
LOG(INFO) << "range image: " << image_rows << "x" << image_cols;
|
||||
|
||||
// 我们生成一个HSV图像以更好地显示图像
|
||||
cv::Mat image(image_rows, image_cols, CV_8UC3, cv::Scalar(0, 0, 0));
|
||||
|
||||
double ele_resolution = FLAGS_elevation_range * 2 / FLAGS_elevation_rows; // elevation分辨率
|
||||
|
||||
for (const auto& pt : cloud->points) {
|
||||
double azimuth = atan2(pt.y, pt.x) * 180 / M_PI;
|
||||
double range = sqrt(pt.x * pt.x + pt.y * pt.y);
|
||||
double elevation = asin((pt.z - FLAGS_lidar_height) / range) * 180 / M_PI;
|
||||
|
||||
// keep in 0~360
|
||||
if (azimuth < 0) {
|
||||
azimuth += 360;
|
||||
}
|
||||
|
||||
int x = int(azimuth / FLAGS_azimuth_resolution_deg); // 行
|
||||
int y = int((elevation + FLAGS_elevation_range) / ele_resolution + 0.5); // 列
|
||||
|
||||
if (x >= 0 && x < image.cols && y >= 0 && y < image.rows) {
|
||||
image.at<cv::Vec3b>(y, x) = cv::Vec3b(uchar(range / 100 * 255.0), 255, 127);
|
||||
}
|
||||
}
|
||||
|
||||
// 沿Y轴翻转，因为我们希望Z轴朝上时Y朝上
|
||||
cv::Mat image_flipped;
|
||||
cv::flip(image, image_flipped, 0);
|
||||
|
||||
// hsv to rgb
|
||||
cv::Mat image_rgb;
|
||||
cv::cvtColor(image_flipped, image_rgb, cv::COLOR_HSV2BGR);
|
||||
cv::imwrite("./range_image.png", image_rgb);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
|
||||
if (FLAGS_pcd_path.empty()) {
|
||||
LOG(ERROR) << "pcd path is empty";
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 读取点云
|
||||
PointCloudType::Ptr cloud(new PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);
|
||||
|
||||
if (cloud->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud file";
|
||||
return -1;
|
||||
}
|
||||
|
||||
LOG(INFO) << "cloud points: " << cloud->size();
|
||||
GenerateRangeImage(cloud);
|
||||
|
||||
return 0;
|
||||
}
|
||||
348
workspace/slam_in_autonomous_driving/src/ch5/test_nn.cc
Normal file
348
workspace/slam_in_autonomous_driving/src/ch5/test_nn.cc
Normal file
@@ -0,0 +1,348 @@
|
||||
//
|
||||
// Created by xiang on 2021/8/19.
|
||||
//
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <pcl/io/pcd_io.h>
|
||||
#include <pcl/search/kdtree.h>
|
||||
|
||||
#include "ch5/bfnn.h"
|
||||
#include "ch5/gridnn.hpp"
|
||||
#include "ch5/kdtree.h"
|
||||
#include "ch5/octo_tree.h"
|
||||
#include "common/point_cloud_utils.h"
|
||||
#include "common/point_types.h"
|
||||
#include "common/sys_utils.h"
|
||||
|
||||
DEFINE_string(first_scan_path, "./data/ch5/first.pcd", "第一个点云路径");
|
||||
DEFINE_string(second_scan_path, "./data/ch5/second.pcd", "第二个点云路径");
|
||||
DEFINE_double(ANN_alpha, 1.0, "AAN的比例因子");
|
||||
|
||||
TEST(CH5_TEST, BFNN) {
|
||||
sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
|
||||
pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
|
||||
|
||||
if (first->empty() || second->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud";
|
||||
FAIL();
|
||||
}
|
||||
|
||||
// voxel grid 至 0.05
|
||||
sad::VoxelGrid(first);
|
||||
sad::VoxelGrid(second);
|
||||
|
||||
LOG(INFO) << "points: " << first->size() << ", " << second->size();
|
||||
|
||||
// 评价单线程和多线程版本的暴力匹配
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second]() {
|
||||
std::vector<std::pair<size_t, size_t>> matches;
|
||||
sad::bfnn_cloud(first, second, matches);
|
||||
},
|
||||
"暴力匹配（单线程）", 5);
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second]() {
|
||||
std::vector<std::pair<size_t, size_t>> matches;
|
||||
sad::bfnn_cloud_mt(first, second, matches);
|
||||
},
|
||||
"暴力匹配（多线程）", 5);
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
/**
|
||||
* 评测最近邻的正确性
|
||||
* @param truth 真值
|
||||
* @param esti 估计
|
||||
*/
|
||||
void EvaluateMatches(const std::vector<std::pair<size_t, size_t>>& truth,
|
||||
const std::vector<std::pair<size_t, size_t>>& esti) {
|
||||
int fp = 0; // false-positive，esti存在但truth中不存在
|
||||
int fn = 0; // false-negative, truth存在但esti不存在
|
||||
|
||||
LOG(INFO) << "truth: " << truth.size() << ", esti: " << esti.size();
|
||||
|
||||
/// 检查某个匹配在另一个容器中存不存在
|
||||
auto exist = [](const std::pair<size_t, size_t>& data, const std::vector<std::pair<size_t, size_t>>& vec) -> bool {
|
||||
return std::find(vec.begin(), vec.end(), data) != vec.end();
|
||||
};
|
||||
|
||||
int effective_esti = 0;
|
||||
for (const auto& d : esti) {
|
||||
if (d.first != sad::math::kINVALID_ID && d.second != sad::math::kINVALID_ID) {
|
||||
effective_esti++;
|
||||
|
||||
if (!exist(d, truth)) {
|
||||
fp++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto& d : truth) {
|
||||
if (!exist(d, esti)) {
|
||||
fn++;
|
||||
}
|
||||
}
|
||||
|
||||
float precision = 1.0 - float(fp) / effective_esti;
|
||||
float recall = 1.0 - float(fn) / truth.size();
|
||||
LOG(INFO) << "precision: " << precision << ", recall: " << recall << ", fp: " << fp << ", fn: " << fn;
|
||||
}
|
||||
|
||||
TEST(CH5_TEST, GRID_NN) {
|
||||
sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
|
||||
pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
|
||||
|
||||
if (first->empty() || second->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud";
|
||||
FAIL();
|
||||
}
|
||||
|
||||
// voxel grid 至 0.05
|
||||
sad::VoxelGrid(first);
|
||||
sad::VoxelGrid(second);
|
||||
|
||||
LOG(INFO) << "points: " << first->size() << ", " << second->size();
|
||||
|
||||
std::vector<std::pair<size_t, size_t>> truth_matches;
|
||||
sad::bfnn_cloud(first, second, truth_matches);
|
||||
|
||||
// 对比不同种类的grid
|
||||
sad::GridNN<2> grid0(0.1, sad::GridNN<2>::NearbyType::CENTER), grid4(0.1, sad::GridNN<2>::NearbyType::NEARBY4),
|
||||
grid8(0.1, sad::GridNN<2>::NearbyType::NEARBY8);
|
||||
sad::GridNN<3> grid3(0.1, sad::GridNN<3>::NearbyType::NEARBY6);
|
||||
|
||||
grid0.SetPointCloud(first);
|
||||
grid4.SetPointCloud(first);
|
||||
grid8.SetPointCloud(first);
|
||||
grid3.SetPointCloud(first);
|
||||
|
||||
// 评价各种版本的Grid NN
|
||||
// sorry没有C17的template lambda... 下面必须写的啰嗦一些
|
||||
LOG(INFO) << "===================";
|
||||
std::vector<std::pair<size_t, size_t>> matches;
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid0, &matches]() { grid0.GetClosestPointForCloud(first, second, matches); },
|
||||
"Grid0 单线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid0, &matches]() { grid0.GetClosestPointForCloudMT(first, second, matches); },
|
||||
"Grid0 多线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid4, &matches]() { grid4.GetClosestPointForCloud(first, second, matches); },
|
||||
"Grid4 单线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid4, &matches]() { grid4.GetClosestPointForCloudMT(first, second, matches); },
|
||||
"Grid4 多线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid8, &matches]() { grid8.GetClosestPointForCloud(first, second, matches); },
|
||||
"Grid8 单线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid8, &matches]() { grid8.GetClosestPointForCloudMT(first, second, matches); },
|
||||
"Grid8 多线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid3, &matches]() { grid3.GetClosestPointForCloud(first, second, matches); },
|
||||
"Grid 3D 单线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
LOG(INFO) << "===================";
|
||||
sad::evaluate_and_call(
|
||||
[&first, &second, &grid3, &matches]() { grid3.GetClosestPointForCloudMT(first, second, matches); },
|
||||
"Grid 3D 多线程", 10);
|
||||
EvaluateMatches(truth_matches, matches);
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(CH5_TEST, KDTREE_BASICS) {
|
||||
sad::CloudPtr cloud(new sad::PointCloudType);
|
||||
sad::PointType p1, p2, p3, p4;
|
||||
p1.x = 0;
|
||||
p1.y = 0;
|
||||
p1.z = 0;
|
||||
|
||||
p2.x = 1;
|
||||
p2.y = 0;
|
||||
p2.z = 0;
|
||||
|
||||
p3.x = 0;
|
||||
p3.y = 1;
|
||||
p3.z = 0;
|
||||
|
||||
p4.x = 1;
|
||||
p4.y = 1;
|
||||
p4.z = 0;
|
||||
|
||||
cloud->points.push_back(p1);
|
||||
cloud->points.push_back(p2);
|
||||
cloud->points.push_back(p3);
|
||||
cloud->points.push_back(p4);
|
||||
|
||||
sad::KdTree kdtree;
|
||||
kdtree.BuildTree(cloud);
|
||||
kdtree.PrintAll();
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(CH5_TEST, KDTREE_KNN) {
|
||||
sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
|
||||
pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
|
||||
|
||||
if (first->empty() || second->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud";
|
||||
FAIL();
|
||||
}
|
||||
|
||||
// voxel grid 至 0.05
|
||||
sad::VoxelGrid(first);
|
||||
sad::VoxelGrid(second);
|
||||
|
||||
sad::KdTree kdtree;
|
||||
sad::evaluate_and_call([&first, &kdtree]() { kdtree.BuildTree(first); }, "Kd Tree build", 1);
|
||||
|
||||
kdtree.SetEnableANN(true, FLAGS_ANN_alpha);
|
||||
|
||||
LOG(INFO) << "Kd tree leaves: " << kdtree.size() << ", points: " << first->size();
|
||||
|
||||
// 比较 bfnn
|
||||
std::vector<std::pair<size_t, size_t>> true_matches;
|
||||
sad::bfnn_cloud_mt_k(first, second, true_matches);
|
||||
|
||||
// 对第2个点云执行knn
|
||||
std::vector<std::pair<size_t, size_t>> matches;
|
||||
sad::evaluate_and_call([&first, &second, &kdtree, &matches]() { kdtree.GetClosestPointMT(second, matches, 5); },
|
||||
"Kd Tree 5NN 多线程", 1);
|
||||
EvaluateMatches(true_matches, matches);
|
||||
|
||||
LOG(INFO) << "building kdtree pcl";
|
||||
// 对比PCL
|
||||
pcl::search::KdTree<sad::PointType> kdtree_pcl;
|
||||
sad::evaluate_and_call([&first, &kdtree_pcl]() { kdtree_pcl.setInputCloud(first); }, "Kd Tree build", 1);
|
||||
|
||||
LOG(INFO) << "searching pcl";
|
||||
matches.clear();
|
||||
std::vector<int> search_indices(second->size());
|
||||
for (int i = 0; i < second->points.size(); i++) {
|
||||
search_indices[i] = i;
|
||||
}
|
||||
|
||||
std::vector<std::vector<int>> result_index;
|
||||
std::vector<std::vector<float>> result_distance;
|
||||
sad::evaluate_and_call(
|
||||
[&]() { kdtree_pcl.nearestKSearch(*second, search_indices, 5, result_index, result_distance); },
|
||||
"Kd Tree 5NN in PCL", 1);
|
||||
for (int i = 0; i < second->points.size(); i++) {
|
||||
for (int j = 0; j < result_index[i].size(); ++j) {
|
||||
int m = result_index[i][j];
|
||||
double d = result_distance[i][j];
|
||||
matches.push_back({m, i});
|
||||
}
|
||||
}
|
||||
EvaluateMatches(true_matches, matches);
|
||||
|
||||
LOG(INFO) << "done.";
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(CH5_TEST, OCTREE_BASICS) {
|
||||
sad::CloudPtr cloud(new sad::PointCloudType);
|
||||
sad::PointType p1, p2, p3, p4;
|
||||
p1.x = 0;
|
||||
p1.y = 0;
|
||||
p1.z = 0;
|
||||
|
||||
p2.x = 1;
|
||||
p2.y = 0;
|
||||
p2.z = 0;
|
||||
|
||||
p3.x = 0;
|
||||
p3.y = 1;
|
||||
p3.z = 0;
|
||||
|
||||
p4.x = 1;
|
||||
p4.y = 1;
|
||||
p4.z = 0;
|
||||
|
||||
cloud->points.push_back(p1);
|
||||
cloud->points.push_back(p2);
|
||||
cloud->points.push_back(p3);
|
||||
cloud->points.push_back(p4);
|
||||
|
||||
sad::OctoTree octree;
|
||||
octree.BuildTree(cloud);
|
||||
octree.SetApproximate(false);
|
||||
LOG(INFO) << "Octo tree leaves: " << octree.size() << ", points: " << cloud->size();
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
TEST(CH5_TEST, OCTREE_KNN) {
|
||||
sad::CloudPtr first(new sad::PointCloudType), second(new sad::PointCloudType);
|
||||
pcl::io::loadPCDFile(FLAGS_first_scan_path, *first);
|
||||
pcl::io::loadPCDFile(FLAGS_second_scan_path, *second);
|
||||
|
||||
if (first->empty() || second->empty()) {
|
||||
LOG(ERROR) << "cannot load cloud";
|
||||
FAIL();
|
||||
}
|
||||
|
||||
// voxel grid 至 0.05
|
||||
sad::VoxelGrid(first);
|
||||
sad::VoxelGrid(second);
|
||||
|
||||
sad::OctoTree octree;
|
||||
sad::evaluate_and_call([&first, &octree]() { octree.BuildTree(first); }, "Octo Tree build", 1);
|
||||
|
||||
octree.SetApproximate(true, FLAGS_ANN_alpha);
|
||||
LOG(INFO) << "Octo tree leaves: " << octree.size() << ", points: " << first->size();
|
||||
|
||||
/// 测试KNN
|
||||
LOG(INFO) << "testing knn";
|
||||
std::vector<std::pair<size_t, size_t>> matches;
|
||||
sad::evaluate_and_call([&first, &second, &octree, &matches]() { octree.GetClosestPointMT(second, matches, 5); },
|
||||
"Octo Tree 5NN 多线程", 1);
|
||||
|
||||
LOG(INFO) << "comparing with bfnn";
|
||||
/// 比较真值
|
||||
std::vector<std::pair<size_t, size_t>> true_matches;
|
||||
sad::bfnn_cloud_mt_k(first, second, true_matches);
|
||||
EvaluateMatches(true_matches, matches);
|
||||
|
||||
LOG(INFO) << "done.";
|
||||
|
||||
SUCCEED();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
google::InitGoogleLogging(argv[0]);
|
||||
FLAGS_stderrthreshold = google::INFO;
|
||||
FLAGS_colorlogtostderr = true;
|
||||
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
google::ParseCommandLineFlags(&argc, &argv, true);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
93
workspace/slam_in_autonomous_driving/src/ch6/CMakeLists.txt
Normal file
93
workspace/slam_in_autonomous_driving/src/ch6/CMakeLists.txt
Normal file
@@ -0,0 +1,93 @@
|
||||
add_library(${PROJECT_NAME}.ch6
|
||||
icp_2d.cc
|
||||
lidar_2d_utils.cc
|
||||
likelihood_field.cc
|
||||
occupancy_map.cc
|
||||
submap.cc
|
||||
mapping_2d.cc
|
||||
multi_resolution_likelihood_field.cc
|
||||
loop_closing.cc
|
||||
frame.cc
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}.ch6
|
||||
${PCL_LIBRARIES}
|
||||
${PROJECT_NAME}.common
|
||||
)
|
||||
|
||||
add_executable(test_2dlidar_io
|
||||
test_2dlidar_io.cc
|
||||
)
|
||||
|
||||
target_link_libraries(test_2dlidar_io
|
||||
${PROJECT_NAME}.ch6
|
||||
${OpenCV_LIBS}
|
||||
${catkin_LIBRARIES}
|
||||
glog gflags
|
||||
${PCL_LIBRARIES}
|
||||
${third_party_libs}
|
||||
)
|
||||
|
||||
add_executable(test_2d_icp_s2s
|
||||
test_2d_icp_s2s.cc
|
||||
)
|
||||
|
||||
target_link_libraries(test_2d_icp_s2s
|
||||
${PROJECT_NAME}.ch6
|
||||
${OpenCV_LIBS}
|
||||
${catkin_LIBRARIES}
|
||||
glog gflags
|
||||
${PCL_LIBRARIES}
|
||||
)
|
||||
|
||||
add_executable(test_2d_icp_likelihood
|
||||
test_2d_icp_likelihood.cc
|
||||
)
|
||||
|
||||
target_link_libraries(test_2d_icp_likelihood
|
||||
${PROJECT_NAME}.ch6
|
||||
${OpenCV_LIBS}
|
||||
${catkin_LIBRARIES}
|
||||
glog gflags
|
||||
${PCL_LIBRARIES}
|
||||
${g2o_libs}
|
||||
)
|
||||
|
||||
add_executable(test_occupancy_grid
|
||||
test_occupancy_grid.cc
|
||||
)
|
||||
|
||||
target_link_libraries(test_occupancy_grid
|
||||
${PROJECT_NAME}.ch6
|
||||
${OpenCV_LIBS}
|
||||
${catkin_LIBRARIES}
|
||||
glog gflags
|
||||
${PCL_LIBRARIES}
|
||||
${g2o_libs}
|
||||
)
|
||||
|
||||
add_executable(test_2d_mapping
|
||||
test_2d_mapping.cc
|
||||
)
|
||||
|
||||
target_link_libraries(test_2d_mapping
|
||||
${PROJECT_NAME}.ch6
|
||||
${OpenCV_LIBS}
|
||||
${catkin_LIBRARIES}
|
||||
glog gflags
|
||||
${PCL_LIBRARIES}
|
||||
${g2o_libs}
|
||||
)
|
||||
|
||||
add_executable(test_mr_matching
|
||||
test_mr_matching.cc
|
||||
)
|
||||
|
||||
target_link_libraries(test_mr_matching
|
||||
${PROJECT_NAME}.ch6
|
||||
${OpenCV_LIBS}
|
||||
${catkin_LIBRARIES}
|
||||
glog gflags
|
||||
${PCL_LIBRARIES}
|
||||
${g2o_libs}
|
||||
)
|
||||
47
workspace/slam_in_autonomous_driving/src/ch6/frame.cc
Normal file
47
workspace/slam_in_autonomous_driving/src/ch6/frame.cc
Normal file
@@ -0,0 +1,47 @@
|
||||
//
|
||||
// Created by xiang on 2022/4/7.
|
||||
//
|
||||
|
||||
#include "frame.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <fstream>
|
||||
|
||||
namespace sad {
|
||||
|
||||
void Frame::Dump(const std::string& filename) {
|
||||
std::ofstream fout(filename);
|
||||
fout << id_ << " " << keyframe_id_ << " " << timestamp_ << std::endl;
|
||||
fout << pose_.translation()[0] << " " << pose_.translation()[1] << " " << pose_.so2().log() << std::endl;
|
||||
fout << scan_->angle_min << " " << scan_->angle_max << " " << scan_->angle_increment << " " << scan_->range_min
|
||||
<< " " << scan_->range_max << " " << scan_->ranges.size() << std::endl;
|
||||
for (auto& r : scan_->ranges) {
|
||||
fout << r << " ";
|
||||
}
|
||||
fout.close();
|
||||
}
|
||||
|
||||
void Frame::Load(const std::string& filename) {
|
||||
std::ifstream fin(filename);
|
||||
if (!fin) {
|
||||
LOG(ERROR) << "cannot load from " << filename;
|
||||
return;
|
||||
}
|
||||
|
||||
fin >> id_ >> keyframe_id_ >> timestamp_ >> pose_.translation()[0] >> pose_.translation()[1];
|
||||
double theta = 0;
|
||||
fin >> theta;
|
||||
pose_.so2() = SO2::exp(theta);
|
||||
scan_.reset(new Scan2d);
|
||||
fin >> scan_->angle_min >> scan_->angle_max >> scan_->angle_increment >> scan_->range_min >> scan_->range_max;
|
||||
|
||||
int range_size;
|
||||
fin >> range_size;
|
||||
for (int i = 0; i < range_size; ++i) {
|
||||
double r;
|
||||
fin >> r;
|
||||
scan_->ranges.emplace_back(r);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
36
workspace/slam_in_autonomous_driving/src/ch6/frame.h
Normal file
36
workspace/slam_in_autonomous_driving/src/ch6/frame.h
Normal file
@@ -0,0 +1,36 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/23.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_FRAME_H
|
||||
#define SLAM_IN_AUTO_DRIVING_FRAME_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/lidar_utils.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 一次2D lidar扫描
|
||||
*/
|
||||
struct Frame {
|
||||
Frame() {}
|
||||
Frame(Scan2d::Ptr scan) : scan_(scan) {}
|
||||
|
||||
/// 将当前帧存入文本文件以供离线调用
|
||||
void Dump(const std::string& filename);
|
||||
|
||||
/// 从文件读取frame数据
|
||||
void Load(const std::string& filename);
|
||||
|
||||
size_t id_ = 0; // scan id
|
||||
size_t keyframe_id_ = 0; // 关键帧 id
|
||||
double timestamp_ = 0; // 时间戳，一般不用
|
||||
Scan2d::Ptr scan_ = nullptr; // 激光扫描数据
|
||||
SE2 pose_; // 位姿，world to scan, T_w_c
|
||||
SE2 pose_submap_; // 位姿，submap to scan, T_s_c
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_FRAME_H
|
||||
133
workspace/slam_in_autonomous_driving/src/ch6/g2o_types.h
Normal file
133
workspace/slam_in_autonomous_driving/src/ch6/g2o_types.h
Normal file
@@ -0,0 +1,133 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/22.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_G2O_TYPES_H
|
||||
#define SLAM_IN_AUTO_DRIVING_G2O_TYPES_H
|
||||
|
||||
#include <g2o/core/base_binary_edge.h>
|
||||
#include <g2o/core/base_unary_edge.h>
|
||||
#include <g2o/core/base_vertex.h>
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/math_utils.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
class VertexSE2 : public g2o::BaseVertex<3, SE2> {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
void setToOriginImpl() override { _estimate = SE2(); }
|
||||
void oplusImpl(const double* update) override {
|
||||
_estimate.translation()[0] += update[0];
|
||||
_estimate.translation()[1] += update[1];
|
||||
_estimate.so2() = _estimate.so2() * SO2::exp(update[2]);
|
||||
}
|
||||
|
||||
bool read(std::istream& is) override { return true; }
|
||||
bool write(std::ostream& os) const override { return true; }
|
||||
};
|
||||
|
||||
class EdgeSE2LikelihoodFiled : public g2o::BaseUnaryEdge<1, double, VertexSE2> {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
|
||||
EdgeSE2LikelihoodFiled(const cv::Mat& field_image, double range, double angle, float resolution = 10.0)
|
||||
: field_image_(field_image), range_(range), angle_(angle), resolution_(resolution) {}
|
||||
|
||||
/// 判定此条边是否在field image外面
|
||||
bool IsOutSide() {
|
||||
VertexSE2* v = (VertexSE2*)_vertices[0];
|
||||
SE2 pose = v->estimate();
|
||||
Vec2d pw = pose * Vec2d(range_ * std::cos(angle_), range_ * std::sin(angle_));
|
||||
Vec2i pf = (pw * resolution_ + Vec2d(field_image_.rows / 2, field_image_.cols / 2)).cast<int>(); // 图像坐标
|
||||
|
||||
if (pf[0] >= image_boarder_ && pf[0] < field_image_.cols - image_boarder_ && pf[1] >= image_boarder_ &&
|
||||
pf[1] < field_image_.rows - image_boarder_) {
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void computeError() override {
|
||||
VertexSE2* v = (VertexSE2*)_vertices[0];
|
||||
SE2 pose = v->estimate();
|
||||
Vec2d pw = pose * Vec2d(range_ * std::cos(angle_), range_ * std::sin(angle_));
|
||||
Vec2d pf =
|
||||
pw * resolution_ + Vec2d(field_image_.rows / 2, field_image_.cols / 2) - Vec2d(0.5, 0.5); // 图像坐标
|
||||
|
||||
if (pf[0] >= image_boarder_ && pf[0] < field_image_.cols - image_boarder_ && pf[1] >= image_boarder_ &&
|
||||
pf[1] < field_image_.rows - image_boarder_) {
|
||||
_error[0] = math::GetPixelValue<float>(field_image_, pf[0], pf[1]);
|
||||
} else {
|
||||
_error[0] = 0;
|
||||
setLevel(1);
|
||||
}
|
||||
}
|
||||
|
||||
void linearizeOplus() override {
|
||||
VertexSE2* v = (VertexSE2*)_vertices[0];
|
||||
SE2 pose = v->estimate();
|
||||
float theta = pose.so2().log();
|
||||
Vec2d pw = pose * Vec2d(range_ * std::cos(angle_), range_ * std::sin(angle_));
|
||||
Vec2d pf =
|
||||
pw * resolution_ + Vec2d(field_image_.rows / 2, field_image_.cols / 2) - Vec2d(0.5, 0.5); // 图像坐标
|
||||
|
||||
if (pf[0] >= image_boarder_ && pf[0] < field_image_.cols - image_boarder_ && pf[1] >= image_boarder_ &&
|
||||
pf[1] < field_image_.rows - image_boarder_) {
|
||||
// 图像梯度
|
||||
float dx = 0.5 * (math::GetPixelValue<float>(field_image_, pf[0] + 1, pf[1]) -
|
||||
math::GetPixelValue<float>(field_image_, pf[0] - 1, pf[1]));
|
||||
float dy = 0.5 * (math::GetPixelValue<float>(field_image_, pf[0], pf[1] + 1) -
|
||||
math::GetPixelValue<float>(field_image_, pf[0], pf[1] - 1));
|
||||
|
||||
_jacobianOplusXi << resolution_ * dx, resolution_ * dy,
|
||||
-resolution_ * dx * range_ * std::sin(angle_ + theta) +
|
||||
resolution_ * dy * range_ * std::cos(angle_ + theta);
|
||||
} else {
|
||||
_jacobianOplusXi.setZero();
|
||||
setLevel(1);
|
||||
}
|
||||
}
|
||||
|
||||
bool read(std::istream& is) override { return true; }
|
||||
bool write(std::ostream& os) const override { return true; }
|
||||
|
||||
private:
|
||||
const cv::Mat& field_image_;
|
||||
double range_ = 0;
|
||||
double angle_ = 0;
|
||||
float resolution_ = 10.0;
|
||||
inline static const int image_boarder_ = 10;
|
||||
};
|
||||
|
||||
/**
|
||||
* SE2 pose graph使用
|
||||
* error = v1.inv * v2 * meas.inv
|
||||
*/
|
||||
class EdgeSE2 : public g2o::BaseBinaryEdge<3, SE2, VertexSE2, VertexSE2> {
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
|
||||
EdgeSE2() {}
|
||||
|
||||
void computeError() override {
|
||||
VertexSE2* v1 = (VertexSE2*)_vertices[0];
|
||||
VertexSE2* v2 = (VertexSE2*)_vertices[1];
|
||||
_error = (v1->estimate().inverse() * v2->estimate() * measurement().inverse()).log();
|
||||
}
|
||||
|
||||
// TODO jacobian
|
||||
|
||||
bool read(std::istream& is) override { return true; }
|
||||
bool write(std::ostream& os) const override { return true; }
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_G2O_TYPES_H
|
||||
204
workspace/slam_in_autonomous_driving/src/ch6/icp_2d.cc
Normal file
204
workspace/slam_in_autonomous_driving/src/ch6/icp_2d.cc
Normal file
@@ -0,0 +1,204 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/15.
|
||||
//
|
||||
|
||||
#include "ch6/icp_2d.h"
|
||||
#include "common/math_utils.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <pcl/kdtree/kdtree_flann.h>
|
||||
#include <pcl/search/impl/kdtree.hpp>
|
||||
|
||||
namespace sad {
|
||||
|
||||
bool Icp2d::AlignGaussNewton(SE2& init_pose) {
|
||||
int iterations = 10;
|
||||
double cost = 0, lastCost = 0;
|
||||
SE2 current_pose = init_pose;
|
||||
const float max_dis2 = 0.01; // 最近邻时的最远距离（平方）
|
||||
const int min_effect_pts = 20; // 最小有效点数
|
||||
|
||||
for (int iter = 0; iter < iterations; ++iter) {
|
||||
Mat3d H = Mat3d::Zero();
|
||||
Vec3d b = Vec3d::Zero();
|
||||
cost = 0;
|
||||
|
||||
int effective_num = 0; // 有效点数
|
||||
|
||||
// 遍历source
|
||||
for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
|
||||
float r = source_scan_->ranges[i];
|
||||
if (r < source_scan_->range_min || r > source_scan_->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
|
||||
float theta = current_pose.so2().log();
|
||||
Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
|
||||
Point2d pt;
|
||||
pt.x = pw.x();
|
||||
pt.y = pw.y();
|
||||
|
||||
// 最近邻
|
||||
std::vector<int> nn_idx;
|
||||
std::vector<float> dis;
|
||||
kdtree_.nearestKSearch(pt, 1, nn_idx, dis);
|
||||
|
||||
if (nn_idx.size() > 0 && dis[0] < max_dis2) {
|
||||
effective_num++;
|
||||
Mat32d J;
|
||||
J << 1, 0, 0, 1, -r * std::sin(angle + theta), r * std::cos(angle + theta);
|
||||
H += J * J.transpose();
|
||||
|
||||
Vec2d e(pt.x - target_cloud_->points[nn_idx[0]].x, pt.y - target_cloud_->points[nn_idx[0]].y);
|
||||
b += -J * e;
|
||||
|
||||
cost += e.dot(e);
|
||||
}
|
||||
}
|
||||
|
||||
if (effective_num < min_effect_pts) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// solve for dx
|
||||
Vec3d dx = H.ldlt().solve(b);
|
||||
if (isnan(dx[0])) {
|
||||
break;
|
||||
}
|
||||
|
||||
cost /= effective_num;
|
||||
if (iter > 0 && cost >= lastCost) {
|
||||
break;
|
||||
}
|
||||
|
||||
LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;
|
||||
|
||||
current_pose.translation() += dx.head<2>();
|
||||
current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
|
||||
lastCost = cost;
|
||||
}
|
||||
|
||||
init_pose = current_pose;
|
||||
LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
|
||||
<< ", theta: " << current_pose.so2().log();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Icp2d::AlignGaussNewtonPoint2Plane(SE2& init_pose) {
|
||||
int iterations = 10;
|
||||
double cost = 0, lastCost = 0;
|
||||
SE2 current_pose = init_pose;
|
||||
const float max_dis = 0.3; // 最近邻时的最远距离
|
||||
const int min_effect_pts = 20; // 最小有效点数
|
||||
|
||||
for (int iter = 0; iter < iterations; ++iter) {
|
||||
Mat3d H = Mat3d::Zero();
|
||||
Vec3d b = Vec3d::Zero();
|
||||
cost = 0;
|
||||
|
||||
int effective_num = 0; // 有效点数
|
||||
|
||||
// 遍历source
|
||||
for (size_t i = 0; i < source_scan_->ranges.size(); ++i) {
|
||||
float r = source_scan_->ranges[i];
|
||||
if (r < source_scan_->range_min || r > source_scan_->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float angle = source_scan_->angle_min + i * source_scan_->angle_increment;
|
||||
float theta = current_pose.so2().log();
|
||||
Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
|
||||
Point2d pt;
|
||||
pt.x = pw.x();
|
||||
pt.y = pw.y();
|
||||
|
||||
// 查找5个最近邻
|
||||
std::vector<int> nn_idx;
|
||||
std::vector<float> dis;
|
||||
kdtree_.nearestKSearch(pt, 5, nn_idx, dis);
|
||||
|
||||
std::vector<Vec2d> effective_pts; // 有效点
|
||||
for (int j = 0; j < nn_idx.size(); ++j) {
|
||||
if (dis[j] < max_dis) {
|
||||
effective_pts.emplace_back(
|
||||
Vec2d(target_cloud_->points[nn_idx[j]].x, target_cloud_->points[nn_idx[j]].y));
|
||||
}
|
||||
}
|
||||
|
||||
if (effective_pts.size() < 3) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// 拟合直线，组装J、H和误差
|
||||
Vec3d line_coeffs;
|
||||
if (math::FitLine2D(effective_pts, line_coeffs)) {
|
||||
effective_num++;
|
||||
Vec3d J;
|
||||
J << line_coeffs[0], line_coeffs[1],
|
||||
-line_coeffs[0] * r * std::sin(angle + theta) + line_coeffs[1] * r * std::cos(angle + theta);
|
||||
H += J * J.transpose();
|
||||
|
||||
double e = line_coeffs[0] * pw[0] + line_coeffs[1] * pw[1] + line_coeffs[2];
|
||||
b += -J * e;
|
||||
|
||||
cost += e * e;
|
||||
}
|
||||
}
|
||||
|
||||
if (effective_num < min_effect_pts) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// solve for dx
|
||||
Vec3d dx = H.ldlt().solve(b);
|
||||
if (isnan(dx[0])) {
|
||||
break;
|
||||
}
|
||||
|
||||
cost /= effective_num;
|
||||
if (iter > 0 && cost >= lastCost) {
|
||||
break;
|
||||
}
|
||||
|
||||
LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;
|
||||
|
||||
current_pose.translation() += dx.head<2>();
|
||||
current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
|
||||
lastCost = cost;
|
||||
}
|
||||
|
||||
init_pose = current_pose;
|
||||
LOG(INFO) << "estimated pose: " << current_pose.translation().transpose()
|
||||
<< ", theta: " << current_pose.so2().log();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Icp2d::BuildTargetKdTree() {
|
||||
if (target_scan_ == nullptr) {
|
||||
LOG(ERROR) << "target is not set";
|
||||
return;
|
||||
}
|
||||
|
||||
target_cloud_.reset(new Cloud2d);
|
||||
for (size_t i = 0; i < target_scan_->ranges.size(); ++i) {
|
||||
if (target_scan_->ranges[i] < target_scan_->range_min || target_scan_->ranges[i] > target_scan_->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
double real_angle = target_scan_->angle_min + i * target_scan_->angle_increment;
|
||||
|
||||
Point2d p;
|
||||
p.x = target_scan_->ranges[i] * std::cos(real_angle);
|
||||
p.y = target_scan_->ranges[i] * std::sin(real_angle);
|
||||
target_cloud_->points.push_back(p);
|
||||
}
|
||||
|
||||
target_cloud_->width = target_cloud_->points.size();
|
||||
target_cloud_->is_dense = false;
|
||||
kdtree_.setInputCloud(target_cloud_);
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
53
workspace/slam_in_autonomous_driving/src/ch6/icp_2d.h
Normal file
53
workspace/slam_in_autonomous_driving/src/ch6/icp_2d.h
Normal file
@@ -0,0 +1,53 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/15.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_ICP_2D_H
|
||||
#define SLAM_IN_AUTO_DRIVING_ICP_2D_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/lidar_utils.h"
|
||||
|
||||
#include <pcl/search/kdtree.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 第六章谈到的各种类型的ICP代码实现
|
||||
* 用法：先SetTarget, 此时构建target点云的KD树；再SetSource，然后调用Align*方法
|
||||
*/
|
||||
class Icp2d {
|
||||
public:
|
||||
using Point2d = pcl::PointXY;
|
||||
using Cloud2d = pcl::PointCloud<Point2d>;
|
||||
Icp2d() {}
|
||||
|
||||
/// 设置目标的Scan
|
||||
void SetTarget(Scan2d::Ptr target) {
|
||||
target_scan_ = target;
|
||||
BuildTargetKdTree();
|
||||
}
|
||||
|
||||
/// 设置被配准的Scan
|
||||
void SetSource(Scan2d::Ptr source) { source_scan_ = source; }
|
||||
|
||||
/// 使用高斯牛顿法进行配准
|
||||
bool AlignGaussNewton(SE2& init_pose);
|
||||
|
||||
/// 使用高斯牛顿法进行配准, Point-to-Plane
|
||||
bool AlignGaussNewtonPoint2Plane(SE2& init_pose);
|
||||
|
||||
private:
|
||||
// 建立目标点云的Kdtree
|
||||
void BuildTargetKdTree();
|
||||
|
||||
pcl::search::KdTree<Point2d> kdtree_;
|
||||
Cloud2d::Ptr target_cloud_; // PCL 形式的target cloud
|
||||
|
||||
Scan2d::Ptr target_scan_ = nullptr;
|
||||
Scan2d::Ptr source_scan_ = nullptr;
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_ICP_2D_H
|
||||
@@ -0,0 +1,44 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/15.
|
||||
//
|
||||
|
||||
#include "ch6/lidar_2d_utils.h"
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
namespace sad {
|
||||
|
||||
void Visualize2DScan(Scan2d::Ptr scan, const SE2& pose, cv::Mat& image, const Vec3b& color, int image_size,
|
||||
float resolution, const SE2& pose_submap) {
|
||||
if (image.data == nullptr) {
|
||||
image = cv::Mat(image_size, image_size, CV_8UC3, cv::Vec3b(255, 255, 255));
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < scan->ranges.size(); ++i) {
|
||||
if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
double real_angle = scan->angle_min + i * scan->angle_increment;
|
||||
double x = scan->ranges[i] * std::cos(real_angle);
|
||||
double y = scan->ranges[i] * std::sin(real_angle);
|
||||
|
||||
if (real_angle < scan->angle_min + 30 * M_PI / 180.0 || real_angle > scan->angle_max - 30 * M_PI / 180.0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Vec2d psubmap = pose_submap.inverse() * (pose * Vec2d(x, y));
|
||||
|
||||
int image_x = int(psubmap[0] * resolution + image_size / 2);
|
||||
int image_y = int(psubmap[1] * resolution + image_size / 2);
|
||||
if (image_x >= 0 && image_x < image.cols && image_y >= 0 && image_y < image.rows) {
|
||||
image.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(color[0], color[1], color[2]);
|
||||
}
|
||||
}
|
||||
|
||||
// 同时画出pose自身所在位置
|
||||
Vec2d pose_in_image =
|
||||
pose_submap.inverse() * (pose.translation()) * double(resolution) + Vec2d(image_size / 2, image_size / 2);
|
||||
cv::circle(image, cv::Point2f(pose_in_image[0], pose_in_image[1]), 5, cv::Scalar(color[0], color[1], color[2]), 2);
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
@@ -0,0 +1,30 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/15.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_LIDAR_2D_UTILS_H
|
||||
#define SLAM_IN_AUTO_DRIVING_LIDAR_2D_UTILS_H
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/lidar_utils.h"
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
/// 为2D lidar的一些辅助函数
|
||||
namespace sad {
|
||||
|
||||
/**
|
||||
* 在image上绘制一个2D scan
|
||||
* @param scan
|
||||
* @param pose
|
||||
* @param image
|
||||
* @param image_size 图片大小
|
||||
* @param resolution 分辨率，一米多少个像素
|
||||
* @param pose_submap 如果是子地图，提供子地图的pose
|
||||
*/
|
||||
void Visualize2DScan(Scan2d::Ptr scan, const SE2& pose, cv::Mat& image, const Vec3b& color, int image_size = 800,
|
||||
float resolution = 20.0, const SE2& pose_submap = SE2());
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_LIDAR_2D_UTILS_H
|
||||
230
workspace/slam_in_autonomous_driving/src/ch6/likelihood_field.cc
Normal file
230
workspace/slam_in_autonomous_driving/src/ch6/likelihood_field.cc
Normal file
@@ -0,0 +1,230 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/18.
|
||||
//
|
||||
|
||||
#include "ch6/g2o_types.h"
|
||||
#include "ch6/likelihood_filed.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include <g2o/core/base_unary_edge.h>
|
||||
#include <g2o/core/block_solver.h>
|
||||
#include <g2o/core/optimization_algorithm_levenberg.h>
|
||||
#include <g2o/core/robust_kernel.h>
|
||||
#include <g2o/core/robust_kernel_impl.h>
|
||||
#include <g2o/core/sparse_optimizer.h>
|
||||
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
|
||||
#include <g2o/solvers/dense/linear_solver_dense.h>
|
||||
|
||||
namespace sad {
|
||||
|
||||
void LikelihoodField::SetTargetScan(Scan2d::Ptr scan) {
|
||||
target_ = scan;
|
||||
|
||||
// 在target点上生成场函数
|
||||
field_ = cv::Mat(1000, 1000, CV_32F, 30.0);
|
||||
|
||||
for (size_t i = 0; i < scan->ranges.size(); ++i) {
|
||||
if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
double real_angle = scan->angle_min + i * scan->angle_increment;
|
||||
double x = scan->ranges[i] * std::cos(real_angle) * resolution_ + 500;
|
||||
double y = scan->ranges[i] * std::sin(real_angle) * resolution_ + 500;
|
||||
|
||||
// 在(x,y)附近填入场函数
|
||||
for (auto& model_pt : model_) {
|
||||
int xx = int(x + model_pt.dx_);
|
||||
int yy = int(y + model_pt.dy_);
|
||||
if (xx >= 0 && xx < field_.cols && yy >= 0 && yy < field_.rows &&
|
||||
field_.at<float>(yy, xx) > model_pt.residual_) {
|
||||
field_.at<float>(yy, xx) = model_pt.residual_;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LikelihoodField::BuildModel() {
|
||||
const int range = 20; // 生成多少个像素的模板
|
||||
for (int x = -range; x <= range; ++x) {
|
||||
for (int y = -range; y <= range; ++y) {
|
||||
model_.emplace_back(x, y, std::sqrt((x * x) + (y * y)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LikelihoodField::SetSourceScan(Scan2d::Ptr scan) { source_ = scan; }
|
||||
|
||||
bool LikelihoodField::AlignGaussNewton(SE2& init_pose) {
|
||||
int iterations = 10;
|
||||
double cost = 0, lastCost = 0;
|
||||
SE2 current_pose = init_pose;
|
||||
const int min_effect_pts = 20; // 最小有效点数
|
||||
const int image_boarder = 20; // 预留图像边界
|
||||
|
||||
has_outside_pts_ = false;
|
||||
for (int iter = 0; iter < iterations; ++iter) {
|
||||
Mat3d H = Mat3d::Zero();
|
||||
Vec3d b = Vec3d::Zero();
|
||||
cost = 0;
|
||||
|
||||
int effective_num = 0; // 有效点数
|
||||
|
||||
// 遍历source
|
||||
for (size_t i = 0; i < source_->ranges.size(); ++i) {
|
||||
float r = source_->ranges[i];
|
||||
if (r < source_->range_min || r > source_->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float angle = source_->angle_min + i * source_->angle_increment;
|
||||
if (angle < source_->angle_min + 30 * M_PI / 180.0 || angle > source_->angle_max - 30 * M_PI / 180.0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float theta = current_pose.so2().log();
|
||||
Vec2d pw = current_pose * Vec2d(r * std::cos(angle), r * std::sin(angle));
|
||||
|
||||
// 在field中的图像坐标
|
||||
Vec2i pf = (pw * resolution_ + Vec2d(500, 500)).cast<int>();
|
||||
|
||||
if (pf[0] >= image_boarder && pf[0] < field_.cols - image_boarder && pf[1] >= image_boarder &&
|
||||
pf[1] < field_.rows - image_boarder) {
|
||||
effective_num++;
|
||||
|
||||
// 图像梯度
|
||||
float dx = 0.5 * (field_.at<float>(pf[1], pf[0] + 1) - field_.at<float>(pf[1], pf[0] - 1));
|
||||
float dy = 0.5 * (field_.at<float>(pf[1] + 1, pf[0]) - field_.at<float>(pf[1] - 1, pf[0]));
|
||||
|
||||
Vec3d J;
|
||||
J << resolution_ * dx, resolution_ * dy,
|
||||
-resolution_ * dx * r * std::sin(angle + theta) + resolution_ * dy * r * std::cos(angle + theta);
|
||||
H += J * J.transpose();
|
||||
|
||||
float e = field_.at<float>(pf[1], pf[0]);
|
||||
b += -J * e;
|
||||
|
||||
cost += e * e;
|
||||
} else {
|
||||
has_outside_pts_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (effective_num < min_effect_pts) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// solve for dx
|
||||
Vec3d dx = H.ldlt().solve(b);
|
||||
if (isnan(dx[0])) {
|
||||
break;
|
||||
}
|
||||
|
||||
cost /= effective_num;
|
||||
if (iter > 0 && cost >= lastCost) {
|
||||
break;
|
||||
}
|
||||
|
||||
LOG(INFO) << "iter " << iter << " cost = " << cost << ", effect num: " << effective_num;
|
||||
|
||||
current_pose.translation() += dx.head<2>();
|
||||
current_pose.so2() = current_pose.so2() * SO2::exp(dx[2]);
|
||||
lastCost = cost;
|
||||
}
|
||||
|
||||
init_pose = current_pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
cv::Mat LikelihoodField::GetFieldImage() {
|
||||
cv::Mat image(field_.rows, field_.cols, CV_8UC3);
|
||||
for (int x = 0; x < field_.cols; ++x) {
|
||||
for (int y = 0; y < field_.rows; ++y) {
|
||||
float r = field_.at<float>(y, x) * 255.0 / 30.0;
|
||||
image.at<cv::Vec3b>(y, x) = cv::Vec3b(uchar(r), uchar(r), uchar(r));
|
||||
}
|
||||
}
|
||||
|
||||
return image;
|
||||
}
|
||||
|
||||
bool LikelihoodField::AlignG2O(SE2& init_pose) {
|
||||
using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
|
||||
using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
|
||||
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
|
||||
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
|
||||
g2o::SparseOptimizer optimizer;
|
||||
optimizer.setAlgorithm(solver);
|
||||
|
||||
auto* v = new VertexSE2();
|
||||
v->setId(0);
|
||||
v->setEstimate(init_pose);
|
||||
optimizer.addVertex(v);
|
||||
|
||||
const double range_th = 15.0; // 不考虑太远的scan，不准
|
||||
const double rk_delta = 0.8;
|
||||
|
||||
has_outside_pts_ = false;
|
||||
// 遍历source
|
||||
for (size_t i = 0; i < source_->ranges.size(); ++i) {
|
||||
float r = source_->ranges[i];
|
||||
if (r < source_->range_min || r > source_->range_max) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (r > range_th) {
|
||||
continue;
|
||||
}
|
||||
|
||||
float angle = source_->angle_min + i * source_->angle_increment;
|
||||
if (angle < source_->angle_min + 30 * M_PI / 180.0 || angle > source_->angle_max - 30 * M_PI / 180.0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto e = new EdgeSE2LikelihoodFiled(field_, r, angle, resolution_);
|
||||
e->setVertex(0, v);
|
||||
|
||||
if (e->IsOutSide()) {
|
||||
has_outside_pts_ = true;
|
||||
delete e;
|
||||
continue;
|
||||
}
|
||||
|
||||
e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
|
||||
auto rk = new g2o::RobustKernelHuber;
|
||||
rk->setDelta(rk_delta);
|
||||
e->setRobustKernel(rk);
|
||||
optimizer.addEdge(e);
|
||||
}
|
||||
|
||||
optimizer.setVerbose(false);
|
||||
optimizer.initializeOptimization();
|
||||
optimizer.optimize(10);
|
||||
|
||||
init_pose = v->estimate();
|
||||
return true;
|
||||
}
|
||||
|
||||
void LikelihoodField::SetFieldImageFromOccuMap(const cv::Mat& occu_map) {
|
||||
const int boarder = 25;
|
||||
field_ = cv::Mat(1000, 1000, CV_32F, 30.0);
|
||||
|
||||
for (int x = boarder; x < occu_map.cols - boarder; ++x) {
|
||||
for (int y = boarder; y < occu_map.rows - boarder; ++y) {
|
||||
if (occu_map.at<uchar>(y, x) < 127) {
|
||||
// 在该点生成一个model
|
||||
for (auto& model_pt : model_) {
|
||||
int xx = int(x + model_pt.dx_);
|
||||
int yy = int(y + model_pt.dy_);
|
||||
if (xx >= 0 && xx < field_.cols && yy >= 0 && yy < field_.rows &&
|
||||
field_.at<float>(yy, xx) > model_pt.residual_) {
|
||||
field_.at<float>(yy, xx) = model_pt.residual_;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
@@ -0,0 +1,70 @@
|
||||
//
|
||||
// Created by xiang on 2022/3/18.
|
||||
//
|
||||
|
||||
#ifndef SLAM_IN_AUTO_DRIVING_LIKELIHOOD_FILED_H
|
||||
#define SLAM_IN_AUTO_DRIVING_LIKELIHOOD_FILED_H
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
#include "common/eigen_types.h"
|
||||
#include "common/lidar_utils.h"
|
||||
|
||||
namespace sad {
|
||||
|
||||
class LikelihoodField {
|
||||
public:
|
||||
/// 2D 场的模板，在设置target scan或map的时候生成
|
||||
struct ModelPoint {
|
||||
ModelPoint(int dx, int dy, float res) : dx_(dx), dy_(dy), residual_(res) {}
|
||||
int dx_ = 0;
|
||||
int dy_ = 0;
|
||||
float residual_ = 0;
|
||||
};
|
||||
|
||||
LikelihoodField() { BuildModel(); }
|
||||
|
||||
/// 增加一个2D的目标scan
|
||||
void SetTargetScan(Scan2d::Ptr scan);
|
||||
|
||||
/// 设置被配准的那个scan
|
||||
void SetSourceScan(Scan2d::Ptr scan);
|
||||
|
||||
/// 从占据栅格地图生成一个似然场地图
|
||||
void SetFieldImageFromOccuMap(const cv::Mat& occu_map);
|
||||
|
||||
/// 使用高斯牛顿法配准
|
||||
bool AlignGaussNewton(SE2& init_pose);
|
||||
|
||||
/**
|
||||
* 使用g2o配准
|
||||
* @param init_pose 初始位姿 NOTE 使用submap时，给定相对于该submap的位姿，估计结果也是针对于这个submap的位姿
|
||||
* @return
|
||||
*/
|
||||
bool AlignG2O(SE2& init_pose);
|
||||
|
||||
/// 获取场函数，转换为RGB图像
|
||||
cv::Mat GetFieldImage();
|
||||
|
||||
bool HasOutsidePoints() const { return has_outside_pts_; }
|
||||
|
||||
void SetPose(const SE2& pose) { pose_ = pose; }
|
||||
|
||||
private:
|
||||
void BuildModel();
|
||||
|
||||
SE2 pose_; // T_W_S
|
||||
Scan2d::Ptr target_ = nullptr;
|
||||
Scan2d::Ptr source_ = nullptr;
|
||||
|
||||
std::vector<ModelPoint> model_; // 2D 模板
|
||||
cv::Mat field_; // 场函数
|
||||
bool has_outside_pts_ = false; // 是否含有出了这个场的点
|
||||
|
||||
// 参数配置
|
||||
inline static const float resolution_ = 20; // 每米多少个像素
|
||||
};
|
||||
|
||||
} // namespace sad
|
||||
|
||||
#endif // SLAM_IN_AUTO_DRIVING_LIKELIHOOD_FILED_H
|
||||
223
workspace/slam_in_autonomous_driving/src/ch6/loop_closing.cc
Normal file
223
workspace/slam_in_autonomous_driving/src/ch6/loop_closing.cc
Normal file
@@ -0,0 +1,223 @@
|
||||
//
|
||||
// Created by xiang on 2022/4/7.
|
||||
//
|
||||
|
||||
#include "loop_closing.h"
|
||||
#include "ch6/g2o_types.h"
|
||||
#include "lidar_2d_utils.h"
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
|
||||
#include <g2o/core/base_unary_edge.h>
|
||||
#include <g2o/core/block_solver.h>
|
||||
#include <g2o/core/optimization_algorithm_levenberg.h>
|
||||
#include <g2o/core/robust_kernel.h>
|
||||
#include <g2o/core/robust_kernel_impl.h>
|
||||
#include <g2o/core/sparse_optimizer.h>
|
||||
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
namespace sad {
|
||||
|
||||
void LoopClosing::AddFinishedSubmap(std::shared_ptr<Submap> submap) {
|
||||
auto mr_field = std::make_shared<MRLikelihoodField>();
|
||||
mr_field->SetPose(submap->GetPose());
|
||||
mr_field->SetFieldImageFromOccuMap(submap->GetOccuMap().GetOccupancyGrid());
|
||||
submap_to_field_.emplace(submap, mr_field);
|
||||
}
|
||||
|
||||
void LoopClosing::AddNewSubmap(std::shared_ptr<Submap> submap) {
|
||||
submaps_.emplace(submap->GetId(), submap);
|
||||
last_submap_id_ = submap->GetId();
|
||||
}
|
||||
|
||||
void LoopClosing::AddNewFrame(std::shared_ptr<Frame> frame) {
|
||||
current_frame_ = frame;
|
||||
if (!DetectLoopCandidates()) {
|
||||
return;
|
||||
}
|
||||
|
||||
MatchInHistorySubmaps();
|
||||
|
||||
if (has_new_loops_) {
|
||||
Optimize();
|
||||
}
|
||||
}
|
||||
|
||||
bool LoopClosing::DetectLoopCandidates() {
|
||||
// 要求当前帧与历史submap有一定间隔
|
||||
has_new_loops_ = false;
|
||||
if (last_submap_id_ < submap_gap_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
current_candidates_.clear();
|
||||
|
||||
for (auto& sp : submaps_) {
|
||||
if ((last_submap_id_ - sp.first) <= submap_gap_) {
|
||||
// 不检查最近的几个submap
|
||||
continue;
|
||||
}
|
||||
|
||||
// 如果这个submap和历史submap已经存在有效的关联，也忽略之
|
||||
auto hist_iter = loop_constraints_.find(std::pair<size_t, size_t>(sp.first, last_submap_id_));
|
||||
if (hist_iter != loop_constraints_.end() && hist_iter->second.valid_) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Vec2d center = sp.second->GetPose().translation();
|
||||
Vec2d frame_pos = current_frame_->pose_.translation();
|
||||
double dis = (center - frame_pos).norm();
|
||||
if (dis < candidate_distance_th_) {
|
||||
/// 如果这个frame离submap中心差距小于阈值，则检查
|
||||
LOG(INFO) << "taking " << current_frame_->keyframe_id_ << " with " << sp.first
|
||||
<< ", last submap id: " << last_submap_id_;
|
||||
current_candidates_.emplace_back(sp.first);
|
||||
}
|
||||
}
|
||||
|
||||
return !current_candidates_.empty();
|
||||
}
|
||||
|
||||
void LoopClosing::MatchInHistorySubmaps() {
|
||||
// 我们先把要检查的scan, pose和submap存到离线文件, 把mr match调完了再实际上线
|
||||
// current_frame_->Dump("./data/ch6/frame_" + std::to_string(current_frame_->id_) + ".txt");
|
||||
|
||||
for (const size_t& can : current_candidates_) {
|
||||
auto mr = submap_to_field_.at(submaps_[can]);
|
||||
mr->SetSourceScan(current_frame_->scan_);
|
||||
|
||||
auto submap = submaps_[can];
|
||||
SE2 pose_in_target_submap = submap->GetPose().inverse() * current_frame_->pose_; // T_S1_C
|
||||
|
||||
if (mr->AlignG2O(pose_in_target_submap)) {
|
||||
// set constraints from current submap to target submap
|
||||
// T_S1_S2 = T_S1_C * T_C_W * T_W_S2
|
||||
SE2 T_this_cur =
|
||||
pose_in_target_submap * current_frame_->pose_.inverse() * submaps_[last_submap_id_]->GetPose();
|
||||
loop_constraints_.emplace(std::pair<size_t, size_t>(can, last_submap_id_),
|
||||
LoopConstraints(can, last_submap_id_, T_this_cur));
|
||||
LOG(INFO) << "adding loop from submap " << can << " to " << last_submap_id_;
|
||||
|
||||
/// 可视化显示
|
||||
auto occu_image = submap->GetOccuMap().GetOccupancyGridBlackWhite();
|
||||
Visualize2DScan(current_frame_->scan_, pose_in_target_submap, occu_image, Vec3b(0, 0, 255), 1000, 20.0,
|
||||
SE2());
|
||||
cv::putText(occu_image, "loop submap " + std::to_string(submap->GetId()), cv::Point2f(20, 20),
|
||||
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
|
||||
cv::putText(occu_image, "keyframes " + std::to_string(submap->NumFrames()), cv::Point2f(20, 50),
|
||||
cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
|
||||
cv::imshow("loop closure", occu_image);
|
||||
|
||||
has_new_loops_ = true;
|
||||
}
|
||||
|
||||
debug_fout_ << current_frame_->id_ << " " << can << " " << submaps_[can]->GetPose().translation().x() << " "
|
||||
<< submaps_[can]->GetPose().translation().y() << " " << submaps_[can]->GetPose().so2().log()
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
current_candidates_.clear();
|
||||
}
|
||||
|
||||
void LoopClosing::Optimize() {
|
||||
// pose graph optimization
|
||||
using BlockSolverType = g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>>;
|
||||
using LinearSolverType = g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType>;
|
||||
auto* solver = new g2o::OptimizationAlgorithmLevenberg(
|
||||
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
|
||||
g2o::SparseOptimizer optimizer;
|
||||
optimizer.setAlgorithm(solver);
|
||||
|
||||
for (auto& sp : submaps_) {
|
||||
auto* v = new VertexSE2();
|
||||
v->setId(sp.first);
|
||||
v->setEstimate(sp.second->GetPose());
|
||||
optimizer.addVertex(v);
|
||||
}
|
||||
|
||||
/// 连续约束
|
||||
for (int i = 0; i < last_submap_id_; ++i) {
|
||||
auto first_submap = submaps_[i];
|
||||
auto next_submap = submaps_[i + 1];
|
||||
|
||||
EdgeSE2* e = new EdgeSE2();
|
||||
e->setVertex(0, optimizer.vertex(i));
|
||||
e->setVertex(1, optimizer.vertex(i + 1));
|
||||
e->setMeasurement(first_submap->GetPose().inverse() * next_submap->GetPose());
|
||||
e->setInformation(Mat3d::Identity() * 1e4);
|
||||
|
||||
optimizer.addEdge(e);
|
||||
}
|
||||
|
||||
/// 回环约束
|
||||
std::map<std::pair<size_t, size_t>, EdgeSE2*> loop_edges;
|
||||
for (auto& c : loop_constraints_) {
|
||||
if (!c.second.valid_) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto first_submap = submaps_[c.first.first];
|
||||
auto second_submap = submaps_[c.first.second];
|
||||
|
||||
EdgeSE2* e = new EdgeSE2();
|
||||
e->setVertex(0, optimizer.vertex(first_submap->GetId()));
|
||||
e->setVertex(1, optimizer.vertex(second_submap->GetId()));
|
||||
e->setMeasurement(c.second.T12_);
|
||||
e->setInformation(Mat3d::Identity());
|
||||
|
||||
auto rk = new g2o::RobustKernelCauchy;
|
||||
rk->setDelta(loop_rk_delta_);
|
||||
e->setRobustKernel(rk);
|
||||
|
||||
optimizer.addEdge(e);
|
||||
loop_edges.emplace(c.first, e);
|
||||
}
|
||||
|
||||
optimizer.setVerbose(true);
|
||||
optimizer.initializeOptimization();
|
||||
optimizer.optimize(10);
|
||||
|
||||
// validate the loop constraints
|
||||
int inliers = 0;
|
||||
for (auto& ep : loop_edges) {
|
||||
if (ep.second->chi2() < loop_rk_delta_) {
|
||||
LOG(INFO) << "loop from " << ep.first.first << " to " << ep.first.second
|
||||
<< " is correct, chi2: " << ep.second->chi2();
|
||||
ep.second->setRobustKernel(nullptr);
|
||||
loop_constraints_.at(ep.first).valid_ = true;
|
||||
inliers++;
|
||||
} else {
|
||||
ep.second->setLevel(1);
|
||||
LOG(INFO) << "loop from " << ep.first.first << " to " << ep.first.second
|
||||
<< " is invalid, chi2: " << ep.second->chi2();
|
||||
loop_constraints_.at(ep.first).valid_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
optimizer.optimize(5);
|
||||
|
||||
for (auto& sp : submaps_) {
|
||||
VertexSE2* v = (VertexSE2*)optimizer.vertex(sp.first);
|
||||
sp.second->SetPose(v->estimate());
|
||||
|
||||
// 更新所属的frame world pose
|
||||
sp.second->UpdateFramePoseWorld();
|
||||
}
|
||||
|
||||
LOG(INFO) << "loop inliers: " << inliers << "/" << loop_constraints_.size();
|
||||
|
||||
// 移除错误的匹配
|
||||
for (auto iter = loop_constraints_.begin(); iter != loop_constraints_.end();) {
|
||||
if (!iter->second.valid_) {
|
||||
iter = loop_constraints_.erase(iter);
|
||||
} else {
|
||||
iter++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sad
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user