Showing preview only (1,265K chars total). Download the full file or copy to clipboard to get everything.
Repository: lovelyyoshino/FAST_LIO2_Noted
Branch: main
Commit: 0a6e863a9824
Files: 99
Total size: 1.2 MB
Directory structure:
gitextract_30l0tz1c/
├── .gitignore
├── FAST-LIO/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── Log/
│ │ ├── fast_lio_time_log_analysis.m
│ │ ├── guide.md
│ │ └── plot.py
│ ├── PCD/
│ │ └── 1
│ ├── README.md
│ ├── config/
│ │ ├── avia.yaml
│ │ ├── horizon.yaml
│ │ ├── ouster64.yaml
│ │ ├── ouster64_mulran.yaml
│ │ └── velodyne.yaml
│ ├── include/
│ │ ├── Exp_mat.h
│ │ ├── IKFoM_toolkit/
│ │ │ ├── esekfom/
│ │ │ │ ├── esekfom.hpp
│ │ │ │ └── util.hpp
│ │ │ └── mtk/
│ │ │ ├── build_manifold.hpp
│ │ │ ├── src/
│ │ │ │ ├── SubManifold.hpp
│ │ │ │ ├── mtkmath.hpp
│ │ │ │ └── vectview.hpp
│ │ │ ├── startIdx.hpp
│ │ │ └── types/
│ │ │ ├── S2.hpp
│ │ │ ├── SOn.hpp
│ │ │ ├── vect.hpp
│ │ │ └── wrapped_cv_mat.hpp
│ │ ├── common_lib.h
│ │ ├── ikd-Tree/
│ │ │ ├── README.md
│ │ │ ├── ikd_Tree.cpp
│ │ │ └── ikd_Tree.h
│ │ ├── matplotlibcpp.h
│ │ ├── so3_math.h
│ │ └── use-ikfom.hpp
│ ├── launch/
│ │ ├── gdb_debug_example.launch
│ │ ├── mapping_avia.launch
│ │ ├── mapping_horizon.launch
│ │ ├── mapping_ouster64.launch
│ │ ├── mapping_ouster64_mulran.launch
│ │ └── mapping_velodyne.launch
│ ├── msg/
│ │ └── Pose6D.msg
│ ├── package.xml
│ ├── rviz_cfg/
│ │ ├── .gitignore
│ │ └── loam_livox.rviz
│ └── src/
│ ├── IMU_Processing.hpp
│ ├── laserMapping.cpp
│ ├── preprocess.cpp
│ └── preprocess.h
├── README.md
└── SC-PGO/
├── CMakeLists.txt
├── README.md
├── docker/
│ ├── Dockerfile
│ ├── Makefile
│ └── run.sh
├── include/
│ ├── aloam_velodyne/
│ │ ├── common.h
│ │ └── tic_toc.h
│ └── scancontext/
│ ├── KDTreeVectorOfVectorsAdaptor.h
│ ├── Scancontext.cpp
│ ├── Scancontext.h
│ └── nanoflann.hpp
├── launch/
│ ├── aloam_mulran.launch
│ ├── aloam_velodyne_HDL_32.launch
│ ├── aloam_velodyne_HDL_64.launch
│ ├── aloam_velodyne_VLP_16.launch
│ ├── fastlio_ouster64.launch
│ └── kitti_helper.launch
├── package.xml
├── rviz_cfg/
│ └── aloam_velodyne.rviz
├── src/
│ ├── kittiHelper.cpp
│ ├── laserMapping.cpp
│ ├── laserOdometry.cpp
│ ├── laserPosegraphOptimization.cpp
│ ├── lidarFactor.hpp
│ └── scanRegistration.cpp
└── utils/
├── python/
│ ├── bone_table.npy
│ ├── jet_table.npy
│ ├── makeMergedMap.py
│ └── pypcdMyUtils.py
└── sample_data/
└── KAIST03/
├── Scans/
│ ├── 000000.pcd
│ ├── 000001.pcd
│ ├── 000002.pcd
│ ├── 000003.pcd
│ ├── 000004.pcd
│ ├── 000005.pcd
│ ├── 000006.pcd
│ ├── 000007.pcd
│ ├── 000008.pcd
│ ├── 000009.pcd
│ ├── 000010.pcd
│ ├── 000011.pcd
│ ├── 000012.pcd
│ ├── 000013.pcd
│ ├── 000014.pcd
│ ├── 000015.pcd
│ ├── 000016.pcd
│ ├── 000017.pcd
│ ├── 000018.pcd
│ ├── 000019.pcd
│ └── 000020.pcd
├── optimized_poses.txt
└── times.txt
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
build
FAST-LIO/Log/*.png
FAST-LIO/Log/*.txt
FAST-LIO/Log/*.csv
FAST-LIO/Log/*.pdf
.vscode/c_cpp_properties.json
.vscode/settings.json
PCD/*.pcd
*.pyc
================================================
FILE: FAST-LIO/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(fast_lio)
SET(CMAKE_BUILD_TYPE "Debug")
ADD_COMPILE_OPTIONS(-std=c++14 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set( CMAKE_CXX_FLAGS "-std=c++14 -O3" )
add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" )
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions")
message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}")
if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )
include(ProcessorCount)
ProcessorCount(N)
message("Processer number: ${N}")
if(N GREATER 4)
add_definitions(-DMP_EN)
add_definitions(-DMP_PROC_NUM=3)
message("core for MP: 3")
elseif(N GREATER 3)
add_definitions(-DMP_EN)
add_definitions(-DMP_PROC_NUM=2)
message("core for MP: 2")
else()
add_definitions(-DMP_PROC_NUM=1)
endif()
else()
add_definitions(-DMP_PROC_NUM=1)
endif()
find_package(OpenMP QUIET)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
find_package(PythonLibs REQUIRED)
find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
pcl_ros
tf
livox_ros_driver
message_generation
eigen_conversions
)
find_package(Eigen3 REQUIRED)
find_package(PCL 1.8 REQUIRED)
message(Eigen: ${EIGEN3_INCLUDE_DIR})
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${PYTHON_INCLUDE_DIRS}
include)
add_message_files(
FILES
Pose6D.msg
)
generate_messages(
DEPENDENCIES
geometry_msgs
)
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
DEPENDS EIGEN3 PCL
INCLUDE_DIRS
)
add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
================================================
FILE: FAST-LIO/LICENSE
================================================
GNU GENERAL PUBLIC LICENSE
Version 2, June 1991
Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
License is intended to guarantee your freedom to share and change free
software--to make sure the software is free for all its users. This
General Public License applies to most of the Free Software
Foundation's software and to any other program whose authors commit to
using it. (Some other Free Software Foundation software is covered by
the GNU Lesser General Public License instead.) You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
this service if you wish), that you receive source code or can get it
if you want it, that you can change the software or use pieces of it
in new free programs; and that you know you can do these things.
To protect your rights, we need to make restrictions that forbid
anyone to deny you these rights or to ask you to surrender the rights.
These restrictions translate to certain responsibilities for you if you
distribute copies of the software, or if you modify it.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must give the recipients all the rights that
you have. You must make sure that they, too, receive or can get the
source code. And you must show them these terms so they know their
rights.
We protect your rights with two steps: (1) copyright the software, and
(2) offer you this license which gives you legal permission to copy,
distribute and/or modify the software.
Also, for each author's protection and ours, we want to make certain
that everyone understands that there is no warranty for this free
software. If the software is modified by someone else and passed on, we
want its recipients to know that what they have is not the original, so
that any problems introduced by others will not reflect on the original
authors' reputations.
Finally, any free program is threatened constantly by software
patents. We wish to avoid the danger that redistributors of a free
program will individually obtain patent licenses, in effect making the
program proprietary. To prevent this, we have made it clear that any
patent must be licensed for everyone's free use or not licensed at all.
The precise terms and conditions for copying, distribution and
modification follow.
GNU GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License applies to any program or other work which contains
a notice placed by the copyright holder saying it may be distributed
under the terms of this General Public License. The "Program", below,
refers to any such program or work, and a "work based on the Program"
means either the Program or any derivative work under copyright law:
that is to say, a work containing the Program or a portion of it,
either verbatim or with modifications and/or translated into another
language. (Hereinafter, translation is included without limitation in
the term "modification".) Each licensee is addressed as "you".
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running the Program is not restricted, and the output from the Program
is covered only if its contents constitute a work based on the
Program (independent of having been made by running the Program).
Whether that is true depends on what the Program does.
1. You may copy and distribute verbatim copies of the Program's
source code as you receive it, in any medium, provided that you
conspicuously and appropriately publish on each copy an appropriate
copyright notice and disclaimer of warranty; keep intact all the
notices that refer to this License and to the absence of any warranty;
and give any other recipients of the Program a copy of this License
along with the Program.
You may charge a fee for the physical act of transferring a copy, and
you may at your option offer warranty protection in exchange for a fee.
2. You may modify your copy or copies of the Program or any portion
of it, thus forming a work based on the Program, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) You must cause the modified files to carry prominent notices
stating that you changed the files and the date of any change.
b) You must cause any work that you distribute or publish, that in
whole or in part contains or is derived from the Program or any
part thereof, to be licensed as a whole at no charge to all third
parties under the terms of this License.
c) If the modified program normally reads commands interactively
when run, you must cause it, when started running for such
interactive use in the most ordinary way, to print or display an
announcement including an appropriate copyright notice and a
notice that there is no warranty (or else, saying that you provide
a warranty) and that users may redistribute the program under
these conditions, and telling the user how to view a copy of this
License. (Exception: if the Program itself is interactive but
does not normally print such an announcement, your work based on
the Program is not required to print an announcement.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Program,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Program, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Program.
In addition, mere aggregation of another work not based on the Program
with the Program (or with a work based on the Program) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may copy and distribute the Program (or a work based on it,
under Section 2) in object code or executable form under the terms of
Sections 1 and 2 above provided that you also do one of the following:
a) Accompany it with the complete corresponding machine-readable
source code, which must be distributed under the terms of Sections
1 and 2 above on a medium customarily used for software interchange; or,
b) Accompany it with a written offer, valid for at least three
years, to give any third party, for a charge no more than your
cost of physically performing source distribution, a complete
machine-readable copy of the corresponding source code, to be
distributed under the terms of Sections 1 and 2 above on a medium
customarily used for software interchange; or,
c) Accompany it with the information you received as to the offer
to distribute corresponding source code. (This alternative is
allowed only for noncommercial distribution and only if you
received the program in object code or executable form with such
an offer, in accord with Subsection b above.)
The source code for a work means the preferred form of the work for
making modifications to it. For an executable work, complete source
code means all the source code for all modules it contains, plus any
associated interface definition files, plus the scripts used to
control compilation and installation of the executable. However, as a
special exception, the source code distributed need not include
anything that is normally distributed (in either source or binary
form) with the major components (compiler, kernel, and so on) of the
operating system on which the executable runs, unless that component
itself accompanies the executable.
If distribution of executable or object code is made by offering
access to copy from a designated place, then offering equivalent
access to copy the source code from the same place counts as
distribution of the source code, even though third parties are not
compelled to copy the source along with the object code.
4. You may not copy, modify, sublicense, or distribute the Program
except as expressly provided under this License. Any attempt
otherwise to copy, modify, sublicense or distribute the Program is
void, and will automatically terminate your rights under this License.
However, parties who have received copies, or rights, from you under
this License will not have their licenses terminated so long as such
parties remain in full compliance.
5. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Program or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Program (or any work based on the
Program), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Program or works based on it.
6. Each time you redistribute the Program (or any work based on the
Program), the recipient automatically receives a license from the
original licensor to copy, distribute or modify the Program subject to
these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties to
this License.
7. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Program at all. For example, if a patent
license would not permit royalty-free redistribution of the Program by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Program.
If any portion of this section is held invalid or unenforceable under
any particular circumstance, the balance of the section is intended to
apply and the section as a whole is intended to apply in other
circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system, which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
8. If the distribution and/or use of the Program is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Program under this License
may add an explicit geographical distribution limitation excluding
those countries, so that distribution is permitted only in or among
countries not thus excluded. In such case, this License incorporates
the limitation as if written in the body of this License.
9. The Free Software Foundation may publish revised and/or new versions
of the General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the Program
specifies a version number of this License which applies to it and "any
later version", you have the option of following the terms and conditions
either of that version or of any later version published by the Free
Software Foundation. If the Program does not specify a version number of
this License, you may choose any version ever published by the Free Software
Foundation.
10. If you wish to incorporate parts of the Program into other free
programs whose distribution conditions are different, write to the author
to ask for permission. For software which is copyrighted by the Free
Software Foundation, write to the Free Software Foundation; we sometimes
make exceptions for this. Our decision will be guided by the two goals
of preserving the free status of all derivatives of our free software and
of promoting the sharing and reuse of software generally.
NO WARRANTY
11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
REPAIR OR CORRECTION.
12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
Also add information on how to contact you by electronic and paper mail.
If the program is interactive, make it output a short notice like this
when it starts in an interactive mode:
Gnomovision version 69, Copyright (C) year name of author
Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, the commands you use may
be called something other than `show w' and `show c'; they could even be
mouse-clicks or menu items--whatever suits your program.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the program, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the program
`Gnomovision' (which makes passes at compilers) written by James Hacker.
<signature of Ty Coon>, 1 April 1989
Ty Coon, President of Vice
This General Public License does not permit incorporating your program into
proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License.
================================================
FILE: FAST-LIO/Log/fast_lio_time_log_analysis.m
================================================
clear
close all
Color_red = [0.6350 0.0780 0.1840];
Color_blue = [0 0.4470 0.7410];
Color_orange = [0.8500 0.3250 0.0980];
Color_green = [0.4660 0.6740 0.1880];
Color_lightblue = [0.3010 0.7450 0.9330];
Color_purple = [0.4940 0.1840 0.5560];
Color_yellow = [0.9290 0.6940 0.1250];
fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0);
timestamp_ikd = fast_lio_ikdtree(:,1);
timestamp_ikd = timestamp_ikd - min(timestamp_ikd);
total_time_ikd = fast_lio_ikdtree(:,2)*1e3;
scan_num = fast_lio_ikdtree(:,3);
incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3;
search_time_ikd = fast_lio_ikdtree(:,5)*1e3;
delete_size_ikd = fast_lio_ikdtree(:,6);
delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3;
tree_size_ikd_st = fast_lio_ikdtree(:,8);
tree_size_ikd = fast_lio_ikdtree(:,9);
add_points = fast_lio_ikdtree(:,10);
fast_lio_forest = csvread("fast_lio_time_log.csv",1,0);
fov_check_time_forest = fast_lio_forest(:,5)*1e3;
average_time_forest = fast_lio_forest(:,2)*1e3;
total_time_forest = fast_lio_forest(:,6)*1e3;
incremental_time_forest = fast_lio_forest(:,3)*1e3;
search_time_forest = fast_lio_forest(:,4)*1e3;
timestamp_forest = fast_lio_forest(:,1);
% Use slide window to calculate average
L = 1; % Length of slide window
for i = 1:length(timestamp_ikd)
if (i<L)
average_time_ikd(i) = mean(total_time_ikd(1:i));
else
average_time_ikd(i) = mean(total_time_ikd(i-L+1:i));
end
end
for i = 1:length(timestamp_forest)
if (i<L)
average_time_forest(i) = mean(total_time_forest(1:i));
else
average_time_forest(i) = mean(total_time_forest(i-L+1:i));
end
end
f = figure;
set(gcf,'Position',[80 433 600 640])
tiled_handler = tiledlayout(3,1);
tiled_handler.TileSpacing = 'compact';
tiled_handler.Padding = 'compact';
nexttile;
hold on;
set(gca,'FontSize',12,'FontName','Times New Roman')
plot(timestamp_ikd, average_time_ikd,'-','Color',Color_blue,'LineWidth',1.2);
plot(timestamp_forest, average_time_forest,'--','Color',Color_orange,'LineWidth',1.2);
lg = legend("ikd-Tree", "ikd-Forest",'location',[0.1314 0.8559 0.2650 0.0789],'fontsize',14,'fontname','Times New Roman')
title("Time Performance on FAST-LIO",'FontSize',16,'FontName','Times New Roman')
xlabel("time/s",'FontSize',16,'FontName','Times New Roman')
yl = ylabel("Run Time/ms",'FontSize',15,'Position',[285.7 5.5000 -1]);
xlim([32,390]);
ylim([0,23]);
ax1 = gca;
ax1.YAxis.FontSize = 12;
ax1.XAxis.FontSize = 12;
grid on
box on
% print('./Figures/fastlio_exp_average','-depsc','-r600')
index_ikd = find(search_time_ikd > 0);
search_time_ikd = search_time_ikd(index_ikd);
index_forest = find(search_time_forest > 0);
search_time_forest = search_time_forest(index_forest);
t = nexttile;
hold on;
boxplot_data_ikd = [incremental_time_ikd,total_time_ikd];
boxplot_data_forest = [incremental_time_forest,total_time_forest];
Colors_ikd = [Color_blue;Color_blue;Color_blue];
Colors_forest = [Color_orange;Color_orange;Color_orange];
% xticks([3,8,13])
h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3);
h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3);
h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3);
h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3);
ax2 = gca;
ax2.YAxis.Scale = 'log';
xlim([0.5,6.0])
ylim([0.0008,100])
xticks([1.25 3.25 5.25])
xticklabels({'Nearest Search',' Incremental Updates','Total Time'});
yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2])
ax2.YAxis.FontSize = 12;
ax2.XAxis.FontSize = 14.5;
% ax.XAxis.FontWeight = 'bold';
ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman')
box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')];
for j=1:length(box_vars)
if (j<=3)
Color = Color_blue;
else
Color = Color_orange;
end
patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color);
end
Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman');
grid on
set(gca,'YMinorGrid','off')
nexttile;
hold on;
grid on;
box on;
set(gca,'FontSize',12,'FontName','Times New Roman')
plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2);
plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2);
plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2);
lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman')
title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman')
xlabel("time/s",'FontSize',16,'FontName','Times New Roman')
yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1])
xlim([32,390]);
ylim([0,0.85]);
ax3 = gca;
ax3.YAxis.FontSize = 12;
ax3.XAxis.FontSize = 12;
% print('./Figures/fastlio_exp_combine','-depsc','-r1200')
% exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector')
================================================
FILE: FAST-LIO/Log/guide.md
================================================
Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h.
================================================
FILE: FAST-LIO/Log/plot.py
================================================
# import matplotlib
# matplotlib.use('Agg')
import numpy as np
import matplotlib.pyplot as plt
#######for ikfom
fig, axs = plt.subplots(4,2)
lab_pre = ['', 'pre-x', 'pre-y', 'pre-z']
lab_out = ['', 'out-x', 'out-y', 'out-z']
plot_ind = range(7,10)
a_pre=np.loadtxt('mat_pre.txt')
a_out=np.loadtxt('mat_out.txt')
time=a_pre[:,0]
axs[0,0].set_title('Attitude')
axs[1,0].set_title('Translation')
axs[2,0].set_title('Extrins-R')
axs[3,0].set_title('Extrins-T')
axs[0,1].set_title('Velocity')
axs[1,1].set_title('bg')
axs[2,1].set_title('ba')
axs[3,1].set_title('Gravity')
for i in range(1,4):
for j in range(8):
axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i])
axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i])
for j in range(8):
# axs[j].set_xlim(386,389)
axs[j%4, j/4].grid()
axs[j%4, j/4].legend()
plt.grid()
#######for ikfom#######
#### Draw IMU data
fig, axs = plt.subplots(2)
imu=np.loadtxt('imu.txt')
time=imu[:,0]
axs[0].set_title('Gyroscope')
axs[1].set_title('Accelerameter')
lab_1 = ['gyr-x', 'gyr-y', 'gyr-z']
lab_2 = ['acc-x', 'acc-y', 'acc-z']
for i in range(3):
# if i==1:
axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i])
axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i])
for i in range(2):
# axs[i].set_xlim(386,389)
axs[i].grid()
axs[i].legend()
plt.grid()
# #### Draw time calculation
# plt.figure(3)
# fig = plt.figure()
# font1 = {'family' : 'Times New Roman',
# 'weight' : 'normal',
# 'size' : 12,
# }
# c="red"
# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt')
# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt')
# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt')
# # n = a_out[:,1].size
# # time_mean = a_out[:,1].mean()
# # time_se = a_out[:,1].std() / np.sqrt(n)
# # time_err = a_out[:,1] - time_mean
# # feat_mean = a_out[:,2].mean()
# # feat_err = a_out[:,2] - feat_mean
# # feat_se = a_out[:,2].std() / np.sqrt(n)
# ax1 = fig.add_subplot(111)
# ax1.set_ylabel('Effective Feature Numbers',font1)
# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9])
# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9])
# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9])
# ax1.set_ylim([0, 3000])
# ax2 = ax1.twinx()
# ax2.spines['right'].set_color('red')
# ax2.set_ylabel('Compute Time (ms)',font1)
# ax2.yaxis.label.set_color('red')
# ax2.tick_params(axis='y', colors='red')
# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c))
# ax2.set_xlim([0.5, 3.5])
# ax2.set_ylim([0, 100])
# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2'))
# # # print(time_se)
# # # print(a_out3[:,2])
# plt.grid()
# plt.savefig("time.pdf", dpi=1200)
plt.show()
================================================
FILE: FAST-LIO/PCD/1
================================================
1
================================================
FILE: FAST-LIO/README.md
================================================
## Related Works
1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search.
2. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter.
3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.
4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
6. [SC-A-LOAM](https://github.com/gisbi-kim/SC-A-LOAM#for-livox-lidar): A [Scan-Context](https://github.com/irapkaist/scancontext) loop closure module that can directly work with FAST-LIO.
## FAST-LIO
**FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues:
1. Fast iterated Kalman filter for odometry optimization;
2. Automaticaly initialized at most steady environments;
3. Parallel KD-Tree Search to decrease the computation;
## FAST-LIO 2.0 (2021-07-05 Update)
<!--  -->
<!-- [](https://youtu.be/2OvjGnxszf8) -->
<div align="left">
<img src="doc/real_experiment2.gif" width=49.6% />
<img src="doc/ulhkwh_fastlio.gif" width = 49.6% >
</div>
**Related video:** [FAST-LIO2](https://youtu.be/2OvjGnxszf8), [FAST-LIO1](https://youtu.be/iYCY6T79oNU), [FAST-LIO1 + Scan-context loop closure](https://youtu.be/Fw9S6D6HozA)
**Pipeline:**
<div align="center">
<img src="doc/overview_fastlio2.svg" width=99% />
</div>
**New Features:**
1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate.
2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
3. Since no requirements for feature extraction, FAST-LIO2 can support may types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs.
4. Support external IMU.
5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM).
**Related papers**:
[FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf)
[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196)
**Contributors**
[Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan)
<!-- <div align="center">
<img src="doc/results/HKU_HW.png" width = 49% >
<img src="doc/results/HKU_MB_001.png" width = 49% >
</div> -->
## 1. Prerequisites
### 1.1 **Ubuntu** and **ROS**
**Ubuntu >= 16.04**
For **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally.
ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
### 1.2. **PCL && Eigen**
PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).
Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
### 1.3. **livox_ros_driver**
Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).
*Remarks:*
- Since the FAST-LIO must support Livox serials LiDAR firstly, so the **livox_ros_driver** must be installed and **sourced** before run any FAST-LIO luanch file.
- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
## 2. Build
Clone the repository and catkin_make:
```
cd ~/$A_ROS_DIR$/src
git clone https://github.com/hku-mars/FAST_LIO.git
cd FAST_LIO
git submodule update --init
cd ../..
catkin_make
source devel/setup.bash
```
- Remember to source the livox_ros_driver before build (follow 1.3 **livox_ros_driver**)
- If you want to use a custom build of PCL, add the following line to ~/.bashrc
```export PCL_ROOT={CUSTOM_PCL_PATH}```
## 3. Directly run
Please make sure the IMU and LiDAR are **Synchronized**, that's important.
### 3.1 For Avia
Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
```
cd ~/$FAST_LIO_ROS_DIR$
source devel/setup.bash
roslaunch fast_lio mapping_avia.launch
roslaunch livox_ros_driver livox_lidar_msg.launch
```
- For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. ``` livox_lidar.launch ``` can not produce it right now.
- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage.
### 3.2 For Livox serials with external IMU
mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:
Edit ``` config/avia.yaml ``` to set the below parameters:
1. LiDAR point cloud topic name: ``` lid_topic ```
2. IMU topic name: ``` imu_topic ```
3. Translational extrinsic: ``` extrinsic_T ```
4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual.
- FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter ```time_sync_en``` to ture to turn on. But turn on **ONLY IF external time synchronization is really not possible**, since the software time sync cannot make sure accuracy.
### 3.3 For Velodyne or Ouster (Velodyne as an example)
Step A: Setup before run
Edit ``` config/velodyne.yaml ``` to set the below parameters:
1. LiDAR point cloud topic name: ``` lid_topic ```
2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
3. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```
4. Translational extrinsic: ``` extrinsic_T ```
5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame).
Step B: Run below
```
cd ~/$FAST_LIO_ROS_DIR$
source devel/setup.bash
roslaunch fast_lio mapping_velodyne.launch
```
Step C: Run LiDAR's ros driver or play rosbag.
*Remarks:*
- We will produce some velodyne datasets which is already transfered to Rosbags, please wait for a while.
### 3.4 PCD file save
Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds.
*Tips for pcl_viewer:*
- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running.
```
1 is all random
2 is X values
3 is Y values
4 is Z values
5 is intensity
```
## 4. Rosbag Example
### 4.1 Indoor rosbag (Livox Avia LiDAR)
<div align="center"><img src="doc/results/HKU_LG_Indoor.png" width=100% /></div>
Download [avia_indoor_quick_shake_example1](https://drive.google.com/file/d/1SWmrwlUD5FlyA-bTr1rakIYx1GxS4xNl/view?usp=sharing) or [avia_indoor_quick_shake_example2](https://drive.google.com/file/d/1wD485CIbzZlNs4z8e20Dv2Q1q-7Gv_AT/view?usp=sharing) and then
```
roslaunch fast_lio mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag
```
### 4.2 Outdoor rosbag (Livox Avia LiDAR)
<div align="center"><img src="doc/results/HKU_MB_002.png" width=100% /></div>
<!-- <div align="center"><img src="doc/results/mid40_outdoor.png" width=90% /></div> -->
Download [avia_hku_main building_mapping](https://drive.google.com/file/d/1GSb9eLQuwqmgI3VWSB5ApEUhOCFG_Sv5/view?usp=sharing) and then
```
roslaunch fast_lio mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag
```
## 5.Implementation on UAV
In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below.
The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future.
<div align="center">
<img src="doc/uav01.jpg" width=40.5% >
<img src="doc/uav_system.png" width=57% >
</div>
## 6.Acknowledgments
Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox).
================================================
FILE: FAST-LIO/config/avia.yaml
================================================
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess:
lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 6
blind: 4
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 70
det_range: 260.0
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
# extrinsic_T: [ 0.0, 0.00, -0.0284 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame
================================================
FILE: FAST-LIO/config/horizon.yaml
================================================
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess:
lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 6
blind: 4
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 100
det_range: 260.0
extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame
================================================
FILE: FAST-LIO/config/ouster64.yaml
================================================
common:
lid_topic: "/os_cloud_node/points"
imu_topic: "/os_cloud_node/imu"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess:
lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 64
blind: 4
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 180
det_range: 150.0
extrinsic_T: [ 0.0, 0.0, 0.0 ]
extrinsic_R: [1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame
================================================
FILE: FAST-LIO/config/ouster64_mulran.yaml
================================================
common:
lid_topic: "/os1_points"
imu_topic: "/imu/data_raw"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess:
lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 64
blind: 4
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 180
det_range: 150.0
extrinsic_T: [ 1.77, 0.0, -0.05 ]
extrinsic_R: [-1, 0, 0,
0, -1, 0,
0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame
================================================
FILE: FAST-LIO/config/velodyne.yaml
================================================
common:
lid_topic: "/velodyne_points"
imu_topic: "/imu/data"
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
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_T: [ 0, 0, 0.28]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
scan_publish_en: true # 'false' will close all the point cloud output
dense_publish_en: ture # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # output the point cloud scans in IMU-body-frame
================================================
FILE: FAST-LIO/include/Exp_mat.h
================================================
#ifndef EXP_MAT_H
#define EXP_MAT_H
#include <math.h>
#include <Eigen/Core>
#include <opencv2/core.hpp>
// #include <common_lib.h>
#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0
template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang)
{
T ang_norm = ang.norm();
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (ang_norm > 0.0000001)
{
Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;
Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_axis);
/// Roderigous Tranformation
return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K;
}
else
{
return Eye3;
}
}
template<typename T, typename Ts>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &ang_vel, const Ts &dt)
{
T ang_vel_norm = ang_vel.norm();
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (ang_vel_norm > 0.0000001)
{
Eigen::Matrix<T, 3, 1> r_axis = ang_vel / ang_vel_norm;
Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_axis);
T r_ang = ang_vel_norm * dt;
/// Roderigous Tranformation
return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K;
}
else
{
return Eye3;
}
}
template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const T &v1, const T &v2, const T &v3)
{
T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
Eigen::Matrix<T, 3, 3> Eye3 = Eigen::Matrix<T, 3, 3>::Identity();
if (norm > 0.00001)
{
T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
Eigen::Matrix<T, 3, 3> K;
K << SKEW_SYM_MATRX(r_ang);
/// Roderigous Tranformation
return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
}
else
{
return Eye3;
}
}
/* Logrithm of a Rotation Matrix */
template<typename T>
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R)
{
T &&theta = std::acos(0.5 * (R.trace() - 1));
Eigen::Matrix<T,3,1> K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1));
return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K);
}
// template<typename T>
// cv::Mat Exp(const T &v1, const T &v2, const T &v3)
// {
// T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3);
// cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F);
// if (norm > 0.0000001)
// {
// T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm};
// cv::Mat K = (cv::Mat_<T>(3,3) << SKEW_SYM_MATRX(r_ang));
// /// Roderigous Tranformation
// return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K;
// }
// else
// {
// return Eye3;
// }
// }
#endif
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp
================================================
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ESEKFOM_EKF_HPP
#define ESEKFOM_EKF_HPP
#include <vector>
#include <cstdlib>
#include <boost/bind.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "../mtk/types/vect.hpp"
#include "../mtk/types/SOn.hpp"
#include "../mtk/types/S2.hpp"
#include "../mtk/startIdx.hpp"
#include "../mtk/build_manifold.hpp"
#include "util.hpp"
//#define USE_sparse
namespace esekfom
{
using namespace Eigen;
// used for iterated error state EKF update
// for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
// applied for measurement as a manifold.
template <typename S, typename M, int measurement_noise_dof = M::DOF>
struct share_datastruct
{
bool valid;
bool converge;
M z;
Eigen::Matrix<typename S::scalar, M::DOF, measurement_noise_dof> h_v;
Eigen::Matrix<typename S::scalar, M::DOF, S::DOF> h_x;
Eigen::Matrix<typename S::scalar, measurement_noise_dof, measurement_noise_dof> R;
};
// used for iterated error state EKF update
// for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
// applied for measurement as an Eigen matrix whose dimension is changing
template <typename T>
struct dyn_share_datastruct
{
bool valid;
bool converge;
Eigen::Matrix<T, Eigen::Dynamic, 1> z;
Eigen::Matrix<T, Eigen::Dynamic, 1> h;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_v;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> R;
};
// used for iterated error state EKF update
// for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
// applied for measurement as a dynamic manifold whose dimension or type is changing
template <typename T>
struct dyn_runtime_share_datastruct
{
bool valid;
bool converge;
// Z z;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_v;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> R;
};
//状态量，噪声维度，输入状态量这三个参数输入
template <typename state, int process_noise_dof, typename input = state, typename measurement = state, int measurement_noise_dof = 0>
class esekf
{
typedef esekf self;
enum
{
n = state::DOF, //状态量自由度，一般代表x的维度
m = state::DIM, //状态量自由度，一般代表res的维度
l = measurement::DOF //测量噪声维度
};
public:
typedef typename state::scalar scalar_type;
typedef Matrix<scalar_type, n, n> cov;
typedef Matrix<scalar_type, m, n> cov_;
typedef SparseMatrix<scalar_type> spMt;
typedef Matrix<scalar_type, n, 1> vectorized_state;
typedef Matrix<scalar_type, m, 1> flatted_state;
typedef flatted_state processModel(state &, const input &); //声明一个函数指针
typedef Eigen::Matrix<scalar_type, m, n> processMatrix1(state &, const input &);
typedef Eigen::Matrix<scalar_type, m, process_noise_dof> processMatrix2(state &, const input &);
typedef Eigen::Matrix<scalar_type, process_noise_dof, process_noise_dof> processnoisecovariance;
typedef measurement measurementModel(state &, bool &);
typedef measurement measurementModel_share(state &, share_datastruct<state, measurement, measurement_noise_dof> &);
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> measurementModel_dyn(state &, bool &);
// typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> measurementModel_dyn_share(state &, dyn_share_datastruct<scalar_type> &);
typedef void measurementModel_dyn_share(state &, dyn_share_datastruct<scalar_type> &);
typedef Eigen::Matrix<scalar_type, l, n> measurementMatrix1(state &, bool &);
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, n> measurementMatrix1_dyn(state &, bool &);
typedef Eigen::Matrix<scalar_type, l, measurement_noise_dof> measurementMatrix2(state &, bool &);
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> measurementMatrix2_dyn(state &, bool &);
typedef Eigen::Matrix<scalar_type, measurement_noise_dof, measurement_noise_dof> measurementnoisecovariance;
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> measurementnoisecovariance_dyn;
esekf(const state &x = state(),
const cov &P = cov::Identity()) : x_(x), P_(P) // esekf初始化，主要是初始化了x_和P_
{
#ifdef USE_sparse
SparseMatrix<scalar_type> ref(n, n);
ref.setIdentity();
l_ = ref;
f_x_2 = ref;
f_x_1 = ref;
#endif
};
// receive system-specific models and their differentions.
// for measurement as a manifold.
void init(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel h_in, measurementMatrix1 h_x_in, measurementMatrix2 h_v_in, int maximum_iteration, scalar_type limit_vector[n])
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
h = h_in;
h_x = h_x_in;
h_v = h_v_in;
maximum_iter = maximum_iteration;
for (int i = 0; i < n; i++)
{
limit[i] = limit_vector[i];
}
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
// receive system-specific models and their differentions.
// for measurement as an Eigen matrix whose dimention is chaing.
void init_dyn(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn h_in, measurementMatrix1_dyn h_x_in, measurementMatrix2_dyn h_v_in, int maximum_iteration, scalar_type limit_vector[n])
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
h_dyn = h_in;
h_x_dyn = h_x_in;
h_v_dyn = h_v_in;
maximum_iter = maximum_iteration;
for (int i = 0; i < n; i++)
{
limit[i] = limit_vector[i];
}
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
// receive system-specific models and their differentions.
// for measurement as a dynamic manifold whose dimension or type is changing.
void init_dyn_runtime(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementMatrix1_dyn h_x_in, measurementMatrix2_dyn h_v_in, int maximum_iteration, scalar_type limit_vector[n])
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
h_x_dyn = h_x_in;
h_v_dyn = h_v_in;
maximum_iter = maximum_iteration;
for (int i = 0; i < n; i++)
{
limit[i] = limit_vector[i];
}
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
// receive system-specific models and their differentions
// for measurement as a manifold.
// calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_share_in).
void init_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_share h_share_in, int maximum_iteration, scalar_type limit_vector[n])
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
h_share = h_share_in;
maximum_iter = maximum_iteration;
for (int i = 0; i < n; i++)
{
limit[i] = limit_vector[i];
}
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
//接收系统特定的模型及其差异
//作为特征矩阵的测量，其维数是变化的。
//通过一个函数(h_dyn_share_in)完成了测量(z)，估计测量(h)，偏微分矩阵(h_x, h_v)和噪声协方差(R)的同时计算。
void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n])
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
h_dyn_share = h_dyn_share_in;
maximum_iter = maximum_iteration;
for (int i = 0; i < n; i++)
{
limit[i] = limit_vector[i];
}
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
// receive system-specific models and their differentions
// for measurement as a dynamic manifold whose dimension or type is changing.
// calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_dyn_share_in).
// for any scenarios where it is needed
void init_dyn_runtime_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, int maximum_iteration, scalar_type limit_vector[n])
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
maximum_iter = maximum_iteration;
for (int i = 0; i < n; i++)
{
limit[i] = limit_vector[i];
}
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
// 迭代误差状态EKF传播
void predict(double &dt, processnoisecovariance &Q, const input &i_in) //这里的参数均从use-ikfom中传入
{
// f函数对应use-ikfom.hpp中的 get_f函数，对应fast_lio2论文公式(2)
flatted_state f_ = f(x_, i_in); // m*1
// 对应use-ikfom.hpp中的 df_dx函数
// 对应fast_lio2论文公式(7)
cov_ f_x_ = f_x(x_, i_in); // m*n
cov f_x_final; // n*n
// 对应use-ikfom.hpp中的 df_dw函数
// 对应fast_lio2论文公式(7)
Matrix<scalar_type, m, process_noise_dof> f_w_ = f_w(x_, i_in);
Matrix<scalar_type, n, process_noise_dof> f_w_final;
state x_before = x_; //保存x_的值，用于后面的更新
// 对应fast_lio2论文公式(2)
x_.oplus(f_, dt); //这个是公式2的整个函数，用于更新x的
F_x1 = cov::Identity(); //状态转移矩阵
// 更新f_x和f_w
for (std::vector<std::pair<std::pair<int, int>, int>>::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++)
{
int idx = (*it).first.first; //状态变量的索引
int dim = (*it).first.second; //状态变量的维数
int dof = (*it).second; //状态变量的自由度
for (int i = 0; i < n; i++)
{
for (int j = 0; j < dof; j++)
{
f_x_final(idx + j, i) = f_x_(dim + j, i); //更新f_x_final，形成n*n阵，用于更新
}
}
for (int i = 0; i < process_noise_dof; i++)
{
for (int j = 0; j < dof; j++)
{
f_w_final(idx + j, i) = f_w_(dim + j, i); //更新f_w_final，形成n*dof，用于更新
}
}
}
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first; //状态变量的索引
int dim = (*it).second; //状态变量的维数
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = -1 * f_(dim + i) * dt; //拿到S03更新值
}
MTK::SO3<scalar_type> res; //残差计算
res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_SO3, scalar_type(1 / 2)); //残差计算
#ifdef USE_sparse
res_temp_SO3 = res.toRotationMatrix();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
f_x_1.coeffRef(idx + i, idx + j) = res_temp_SO3(i, j);
}
}
#else
F_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix(); //更新f_x_1
#endif
res_temp_SO3 = MTK::A_matrix(seg_SO3); //转为矩阵形式
for (int i = 0; i < n; i++)
{
f_x_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_.template block<3, 1>(dim, i)); // F矩阵
}
for (int i = 0; i < process_noise_dof; i++)
{
f_w_final.template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_.template block<3, 1>(dim, i));
}
}
Matrix<scalar_type, 2, 3> res_temp_S2;
Matrix<scalar_type, 2, 2> res_temp_S2_;
MTK::vect<3, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_S2(i) = f_(dim + i) * dt;
}
MTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero();
MTK::SO3<scalar_type> res;
res.w() = MTK::exp<scalar_type, 3>(res.vec(), seg_S2, scalar_type(1 / 2));
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_before.S2_Mx(Mx, vec, idx);
#ifdef USE_sparse
res_temp_S2_ = Nx * res.toRotationMatrix() * Mx;
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
f_x_1.coeffRef(idx + i, idx + j) = res_temp_S2_(i, j);
}
}
#else
F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx;
#endif
Eigen::Matrix<scalar_type, 3, 3> x_before_hat;
x_before.S2_hat(x_before_hat, idx);
res_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat * MTK::A_matrix(seg_S2).transpose();
for (int i = 0; i < n; i++)
{
f_x_final.template block<2, 1>(idx, i) = res_temp_S2 * (f_x_.template block<3, 1>(dim, i));
}
for (int i = 0; i < process_noise_dof; i++)
{
f_w_final.template block<2, 1>(idx, i) = res_temp_S2 * (f_w_.template block<3, 1>(dim, i));
}
}
#ifdef USE_sparse
f_x_1.makeCompressed();
spMt f_x2 = f_x_final.sparseView();
spMt f_w1 = f_w_final.sparseView();
spMt xp = f_x_1 + f_x2 * dt;
P_ = xp * P_ * xp.transpose() + (f_w1 * dt) * Q * (f_w1 * dt).transpose();
#else
F_x1 += f_x_final * dt;
P_ = (F_x1)*P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();
#endif
}
// iterated error state EKF update for measurement as a manifold.
void update_iterated(measurement &z, measurementnoisecovariance &R)
{
if (!(is_same<typename measurement::scalar, scalar_type>()))
{
std::cerr << "the scalar type of measurment must be the same as the state" << std::endl;
std::exit(100);
}
int t = 0;
bool converg = true;
bool valid = true;
state x_propagated = x_;
cov P_propagated = P_;
for (int i = -1; i < maximum_iter; i++)
{
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
#ifdef USE_sparse
spMt h_x_ = h_x(x_, valid).sparseView();
spMt h_v_ = h_v(x_, valid).sparseView();
spMt R_ = R.sparseView();
#else
Matrix<scalar_type, l, n> h_x_ = h_x(x_, valid);
Matrix<scalar_type, l, Eigen::Dynamic> h_v_ = h_v(x_, valid);
#endif
if (!valid)
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, n, l> K_;
if (n > l)
{
#ifdef USE_sparse
Matrix<scalar_type, l, l> K_temp = h_x_ * P_ * h_x_.transpose();
spMt R_temp = h_v_ * R_ * h_v_.transpose();
K_temp += R_temp;
K_ = P_ * h_x_.transpose() * K_temp.inverse();
#else
K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
measurementnoisecovariance b = measurementnoisecovariance::Identity();
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
measurementnoisecovariance R_in_temp = solver.solve(b);
spMt R_in = R_in_temp.sparseView();
spMt K_temp = h_x_.transpose() * R_in * h_x_;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x_.transpose() * R_in;
#else
measurementnoisecovariance R_in = (h_v_ * R * h_v_.transpose()).inverse();
K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
#endif
}
Matrix<scalar_type, l, 1> innovation;
z.boxminus(innovation, h(x_, valid));
cov K_x = K_ * h_x_;
Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
converg = true;
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
converg = false;
break;
}
}
if (converg)
t++;
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = A_matrix(seg_SO3).transpose();
for (int i = 0; i < n; i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
if (n > l)
{
for (int i = 0; i < l; i++)
{
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
if (n > l)
{
for (int i = 0; i < l; i++)
{
K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if (n > l)
{
P_ = L_ - K_ * h_x_ * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
// iterated error state EKF update for measurement as a manifold.
// calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
void update_iterated_share()
{
if (!(is_same<typename measurement::scalar, scalar_type>()))
{
std::cerr << "the scalar type of measurment must be the same as the state" << std::endl;
std::exit(100);
}
int t = 0;
share_datastruct<state, measurement, measurement_noise_dof> _share;
_share.valid = true;
_share.converge = true;
state x_propagated = x_;
cov P_propagated = P_;
for (int i = -1; i < maximum_iter; i++)
{
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
measurement h = h_share(x_, _share);
measurement z = _share.z;
measurementnoisecovariance R = _share.R;
#ifdef USE_sparse
spMt h_x_ = _share.h_x.sparseView();
spMt h_v_ = _share.h_v.sparseView();
spMt R_ = _share.R.sparseView();
#else
Matrix<scalar_type, l, n> h_x_ = _share.h_x;
Matrix<scalar_type, l, Eigen::Dynamic> h_v_ = _share.h_v;
#endif
if (!_share.valid)
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, n, l> K_;
if (n > l)
{
#ifdef USE_sparse
Matrix<scalar_type, l, l> K_temp = h_x_ * P_ * h_x_.transpose();
spMt R_temp = h_v_ * R_ * h_v_.transpose();
K_temp += R_temp;
K_ = P_ * h_x_.transpose() * K_temp.inverse();
#else
K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
measurementnoisecovariance b = measurementnoisecovariance::Identity();
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
measurementnoisecovariance R_in_temp = solver.solve(b);
spMt R_in = R_in_temp.sparseView();
spMt K_temp = h_x_.transpose() * R_in * h_x_;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x_.transpose() * R_in;
#else
measurementnoisecovariance R_in = (h_v_ * R * h_v_.transpose()).inverse();
K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
#endif
}
Matrix<scalar_type, l, 1> innovation;
z.boxminus(innovation, h);
cov K_x = K_ * h_x_;
Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
_share.converge = true;
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
_share.converge = false;
break;
}
}
if (_share.converge)
t++;
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = A_matrix(seg_SO3).transpose();
for (int i = 0; i < n; i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
if (n > l)
{
for (int i = 0; i < l; i++)
{
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
if (n > l)
{
for (int i = 0; i < l; i++)
{
K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if (n > l)
{
P_ = L_ - K_ * h_x_ * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
// iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing.
void update_iterated_dyn(Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> z, measurementnoisecovariance_dyn R)
{
int t = 0;
bool valid = true;
bool converg = true;
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
int dof_Measurement_noise = R.rows();
for (int i = -1; i < maximum_iter; i++)
{
valid = true;
#ifdef USE_sparse
spMt h_x_ = h_x_dyn(x_, valid).sparseView();
spMt h_v_ = h_v_dyn(x_, valid).sparseView();
spMt R_ = R.sparseView();
#else
Matrix<scalar_type, Eigen::Dynamic, n> h_x_ = h_x_dyn(x_, valid);
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v_ = h_v_dyn(x_, valid);
#endif
Matrix<scalar_type, Eigen::Dynamic, 1> h_ = h_dyn(x_, valid);
dof_Measurement = h_.rows();
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
if (!valid)
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
if (n > dof_Measurement)
{
#ifdef USE_sparse
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x_ * P_ * h_x_.transpose();
spMt R_temp = h_v_ * R_ * h_v_.transpose();
K_temp += R_temp;
K_ = P_ * h_x_.transpose() * K_temp.inverse();
#else
K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in = R_in_temp.sparseView();
spMt K_temp = h_x_.transpose() * R_in * h_x_;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x_.transpose() * R_in;
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v_ * R * h_v_.transpose()).inverse();
K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
#endif
}
cov K_x = K_ * h_x_;
Matrix<scalar_type, n, 1> dx_ = K_ * (z - h_) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
converg = true;
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
converg = false;
break;
}
}
if (converg)
t++;
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
std::cout << "iteration time:" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for (int i = 0; i < n; i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if (n > dof_Measurement)
{
P_ = L_ - K_ * h_x_ * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
// iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing.
// calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
void update_iterated_dyn_share()
{
int t = 0;
dyn_share_datastruct<scalar_type> dyn_share;
dyn_share.valid = true;
dyn_share.converge = true;
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
int dof_Measurement_noise;
for (int i = -1; i < maximum_iter; i++)
{
dyn_share.valid = true;
h_dyn_share(x_, dyn_share);
// Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share (x_, dyn_share);
Matrix<scalar_type, Eigen::Dynamic, 1> z = dyn_share.z;
Matrix<scalar_type, Eigen::Dynamic, 1> h = dyn_share.h;
#ifdef USE_sparse
spMt h_x = dyn_share.h_x.sparseView();
spMt h_v = dyn_share.h_v.sparseView();
spMt R_ = dyn_share.R.sparseView();
#else
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
#endif
dof_Measurement = h_x.rows();
dof_Measurement_noise = dyn_share.R.rows();
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
if (!(dyn_share.valid))
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
if (n > dof_Measurement)
{
#ifdef USE_sparse
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
spMt R_temp = h_v * R_ * h_v.transpose();
K_temp += R_temp;
K_ = P_ * h_x.transpose() * K_temp.inverse();
#else
K_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in = R_in_temp.sparseView();
spMt K_temp = h_x.transpose() * R_in * h_x;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x.transpose() * R_in;
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v * R * h_v.transpose()).inverse();
K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;
#endif
}
cov K_x = K_ * h_x;
Matrix<scalar_type, n, 1> dx_ = K_ * (z - h) + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
dyn_share.converge = true;
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
dyn_share.converge = false;
break;
}
}
if (dyn_share.converge)
t++;
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
std::cout << "iteration time:" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for (int i = 0; i < int(n); i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if (n > dof_Measurement)
{
P_ = L_ - K_ * h_x * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
// iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.
// the measurement and the measurement model are received in a dynamic manner.
template <typename measurement_runtime, typename measurementModel_runtime>
void update_iterated_dyn_runtime(measurement_runtime z, measurementnoisecovariance_dyn R, measurementModel_runtime h_runtime)
{
int t = 0;
bool valid = true;
bool converg = true;
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
int dof_Measurement_noise;
for (int i = -1; i < maximum_iter; i++)
{
valid = true;
#ifdef USE_sparse
spMt h_x_ = h_x_dyn(x_, valid).sparseView();
spMt h_v_ = h_v_dyn(x_, valid).sparseView();
spMt R_ = R.sparseView();
#else
Matrix<scalar_type, Eigen::Dynamic, n> h_x_ = h_x_dyn(x_, valid);
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v_ = h_v_dyn(x_, valid);
#endif
measurement_runtime h_ = h_runtime(x_, valid);
dof_Measurement = measurement_runtime::DOF;
dof_Measurement_noise = R.rows();
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
if (!valid)
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
if (n > dof_Measurement)
{
#ifdef USE_sparse
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x_ * P_ * h_x_.transpose();
spMt R_temp = h_v_ * R_ * h_v_.transpose();
K_temp += R_temp;
K_ = P_ * h_x_.transpose() * K_temp.inverse();
#else
K_ = P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in = R_in_temp.sparseView();
spMt K_temp = h_x_.transpose() * R_in * h_x_;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x_.transpose() * R_in;
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v_ * R * h_v_.transpose()).inverse();
K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in;
#endif
}
cov K_x = K_ * h_x_;
Eigen::Matrix<scalar_type, measurement_runtime::DOF, 1> innovation;
z.boxminus(innovation, h_);
Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
converg = true;
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
converg = false;
break;
}
}
if (converg)
t++;
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
std::cout << "iteration time:" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for (int i = 0; i < n; i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if (n > dof_Measurement)
{
P_ = L_ - K_ * h_x_ * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
// iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing.
// the measurement and the measurement model are received in a dynamic manner.
// calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function.
template <typename measurement_runtime, typename measurementModel_dyn_runtime_share>
void update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h)
{
int t = 0;
dyn_runtime_share_datastruct<scalar_type> dyn_share;
dyn_share.valid = true;
dyn_share.converge = true;
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
int dof_Measurement_noise;
for (int i = -1; i < maximum_iter; i++)
{
dyn_share.valid = true;
measurement_runtime h_ = h(x_, dyn_share);
// measurement_runtime z = dyn_share.z;
#ifdef USE_sparse
spMt h_x = dyn_share.h_x.sparseView();
spMt h_v = dyn_share.h_v.sparseView();
spMt R_ = dyn_share.R.sparseView();
#else
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R = dyn_share.R;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x = dyn_share.h_x;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_v = dyn_share.h_v;
#endif
dof_Measurement = measurement_runtime::DOF;
dof_Measurement_noise = dyn_share.R.rows();
vectorized_state dx, dx_new;
x_.boxminus(dx, x_propagated);
dx_new = dx;
if (!(dyn_share.valid))
{
continue;
}
P_ = P_propagated;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
if (n > dof_Measurement)
{
#ifdef USE_sparse
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
spMt R_temp = h_v * R_ * h_v.transpose();
K_temp += R_temp;
K_ = P_ * h_x.transpose() * K_temp.inverse();
#else
K_ = P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
#endif
}
else
{
#ifdef USE_sparse
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> b = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Identity(dof_Measurement_noise, dof_Measurement_noise);
Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in = R_in_temp.sparseView();
spMt K_temp = h_x.transpose() * R_in * h_x;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x.transpose() * R_in;
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in = (h_v * R * h_v.transpose()).inverse();
K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in;
#endif
}
cov K_x = K_ * h_x;
Eigen::Matrix<scalar_type, measurement_runtime::DOF, 1> innovation;
z.boxminus(innovation, h_);
Matrix<scalar_type, n, 1> dx_ = K_ * innovation + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;
state x_before = x_;
x_.boxplus(dx_);
dyn_share.converge = true;
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
dyn_share.converge = false;
break;
}
}
if (dyn_share.converge)
t++;
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
std::cout << "iteration time:" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for (int i = 0; i < int(n); i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_.template block<3, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
if (n > dof_Measurement)
{
for (int i = 0; i < dof_Measurement; i++)
{
K_.template block<2, 1>(idx, i) = res_temp_S2 * (K_.template block<2, 1>(idx, i));
}
}
else
{
for (int i = 0; i < n; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
if (n > dof_Measurement)
{
P_ = L_ - K_ * h_x * P_;
}
else
{
P_ = L_ - K_x * P_;
}
return;
}
}
}
// 迭代错误状态EKF更新修改为一个特定的系统
void update_iterated_dyn_share_modified(double R, double &solve_time)
{
dyn_share_datastruct<scalar_type> dyn_share;
dyn_share.valid = true;
dyn_share.converge = true;
int t = 0;
//获取上一次的状态和协方差矩阵
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;
Matrix<scalar_type, n, 1> K_h;
Matrix<scalar_type, n, n> K_x;
vectorized_state dx_new = vectorized_state::Zero();
// 最多进行maximum_iter次迭代优化
for (int i = -1; i < maximum_iter; i++)
{
dyn_share.valid = true;
// 计算测量模型方程的雅克比，也就是点面残差的导数 H(代码里是h_x)
h_dyn_share(x_, dyn_share);
// Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
// 获取测量模型的雅克比d(pos, rot, 0, 0)/dx
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif
double solve_start = omp_get_wtime();
dof_Measurement = h_x_.rows(); // 观测方程个数m
vectorized_state dx; // 定义误差状态
x_.boxminus(dx, x_propagated); //获取误差dx
dx_new = dx; // 用于迭代的误差状态
if (!dyn_share.valid)
{
continue;
}
// 预测得到的误差状态协方差矩阵
//协方差矩阵在迭代过程中不会代入下一次迭代，直到最后一次退出时更新，在迭代过程中更新的只是先验
P_ = P_propagated;
// 这一大段都在求协方差的先验更新，大致上是P=(J^-1)*P*(J^-T)如论文式16~18
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx(idx + i);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
int dim = (*it).second;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx(idx + i);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0);
for (int i = 0; i < n; i++)
{
P_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
for (int i = 0; i < n; i++)
{
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
// Matrix<scalar_type, n, Eigen::Dynamic> K_;
// Matrix<scalar_type, n, 1> K_h;
// Matrix<scalar_type, n, n> K_x;
/*
if(n > dof_Measurement)
{
K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
}
else
{
K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
}
*/
// 状态维度 n > 测量维度 dof_Measurement
// 如果状态量维度大于观测方程 n > m，不满秩
if (n > dof_Measurement)
{
//#ifdef USE_sparse
// Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_temp = h_x * P_ * h_x.transpose();
// spMt R_temp = h_v * R_ * h_v.transpose();
// K_temp += R_temp;
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
//每一次迭代将重新计算增益K，即论文式18
h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
/*
h_x_cur.col(0) = h_x_.col(0);
h_x_cur.col(1) = h_x_.col(1);
h_x_cur.col(2) = h_x_.col(2);
h_x_cur.col(3) = h_x_.col(3);
h_x_cur.col(4) = h_x_.col(4);
h_x_cur.col(5) = h_x_.col(5);
h_x_cur.col(6) = h_x_.col(6);
h_x_cur.col(7) = h_x_.col(7);
h_x_cur.col(8) = h_x_.col(8);
h_x_cur.col(9) = h_x_.col(9);
h_x_cur.col(10) = h_x_.col(10);
h_x_cur.col(11) = h_x_.col(11);
*/
// 重新计算增益矩阵K
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose() / R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse() / R;
K_h = K_ * dyn_share.h;
K_x = K_ * h_x_cur;
//#else
// K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse();
//#endif
}
else
{
//避免求逆矩阵，K按稀疏矩阵分解的方法如论文式20
#ifdef USE_sparse
// Eigen::Matrix<scalar_type, n, n> b = Eigen::Matrix<scalar_type, n, n>::Identity();
// Eigen::SparseQR<Eigen::SparseMatrix<scalar_type>, Eigen::COLAMDOrdering<int>> solver;
spMt A = h_x_.transpose() * h_x_;
cov P_temp = (P_ / R).inverse();
P_temp.template block<12, 12>(0, 0) += A;
P_temp = P_temp.inverse();
/*
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
h_x_cur.col(0) = h_x_.col(0);
h_x_cur.col(1) = h_x_.col(1);
h_x_cur.col(2) = h_x_.col(2);
h_x_cur.col(3) = h_x_.col(3);
h_x_cur.col(4) = h_x_.col(4);
h_x_cur.col(5) = h_x_.col(5);
h_x_cur.col(6) = h_x_.col(6);
h_x_cur.col(7) = h_x_.col(7);
h_x_cur.col(8) = h_x_.col(8);
h_x_cur.col(9) = h_x_.col(9);
h_x_cur.col(10) = h_x_.col(10);
h_x_cur.col(11) = h_x_.col(11);
*/
K_ = P_temp.template block<n, 12>(0, 0) * h_x_.transpose();
K_x = cov::Zero();
K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH;
/*
solver.compute(R_);
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> R_in_temp = solver.solve(b);
spMt R_in =R_in_temp.sparseView();
spMt K_temp = h_x.transpose() * R_in * h_x;
cov P_temp = P_.inverse();
P_temp += K_temp;
K_ = P_temp.inverse() * h_x.transpose() * R_in;
*/
#else
cov P_temp = (P_ / R).inverse();
// Eigen::Matrix<scalar_type, 12, Eigen::Dynamic> h_T = h_x_.transpose();
Eigen::Matrix<scalar_type, 12, 12> HTH = h_x_.transpose() * h_x_;
P_temp.template block<12, 12>(0, 0) += HTH;
/*
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
//std::cout << "line 1767" << std::endl;
h_x_cur.col(0) = h_x_.col(0);
h_x_cur.col(1) = h_x_.col(1);
h_x_cur.col(2) = h_x_.col(2);
h_x_cur.col(3) = h_x_.col(3);
h_x_cur.col(4) = h_x_.col(4);
h_x_cur.col(5) = h_x_.col(5);
h_x_cur.col(6) = h_x_.col(6);
h_x_cur.col(7) = h_x_.col(7);
h_x_cur.col(8) = h_x_.col(8);
h_x_cur.col(9) = h_x_.col(9);
h_x_cur.col(10) = h_x_.col(10);
h_x_cur.col(11) = h_x_.col(11);
*/
cov P_inv = P_temp.inverse();
// std::cout << "line 1781" << std::endl;
K_h = P_inv.template block<n, 12>(0, 0) * h_x_.transpose() * dyn_share.h; // (H_T_H + P^-1)^-1 * H^T * h(残差) = K * h
// std::cout << "line 1780" << std::endl;
// cov HTH_cur = cov::Zero();
// HTH_cur. template block<12, 12>(0, 0) = HTH;
K_x.setZero(); // = cov::Zero();
K_x.template block<n, 12>(0, 0) = P_inv.template block<n, 12>(0, 0) * HTH; //(H_T_H + P^-1)^-1 * H_T_H = KH
// K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
#endif
}
// K_x = K_ * h_x_;
//由于是误差迭代KF，得到的是误差的最优估计！
Matrix<scalar_type, n, 1> dx_ = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new; //误差增量后验 K*h + (K*H - I) dx
state x_before = x_; // 加上校正后的误差状态dx_
x_.boxplus(dx_); //根据计算得到的误差增量后验，更新状态量
// 判断迭代是否发散
dyn_share.converge = true;
//判断已收敛的条件是误差的估计值小于阈值
for (int i = 0; i < n; i++)
{
if (std::fabs(dx_[i]) > limit[i])
{
dyn_share.converge = false;
break;
}
}
if (dyn_share.converge)
t++;
if (!t && i == maximum_iter - 2)
{
dyn_share.converge = true;
}
// 迭代完成后更新误差状态协方差矩阵
//结束迭代后，更新协方差矩阵的后验值，大致上是P=(I-K*H)*P，如论文式19
if (t > 1 || i == maximum_iter - 1)
{
L_ = P_;
// std::cout << "iteration time" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 3; i++)
{
seg_SO3(i) = dx_(i + idx);
}
res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose();
for (int i = 0; i < n; i++)
{
L_.template block<3, 1>(idx, i) = res_temp_SO3 * (P_.template block<3, 1>(idx, i));
}
// if(n > dof_Measurement)
// {
// for(int i = 0; i < dof_Measurement; i++){
// K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
// }
// }
// else
// {
for (int i = 0; i < 12; i++)
{
K_x.template block<3, 1>(idx, i) = res_temp_SO3 * (K_x.template block<3, 1>(idx, i));
}
//}
for (int i = 0; i < n; i++)
{
L_.template block<1, 3>(i, idx) = (L_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_.template block<1, 3>(i, idx) = (P_.template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
}
}
Matrix<scalar_type, 2, 2> res_temp_S2;
MTK::vect<2, scalar_type> seg_S2;
for (typename std::vector<std::pair<int, int>>::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++)
{
int idx = (*it).first;
for (int i = 0; i < 2; i++)
{
seg_S2(i) = dx_(i + idx);
}
Eigen::Matrix<scalar_type, 2, 3> Nx;
Eigen::Matrix<scalar_type, 3, 2> Mx;
x_.S2_Nx_yy(Nx, idx);
x_propagated.S2_Mx(Mx, seg_S2, idx);
res_temp_S2 = Nx * Mx;
for (int i = 0; i < n; i++)
{
L_.template block<2, 1>(idx, i) = res_temp_S2 * (P_.template block<2, 1>(idx, i));
}
// if(n > dof_Measurement)
// {
// for(int i = 0; i < dof_Measurement; i++){
// K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
// }
// }
// else
// {
for (int i = 0; i < 12; i++)
{
K_x.template block<2, 1>(idx, i) = res_temp_S2 * (K_x.template block<2, 1>(idx, i));
}
//}
for (int i = 0; i < n; i++)
{
L_.template block<1, 2>(i, idx) = (L_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_.template block<1, 2>(i, idx) = (P_.template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
// if(n > dof_Measurement)
// {
// Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
// h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;
// /*
// h_x_cur.col(0) = h_x_.col(0);
// h_x_cur.col(1) = h_x_.col(1);
// h_x_cur.col(2) = h_x_.col(2);
// h_x_cur.col(3) = h_x_.col(3);
// h_x_cur.col(4) = h_x_.col(4);
// h_x_cur.col(5) = h_x_.col(5);
// h_x_cur.col(6) = h_x_.col(6);
// h_x_cur.col(7) = h_x_.col(7);
// h_x_cur.col(8) = h_x_.col(8);
// h_x_cur.col(9) = h_x_.col(9);
// h_x_cur.col(10) = h_x_.col(10);
// h_x_cur.col(11) = h_x_.col(11);
// */
// P_ = L_ - K_*h_x_cur * P_;
// }
// else
//{
P_ = L_ - K_x.template block<n, 12>(0, 0) * P_.template block<12, n>(0, 0);
//}
solve_time += omp_get_wtime() - solve_start;
return;
}
solve_time += omp_get_wtime() - solve_start;
}
}
void change_x(state &input_state)
{
x_ = input_state;
if ((!x_.vect_state.size()) && (!x_.SO3_state.size()) && (!x_.S2_state.size()))
{
x_.build_S2_state();
x_.build_SO3_state();
x_.build_vect_state();
}
}
// 修改P_的状态
void change_P(cov &input_cov)
{
P_ = input_cov;
}
// 获得x_的状态
const state &get_x() const
{
return x_;
}
const cov &get_P() const
{
return P_;
}
private:
state x_;
measurement m_;
cov P_;
spMt l_;
spMt f_x_1;
spMt f_x_2;
cov F_x1 = cov::Identity();
cov F_x2 = cov::Identity();
cov L_ = cov::Identity();
processModel *f;
processMatrix1 *f_x;
processMatrix2 *f_w;
measurementModel *h;
measurementMatrix1 *h_x;
measurementMatrix2 *h_v;
measurementModel_dyn *h_dyn;
measurementMatrix1_dyn *h_x_dyn;
measurementMatrix2_dyn *h_v_dyn;
measurementModel_share *h_share;
measurementModel_dyn_share *h_dyn_share;
int maximum_iter = 0;
scalar_type limit[n];
template <typename T>
T check_safe_update(T _temp_vec)
{
T temp_vec = _temp_vec;
if (std::isnan(temp_vec(0, 0)))
{
temp_vec.setZero();
return temp_vec;
}
double angular_dis = temp_vec.block(0, 0, 3, 1).norm() * 57.3;
double pos_dis = temp_vec.block(3, 0, 3, 1).norm();
if (angular_dis >= 20 || pos_dis > 1)
{
printf("Angular dis = %.2f, pos dis = %.2f\r\n", angular_dis, pos_dis);
temp_vec.setZero();
}
return temp_vec;
}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace esekfom
#endif // ESEKFOM_EKF_HPP
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp
================================================
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Author: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef __MEKFOM_UTIL_HPP__
#define __MEKFOM_UTIL_HPP__
#include <Eigen/Core>
#include "../mtk/src/mtkmath.hpp"
namespace esekfom {
template <typename T1, typename T2>
class is_same {
public:
operator bool() {
return false;
}
};
template<typename T1>
class is_same<T1, T1> {
public:
operator bool() {
return true;
}
};
template <typename T>
class is_double {
public:
operator bool() {
return false;
}
};
template<>
class is_double<double> {
public:
operator bool() {
return true;
}
};
template<typename T>
static T
id(const T &x)
{
return x;
}
} // namespace esekfom
#endif // __MEKFOM_UTIL_HPP__
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp
================================================
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/build_manifold.hpp
* @brief Macro to automatically construct compound manifolds.
*
*/
#ifndef MTK_AUTOCONSTRUCT_HPP_
#define MTK_AUTOCONSTRUCT_HPP_
#include <vector>
#include <boost/preprocessor/seq.hpp>
#include <boost/preprocessor/cat.hpp>
#include <Eigen/Core>
#include "src/SubManifold.hpp"
#include "startIdx.hpp"
#ifndef PARSED_BY_DOXYGEN
//////// internals //////
#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple
#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries))
#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)
#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type()
#define MTK_CONSTRUCTOR_COPY( type, id) id(id)
#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale);
#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale);
#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id);
#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);}
#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);}
#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);}
#define MTK_OSTREAM( type, id) << __var.id << " "
#define MTK_ISTREAM( type, id) >> __var.id
#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));}
#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));}
#define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));}
#define MTK_SUBVARLIST(seq, S2state, SO3state) \
BOOST_PP_FOR_1( \
( \
BOOST_PP_SEQ_SIZE(seq), \
BOOST_PP_SEQ_HEAD(seq), \
BOOST_PP_SEQ_TAIL(seq) (~), \
0,\
0,\
S2state,\
SO3state ),\
MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT)
#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \
MTK::SubManifold<type, dof, dim> id;
#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \
MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \
enum {DOF = type::DOF + dof}; \
enum {DIM = type::DIM+dim}; \
typedef type::scalar scalar;
#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state
#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \
MTK_APPLY_MACRO_ON_TUPLE(~, \
BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \
( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state))
#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state
//! this used to be BOOST_PP_TUPLE_ELEM_4_0:
#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a
#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state
#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \
BOOST_PP_DEC(len), \
BOOST_PP_SEQ_HEAD(seq), \
BOOST_PP_SEQ_TAIL(seq), \
dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\
dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\
S2state,\
SO3state)
#endif /* not PARSED_BY_DOXYGEN */
/**
* Construct a manifold.
* @param name is the class-name of the manifold,
* @param entries is the list of sub manifolds
*
* Entries must be given in a list like this:
* @code
* typedef MTK::trafo<MTK::SO3<double> > Pose;
* typedef MTK::vect<double, 3> Vec3;
* MTK_BUILD_MANIFOLD(imu_state,
* ((Pose, pose))
* ((Vec3, vel))
* ((Vec3, acc_bias))
* )
* @endcode
* Whitespace is optional, but the double parentheses are necessary.
* Construction is done entirely in preprocessor.
* After construction @a name is also a manifold. Its members can be
* accessed by names given in @a entries.
*
* @note Variable types are not allowed to have commas, thus types like
* @c vect<double, 3> need to be typedef'ed ahead.
*/
#define MTK_BUILD_MANIFOLD(name, entries) \
struct name { \
typedef name self; \
std::vector<std::pair<int, int> > S2_state;\
std::vector<std::pair<int, int> > SO3_state;\
std::vector<std::pair<std::pair<int, int>, int> > vect_state;\
MTK_SUBVARLIST(entries, S2_state, SO3_state) \
name ( \
MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \
) : \
MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\
int getDOF() const { return DOF; } \
void boxplus(const MTK::vectview<const scalar, DOF> & __vec, scalar __scale = 1 ) { \
MTK_TRANSFORM(MTK_BOXPLUS, entries)\
} \
void oplus(const MTK::vectview<const scalar, DIM> & __vec, scalar __scale = 1 ) { \
MTK_TRANSFORM(MTK_OPLUS, entries)\
} \
void boxminus(MTK::vectview<scalar,DOF> __res, const name& __oth) const { \
MTK_TRANSFORM(MTK_BOXMINUS, entries)\
} \
friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \
return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \
} \
void build_S2_state(){\
MTK_TRANSFORM(MTK_S2_state, entries)\
}\
void build_vect_state(){\
MTK_TRANSFORM(MTK_vect_state, entries)\
}\
void build_SO3_state(){\
MTK_TRANSFORM(MTK_SO3_state, entries)\
}\
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res, int idx) {\
MTK_TRANSFORM(MTK_S2_hat, entries)\
}\
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res, int idx) {\
MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\
}\
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, Eigen::Matrix<scalar, 2, 1> dx, int idx) {\
MTK_TRANSFORM(MTK_S2_Mx, entries)\
}\
friend std::istream& operator>>(std::istream& __is, name& __var){ \
return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \
} \
};
#endif /*MTK_AUTOCONSTRUCT_HPP_*/
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp
================================================
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/src/SubManifold.hpp
* @brief Defines the SubManifold class
*/
#ifndef SUBMANIFOLD_HPP_
#define SUBMANIFOLD_HPP_
#include "vectview.hpp"
namespace MTK {
/**
* @ingroup SubManifolds
* Helper class for compound manifolds.
* This class wraps a manifold T and provides an enum IDX refering to the
* index of the SubManifold within the compound manifold.
*
* Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds".
*
* @tparam T The manifold type of the sub-type
* @tparam idx The index of the sub-type within the compound manifold
*/
template<class T, int idx, int dim>
struct SubManifold : public T
{
enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ };
//! manifold type
typedef T type;
//! Construct from derived type
template<class X>
explicit
SubManifold(const X& t) : T(t) {};
//! Construct from internal type
//explicit
SubManifold(const T& t) : T(t) {};
//! inherit assignment operator
using T::operator=;
};
} // namespace MTK
#endif /* SUBMANIFOLD_HPP_ */
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp
================================================
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/src/mtkmath.hpp
* @brief several math utility functions.
*/
#ifndef MTKMATH_H_
#define MTKMATH_H_
#include <cmath>
#include <boost/math/tools/precision.hpp>
#include "../types/vect.hpp"
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
namespace MTK {
namespace internal {
template<class Manifold>
struct traits {
typedef typename Manifold::scalar scalar;
enum {DOF = Manifold::DOF};
typedef vect<DOF, scalar> vectorized_type;
typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
};
template<>
struct traits<float> : traits<Scalar<float> > {};
template<>
struct traits<double> : traits<Scalar<double> > {};
} // namespace internal
/**
* \defgroup MTKMath Mathematical helper functions
*/
//@{
//! constant @f$ \pi @f$
const double pi = M_PI;
template<class scalar> inline scalar tolerance();
template<> inline float tolerance<float >() { return 1e-5f; }
template<> inline double tolerance<double>() { return 1e-11; }
/**
* normalize @a x to @f$[-bound, bound] @f$.
*
* result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
*/
template<class scalar>
inline scalar normalize(scalar x, scalar bound){ //not used
if(std::fabs(x) <= bound) return x;
int r = (int)(x *(scalar(1.0)/ bound));
return x - ((r + (r>>31) + 1) & ~1)*bound;
}
/**
* Calculate cosine and sinc of sqrt(x2).
* @param x2 the squared angle must be non-negative
* @return a pair containing cos and sinc of sqrt(x2)
*/
template<class scalar>
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
using std::sqrt;
using std::cos;
using std::sin;
static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
static scalar const taylor_2_bound = sqrt(taylor_0_bound);
static scalar const taylor_n_bound = sqrt(taylor_2_bound);
assert(x2>=0 && "argument must be non-negative");
// FIXME check if bigger bounds are possible
if(x2>=taylor_n_bound) {
// slow fall-back solution
scalar x = sqrt(x2);
return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
}
// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
// TODO Find optimal coefficients using Remez algorithm
static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
scalar cosi = 1., sinc=1;
scalar term = -1/2. * x2;
for(int i=0; i<3; ++i) {
cosi += term;
term *= inv[2*i];
sinc += term;
term *= -inv[2*i+1] * x2;
}
return std::make_pair(cosi, sinc);
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
Eigen::Matrix<typename Base::scalar, 3, 3> res;
res << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return res;
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
Eigen::Matrix<typename Base::scalar, 3, 3> res;
if(v.norm() > MTK::tolerance<typename Base::scalar>())
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
}
else
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
}
return res;
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
Eigen::Matrix<typename Base::scalar, 3, 3> res;
if(v.norm() > MTK::tolerance<typename Base::scalar>())
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() - 0.5 * hat<Base>(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm();
}
else
{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
}
return res;
}
template<typename scalar>
Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v, scalar length)
{
Eigen::Matrix<scalar, 2, 3> res;
scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]);
if(norm < MTK::tolerance<scalar>()){
res = Eigen::Matrix<scalar, 2, 3>::Zero();
res(0, 1) = 1;
res(1, 2) = 1;
res /= length;
}
else{
res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0,
-v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm);
res /= length;
}
}
template<typename Base>
Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
Eigen::Matrix<typename Base::scalar, 3, 3> res;
double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
double norm = std::sqrt(squaredNorm);
if(norm < MTK::tolerance<typename Base::scalar>()){
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity();
}
else{
res = Eigen::Matrix<typename Base::scalar, 3, 3>::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v);
}
return res;
}
template<class scalar, int n>
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
scalar norm2 = vec.squaredNorm();
std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
scalar mult = cos_sinc.second * scale;
result = mult * vec;
return cos_sinc.first;
}
/**
* Inverse function to @c exp.
*
* @param result @c vectview to the result
* @param w scalar part of input
* @param vec vector part of input
* @param scale scale result by this value
* @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
*/
template<class scalar, int n>
void log(vectview<scalar, n> result,
const scalar &w, const vectview<const scalar, n> vec,
const scalar &scale, bool plus_minus_periodicity)
{
// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
scalar nv = vec.norm();
if(nv < tolerance<scalar>()) {
if(!plus_minus_periodicity && w < 0) {
// find the maximal entry:
int i;
nv = vec.cwiseAbs().maxCoeff(&i);
result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
return;
}
nv = tolerance<scalar>();
}
scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );
result = s * vec;
}
} // namespace MTK
#endif /* MTKMATH_H_ */
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp
================================================
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/src/vectview.hpp
* @brief Wrapper class around a pointer used as interface for plain vectors.
*/
#ifndef VECTVIEW_HPP_
#define VECTVIEW_HPP_
#include <Eigen/Core>
namespace MTK {
/**
* A view to a vector.
* Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions.
* The dimension of the vector is given as template parameter and type-checked when used in expressions.
* Data has to be modifiable.
*
* @tparam scalar Scalar type of the vector.
* @tparam dim Dimension of the vector.
*
* @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct
*/
namespace internal {
template<class Base, class T1, class T2>
struct CovBlock {
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF>, T1::DOF, T2::DOF> ConstType;
};
template<class Base, class T1, class T2>
struct CovBlock_ {
typedef typename Eigen::Block<Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM>, T1::DIM, T2::DIM> ConstType;
};
template<typename Base1, typename Base2, typename T1, typename T2>
struct CrossCovBlock {
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF>, T1::DOF, T2::DOF> ConstType;
};
template<typename Base1, typename Base2, typename T1, typename T2>
struct CrossCovBlock_ {
typedef typename Eigen::Block<Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> Type;
typedef typename Eigen::Block<const Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM>, T1::DIM, T2::DIM> ConstType;
};
template<class scalar, int dim>
struct VectviewBase {
typedef Eigen::Matrix<scalar, dim, 1> matrix_type;
typedef typename matrix_type::MapType Type;
typedef typename matrix_type::ConstMapType ConstType;
};
template<class T>
struct UnalignedType {
typedef T type;
};
}
template<class scalar, int dim>
class vectview : public internal::VectviewBase<scalar, dim>::Type {
typedef internal::VectviewBase<scalar, dim> VectviewBase;
public:
//! plain matrix type
typedef typename VectviewBase::matrix_type matrix_type;
//! base type
typedef typename VectviewBase::Type base;
//! construct from pointer
explicit
vectview(scalar* data, int dim_=dim) : base(data, dim_) {}
//! construct from plain matrix
vectview(matrix_type& m) : base(m.data(), m.size()) {}
//! construct from another @c vectview
vectview(const vectview &v) : base(v) {}
//! construct from Eigen::Block:
template<class Base>
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0), block.size()) {}
template<class Base, bool PacketAccess>
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0), block.size()) {}
//! inherit assignment operator
using base::operator=;
//! data pointer
scalar* data() {return const_cast<scalar*>(base::data());}
};
/**
* @c const version of @c vectview.
* Compared to @c Eigen::Map this implementation is const correct, i.e.,
* data will not be modifiable using this view.
*
* @tparam scalar Scalar type of the vector.
* @tparam dim Dimension of the vector.
*
* @sa vectview
*/
template<class scalar, int dim>
class vectview<const scalar, dim> : public internal::VectviewBase<scalar, dim>::ConstType {
typedef internal::VectviewBase<scalar, dim> VectviewBase;
public:
//! plain matrix type
typedef typename VectviewBase::matrix_type matrix_type;
//! base type
typedef typename VectviewBase::ConstType base;
//! construct from const pointer
explicit
vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {}
//! construct from column vector
template<int options>
vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.data()) {}
//! construct from row vector
template<int options, int phony>
vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : base(m.data()) {}
//! construct from another @c vectview
vectview(vectview<scalar, dim> x) : base(x.data()) {}
//! construct from base
vectview(const base &x) : base(x) {}
/**
* Construct from Block
* @todo adapt this, when Block gets const-correct
*/
template<class Base>
vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef(0)) {}
template<class Base, bool PacketAccess>
vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&block.coeffRef(0)) {}
};
} // namespace MTK
#endif /* VECTVIEW_HPP_ */
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/startIdx.hpp
================================================
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/startIdx.hpp
* @brief Tools to access sub-elements of compound manifolds.
*/
#ifndef GET_START_INDEX_H_
#define GET_START_INDEX_H_
#include <Eigen/Core>
#include "src/SubManifold.hpp"
#include "src/vectview.hpp"
namespace MTK {
/**
* \defgroup SubManifolds Accessing Submanifolds
* For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers
* can be used to get sub-vectors or matrix-blocks of a corresponding big matrix.
* E.g. for a type @a pose consisting of @a orient and @a trans the member pointers
* @c &pose::orient and @c &pose::trans give all required information and are still
* valid if the base type gets extended or the actual types of @a orient and @a trans
* change (e.g. from 2D to 3D).
*
* @todo Maybe require manifolds to typedef MatrixType and VectorType, etc.
*/
//@{
/**
* Determine the index of a sub-variable within a compound variable.
*/
template<class Base, class T, int idx, int dim>
int getStartIdx( MTK::SubManifold<T, idx, dim> Base::*)
{
return idx;
}
template<class Base, class T, int idx, int dim>
int getStartIdx_( MTK::SubManifold<T, idx, dim> Base::*)
{
return dim;
}
/**
* Determine the degrees of freedom of a sub-variable within a compound variable.
*/
template<class Base, class T, int idx, int dim>
int getDof( MTK::SubManifold<T, idx, dim> Base::*)
{
return T::DOF;
}
template<class Base, class T, int idx, int dim>
int getDim( MTK::SubManifold<T, idx, dim> Base::*)
{
return T::DIM;
}
/**
* set the diagonal elements of a covariance matrix corresponding to a sub-variable
*/
template<class Base, class T, int idx, int dim>
void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
{
cov.diagonal().template segment<T::DOF>(idx).setConstant(val);
}
template<class Base, class T, int idx, int dim>
void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
MTK::SubManifold<T, idx, dim> Base::*, const typename Base::scalar &val)
{
cov.diagonal().template segment<T::DIM>(dim).setConstant(val);
}
/**
* Get the subblock of corresponding to two members, i.e.
* \code
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
* MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression;
* MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans();
* \endcode
* lets you modify mixed covariance entries in a bigger covariance matrix.
*/
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
typename MTK::internal::CovBlock<Base, T1, T2>::Type
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
{
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
}
template<class Base, class T1, int idx1, int dim1, class T2, int idx2, int dim2>
typename MTK::internal::CovBlock_<Base, T1, T2>::Type
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
MTK::SubManifold<T1, idx1, dim1> Base::*, MTK::SubManifold<T2, idx2, dim2> Base::*)
{
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
}
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type
subblock(Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
{
return cov.template block<T1::DOF, T2::DOF>(idx1, idx2);
}
template<typename Base1, typename Base2, typename T1, typename T2, int idx1, int idx2, int dim1, int dim2>
typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type
subblock_(Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> &cov, MTK::SubManifold<T1, idx1, dim1> Base1::*, MTK::SubManifold<T2, idx2, dim2> Base2::*)
{
return cov.template block<T1::DIM, T2::DIM>(dim1, dim2);
}
/**
* Get the subblock of corresponding to a member, i.e.
* \code
* Eigen::Matrix<double, Pose::DOF, Pose::DOF> m;
* MTK::subblock(m, &Pose::orient) = some_expression;
* \endcode
* lets you modify covariance entries in a bigger covariance matrix.
*/
template<class Base, class T, int idx, int dim>
typename MTK::internal::CovBlock_<Base, T, T>::Type
subblock_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> &cov,
MTK::SubManifold<T, idx, dim> Base::*)
{
return cov.template block<T::DIM, T::DIM>(dim, dim);
}
template<class Base, class T, int idx, int dim>
typename MTK::internal::CovBlock<Base, T, T>::Type
subblock(Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> &cov,
MTK::SubManifold<T, idx, dim> Base::*)
{
return cov.template block<T::DOF, T::DOF>(idx, idx);
}
template<typename Base>
class get_cov {
public:
typedef Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> type;
typedef const Eigen::Matrix<typename Base::scalar, Base::DOF, Base::DOF> const_type;
};
template<typename Base>
class get_cov_ {
public:
typedef Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> type;
typedef const Eigen::Matrix<typename Base::scalar, Base::DIM, Base::DIM> const_type;
};
template<typename Base1, typename Base2>
class get_cross_cov {
public:
typedef Eigen::Matrix<typename Base1::scalar, Base1::DOF, Base2::DOF> type;
typedef const type const_type;
};
template<typename Base1, typename Base2>
class get_cross_cov_ {
public:
typedef Eigen::Matrix<typename Base1::scalar, Base1::DIM, Base2::DIM> type;
typedef const type const_type;
};
template<class Base, class T, int idx, int dim>
vectview<typename Base::scalar, T::DIM>
subvector_impl_(vectview<typename Base::scalar, Base::DIM> vec, SubManifold<T, idx, dim> Base::*)
{
return vec.template segment<T::DIM>(dim);
}
template<class Base, class T, int idx, int dim>
vectview<typename Base::scalar, T::DOF>
subvector_impl(vectview<typename Base::scalar, Base::DOF> vec, SubManifold<T, idx, dim> Base::*)
{
return vec.template segment<T::DOF>(idx);
}
/**
* Get the subvector corresponding to a sub-manifold from a bigger vector.
*/
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<Scalar, T::DIM>
subvector_(vectview<Scalar, BaseDIM> vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl_(vec, ptr);
}
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<Scalar, T::DOF>
subvector(vectview<Scalar, BaseDOF> vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(vec, ptr);
}
/**
* @todo This should be covered already by subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
*/
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<Scalar, T::DOF>
subvector(Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(vectview<Scalar, BaseDOF>(vec), ptr);
}
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<Scalar, T::DIM>
subvector_(Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl_(vectview<Scalar, BaseDIM>(vec), ptr);
}
template<class Scalar, int BaseDIM, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DIM>
subvector_(const Eigen::Matrix<Scalar, BaseDIM, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl_(vectview<const Scalar, BaseDIM>(vec), ptr);
}
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DOF>
subvector(const Eigen::Matrix<Scalar, BaseDOF, 1>& vec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(vectview<const Scalar, BaseDOF>(vec), ptr);
}
/**
* const version of subvector(vectview<typename Base::scalar,Base::DOF> vec,SubManifold<T,idx> Base::*)
*/
template<class Base, class T, int idx, int dim>
vectview<const typename Base::scalar, T::DOF>
subvector_impl(const vectview<const typename Base::scalar, Base::DOF> cvec, SubManifold<T, idx, dim> Base::*)
{
return cvec.template segment<T::DOF>(idx);
}
template<class Base, class T, int idx, int dim>
vectview<const typename Base::scalar, T::DIM>
subvector_impl_(const vectview<const typename Base::scalar, Base::DIM> cvec, SubManifold<T, idx, dim> Base::*)
{
return cvec.template segment<T::DIM>(dim);
}
template<class Scalar, int BaseDOF, class Base, class T, int idx, int dim>
vectview<const Scalar, T::DOF>
subvector(const vectview<const Scalar, BaseDOF> cvec, SubManifold<T, idx, dim> Base::* ptr)
{
return subvector_impl(cvec, ptr);
}
} // namespace MTK
#endif // GET_START_INDEX_H_
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/S2.hpp
================================================
// This is a NEW implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/types/S2.hpp
* @brief Unit vectors on the sphere, or directions in 3D.
*/
#ifndef S2_H_
#define S2_H_
#include "vect.hpp"
#include "SOn.hpp"
#include "../src/mtkmath.hpp"
namespace MTK {
/**
* Manifold representation of @f$ S^2 @f$.
* Used for unit vectors on the sphere or directions in 3D.
*
* @todo add conversions from/to polar angles?
*/
template<class _scalar = double, int den = 1, int num = 1, int S2_typ = 3>
struct S2 {
typedef _scalar scalar;
typedef vect<3, scalar> vect_type;
typedef SO3<scalar> SO3_type;
typedef typename vect_type::base vec3;
scalar length = scalar(den)/scalar(num);
enum {DOF=2, TYP = 1, DIM = 3};
//private:
/**
* Unit vector on the sphere, or vector pointing in a direction
*/
vect_type vec;
public:
S2() {
if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1));
if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0);
if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0);
}
S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) {
vec.normalize();
vec = vec * length;
}
S2(const vect_type &_vec) : vec(_vec) {
vec.normalize();
vec = vec * length;
}
void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1)
{
SO3_type res;
res.w() = MTK::exp<scalar, 3>(res.vec(), delta, scalar(scale/2));
vec = res.toRotationMatrix() * vec;
}
void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale=1) {
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
vect_type Bu = Bx*delta;SO3_type res;
res.w() = MTK::exp<scalar, 3>(res.vec(), Bu, scalar(scale/2));
vec = res.toRotationMatrix() * vec;
}
void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, num, S2_typ>& other) const {
scalar v_sin = (MTK::hat(vec)*other.vec).norm();
scalar v_cos = vec.transpose() * other.vec;
scalar theta = std::atan2(v_sin, v_cos);
if(v_sin < MTK::tolerance<scalar>())
{
if(std::fabs(theta) > MTK::tolerance<scalar>() )
{
res[0] = 3.1415926;
res[1] = 0;
}
else{
res[0] = 0;
res[1] = 0;
}
}
else
{
S2<scalar, den, num, S2_typ> other_copy = other;
Eigen::Matrix<scalar, 3, 2>Bx;
other_copy.S2_Bx(Bx);
res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec;
}
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
Eigen::Matrix<scalar, 3, 3> skew_vec;
skew_vec << scalar(0), -vec[2], vec[1],
vec[2], scalar(0), -vec[0],
-vec[1], vec[0], scalar(0);
res = skew_vec;
}
void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res)
{
if(S2_typ == 3)
{
if(vec[2] + length > tolerance<scalar>())
{
res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]),
-vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]),
-vec[0], -vec[1];
res /= length;
}
else
{
res = Eigen::Matrix<scalar, 3, 2>::Zero();
res(1, 1) = -1;
res(2, 0) = 1;
}
}
else if(S2_typ == 2)
{
if(vec[1] + length > tolerance<scalar>())
{
res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]),
-vec[0], -vec[2],
-vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]);
res /= length;
}
else
{
res = Eigen::Matrix<scalar, 3, 2>::Zero();
res(1, 1) = -1;
res(2, 0) = 1;
}
}
else
{
if(vec[0] + length > tolerance<scalar>())
{
res << -vec[1], -vec[2],
length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]),
-vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]);
res /= length;
}
else
{
res = Eigen::Matrix<scalar, 3, 2>::Zero();
res(1, 1) = -1;
res(2, 0) = 1;
}
}
}
void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2_typ>& subtrahend)
{
if((vec+subtrahend.vec).norm() > tolerance<scalar>())
{
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
if((vec-subtrahend.vec).norm() > tolerance<scalar>())
{
scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm();
scalar v_cos = vec.transpose() * subtrahend.vec;
res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length));
}
else
{
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
}
}
else
{
std::cerr << "No N(x, y) for x=-y" << std::endl;
std::exit(100);
}
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
res = 1/length/length*Bx.transpose()*MTK::hat(vec);
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
Eigen::Matrix<scalar, 3, 2> Bx;
S2_Bx(Bx);
if(delta.norm() < tolerance<scalar>())
{
res = -MTK::hat(vec)*Bx;
}
else{
vect_type Bu = Bx*delta;
SO3_type exp_delta;
exp_delta.w() = MTK::exp<scalar, 3>(exp_delta.vec(), Bu, scalar(1/2));
res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx;
}
}
operator const vect_type&() const{
return vec;
}
const vect_type& get_vect() const {
return vec;
}
friend S2<scalar, den, num, S2_typ> operator*(const SO3<scalar>& rot, const S2<scalar, den, num, S2_typ>& dir)
{
S2<scalar, den, num, S2_typ> ret;
ret.vec = rot * dir.vec;
return ret;
}
scalar operator[](int idx) const {return vec[idx]; }
friend std::ostream& operator<<(std::ostream &os, const S2<scalar, den, num, S2_typ>& vec){
return os << vec.vec.transpose() << " ";
}
friend std::istream& operator>>(std::istream &is, S2<scalar, den, num, S2_typ>& vec){
for(int i=0; i<3; ++i)
is >> vec.vec[i];
vec.vec.normalize();
vec.vec = vec.vec * vec.length;
return is;
}
};
} // namespace MTK
#endif /*S2_H_*/
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp
================================================
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/types/SOn.hpp
* @brief Standard Orthogonal Groups i.e.\ rotatation groups.
*/
#ifndef SON_H_
#define SON_H_
#include <Eigen/Geometry>
#include "vect.hpp"
#include "../src/mtkmath.hpp"
namespace MTK {
/**
* Two-dimensional orientations represented as scalar.
* There is no guarantee that the representing scalar is within any interval,
* but the result of boxminus will always have magnitude @f$\le\pi @f$.
*/
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO2 : public Eigen::Rotation2D<_scalar> {
enum {DOF = 1, DIM = 2, TYP = 3};
typedef _scalar scalar;
typedef Eigen::Rotation2D<scalar> base;
typedef vect<DIM, scalar, Options> vect_type;
//! Construct from angle
SO2(const scalar& angle = 0) : base(angle) { }
//! Construct from Eigen::Rotation2D
SO2(const base& src) : base(src) {}
/**
* Construct from 2D vector.
* Resulting orientation will rotate the first unit vector to point to vec.
*/
SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};
//! Calculate @c this->inverse() * @c r
SO2 operator%(const base &r) const {
return base::inverse() * r;
}
//! Calculate @c this->inverse() * @c r
template<class Derived>
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
return base::inverse() * vec;
}
//! Calculate @c *this * @c r.inverse()
SO2 operator/(const SO2 &r) const {
return *this * r.inverse();
}
//! Gets the angle as scalar.
operator scalar() const {
return base::angle();
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
//! @name Manifold requirements
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
base::angle() += scale * vec[0];
}
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
base::angle() += scale * vec[0];
}
void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
}
friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
return is >> ang.angle();
}
};
/**
* Three-dimensional orientations represented as Quaternion.
* It is assumed that the internal Quaternion always stays normalized,
* should this not be the case, call inherited member function @c normalize().
*/
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO3 : public Eigen::Quaternion<_scalar, Options> {
enum {DOF = 3, DIM = 3, TYP = 2};
typedef _scalar scalar;
typedef Eigen::Quaternion<scalar, Options> base;
typedef Eigen::Quaternion<scalar> Quaternion;
typedef vect<DIM, scalar, Options> vect_type;
//! Calculate @c this->inverse() * @c r
template<class OtherDerived> EIGEN_STRONG_INLINE
Quaternion operator%(const Eigen::QuaternionBase<OtherDerived> &r) const {
return base::conjugate() * r;
}
//! Calculate @c this->inverse() * @c r
template<class Derived>
vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
return base::conjugate() * vec;
}
//! Calculate @c this * @c r.conjugate()
template<class OtherDerived> EIGEN_STRONG_INLINE
Quaternion operator/(const Eigen::QuaternionBase<OtherDerived> &r) const {
return *this * r.conjugate();
}
/**
* Construct from real part and three imaginary parts.
* Quaternion is normalized after construction.
*/
SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) {
base::normalize();
}
/**
* Construct from Eigen::Quaternion.
* @note Non-normalized input may result result in spurious behavior.
*/
SO3(const base& src = base::Identity()) : base(src) {}
/**
* Construct from rotation matrix.
* @note Invalid rotation matrices may lead to spurious behavior.
*/
template<class Derived>
SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
/**
* Construct from arbitrary rotation type.
* @note Invalid rotation matrices may lead to spurious behavior.
*/
template<class Derived>
SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}
//! @name Manifold requirements
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
SO3 delta = exp(vec, scale);
*this = *this * delta;
}
void boxminus(MTK::vectview<scalar, DOF> res, const SO3<scalar>& other) const {
res = SO3::log(other.conjugate() * *this);
}
//}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
SO3 delta = exp(vec, scale);
*this = *this * delta;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const SO3<scalar, Options>& q){
return os << q.coeffs().transpose() << " ";
}
friend std::istream& operator>>(std::istream &is, SO3<scalar, Options>& q){
vect<4,scalar> coeffs;
is >> coeffs;
q.coeffs() = coeffs.normalized();
return is;
}
//! @name Helper functions
//{
/**
* Calculate the exponential map. In matrix terms this would correspond
* to the Rodrigues formula.
*/
// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
// static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
static SO3 exp(const Eigen::Matrix<scalar, 3, 1>& dvec, scalar scale = 1){
SO3 res;
res.w() = MTK::exp<scalar, 3>(res.vec(), dvec, scalar(scale/2));
return res;
}
/**
* Calculate the inverse of @c exp.
* Only guarantees that <code>exp(log(x)) == x </code>
*/
static typename base::Vector3 log(const SO3 &orient){
typename base::Vector3 res;
MTK::log<scalar, 3>(res, orient.w(), orient.vec(), scalar(2), true);
return res;
}
};
namespace internal {
template<class Scalar, int Options>
struct UnalignedType<SO2<Scalar, Options > >{
typedef SO2<Scalar, Options | Eigen::DontAlign> type;
};
template<class Scalar, int Options>
struct UnalignedType<SO3<Scalar, Options > >{
typedef SO3<Scalar, Options | Eigen::DontAlign> type;
};
} // namespace internal
} // namespace MTK
#endif /*SON_H_*/
================================================
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/vect.hpp
================================================
// This is an advanced implementation of the algorithm described in the
// following paper:
// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds.
// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119
/*
* Copyright (c) 2019--2023, The University of Hong Kong
* All rights reserved.
*
* Modifier: Dongjiao HE <hdj65822@connect.hku.hk>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/*
* Copyright (c) 2008--2011, Universitaet Bremen
* All rights reserved.
*
* Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Universitaet Bremen 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.
*/
/**
* @file mtk/types/vect.hpp
* @brief Basic vectors interpreted as manifolds.
*
* This file also implements a simple wrapper for matrices, for arbitrary scalars
* and for positive scalars.
*/
#ifndef VECT_H_
#define VECT_H_
#include <iosfwd>
#include <iostream>
#include <vector>
#include "../src/vectview.hpp"
namespace MTK {
static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]");
/**
* A simple vector class.
* Implementation is basically a wrapper around Eigen::Matrix with manifold
* requirements added.
*/
template<int D = 3, class _scalar = double, int _Options=Eigen::AutoAlign>
struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {
typedef Eigen::Matrix<_scalar, D, 1, _Options> base;
enum {DOF = D, DIM = D, TYP = 0};
typedef _scalar scalar;
//using base::operator=;
/** Standard constructor. Sets all values to zero. */
vect(const base &src = base::Zero()) : base(src) {}
/** Constructor copying the value of the expression \a other */
template<typename OtherDerived>
EIGEN_STRONG_INLINE vect(const Eigen::DenseBase<OtherDerived>& other) : base(other) {}
/** Construct from memory. */
vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { }
void boxplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
*this += scale * vec;
}
void boxminus(MTK::vectview<scalar, D> res, const vect<D, scalar>& other) const {
res = *this - other;
}
void oplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
*this += scale * vec;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const vect<D, scalar, _Options>& v){
// Eigen sometimes messes with the streams flags, so output manually:
for(int i=0; i<DOF; ++i)
os << v(i) << " ";
return os;
}
friend std::istream& operator>>(std::istream &is, vect<D, scalar, _Options>& v){
char term=0;
is >> std::ws; // skip whitespace
switch(is.peek()) {
case '(': term=')'; is.ignore(1); break;
case '[': term=']'; is.ignore(1); break;
case '{': term='}'; is.ignore(1); break;
default: break;
}
if(D==Eigen::Dynamic) {
assert(term !=0 && "Dynamic vectors must be embraced");
std::vector<scalar> temp;
while(is.good() && is.peek() != term) {
scalar x;
is >> x;
temp.push_back(x);
if(is.peek()==',') is.ignore(1);
}
v = vect::Map(temp.data(), temp.size());
} else
for(int i=0; i<v.size(); ++i){
is >> v[i];
if(is.peek()==',') { // ignore commas between values
is.ignore(1);
}
}
if(term!=0) {
char x;
is >> x;
if(x!=term) {
is.setstate(is.badbit);
// assert(x==term && "start and end bracket do not match!");
}
}
return is;
}
template<int dim>
vectview<scalar, dim> tail(){
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template tail<dim>();
}
template<int dim>
vectview<const scalar, dim> tail() const{
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template tail<dim>();
}
template<int dim>
vectview<scalar, dim> head(){
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template head<dim>();
}
template<int dim>
vectview<const scalar, dim> head() const{
BOOST_STATIC_ASSERT(0< dim && dim <= DOF);
return base::template head<dim>();
}
};
/**
* A simple matrix class.
* Implementation is basically a wrapper around Eigen::Matrix with manifold
* requirements added, i.e., matrix is viewed as a plain vector for that.
*/
template<int M, int N, class _scalar = double, int _Options = Eigen::Matrix<_scalar, M, N>::Options>
struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {
typedef Eigen::Matrix<_scalar, M, N, _Options> base;
enum {DOF = M * N, TYP = 4, DIM=0};
typedef _scalar scalar;
using base::operator=;
/** Standard constructor. Sets all values to zero. */
matrix() {
base::setZero();
}
/** Constructor copying the value of the expression \a other */
template<typename OtherDerived>
EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase<OtherDerived>& other) : base(other) {}
/** Construct from memory. */
matrix(const scalar* src) : base(src) { }
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
*this += scale * base::Map(vec.data());
}
void boxminus(MTK::vectview<scalar, DOF> res, const matrix& other) const {
base::Map(res.data()) = *this - other;
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
*this += scale * base::Map(vec.data());
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::ostream& operator<<(std::ostream &os, const matrix<M, N, scalar, _Options>& mat){
for(int i=0; i<DOF; ++i){
os << mat.data()[i] << " ";
}
return os;
}
friend std::istream& operator>>(std::istream &is, matrix<M, N, scalar, _Options>& mat){
for(int i=0; i<DOF; ++i){
is >> mat.data()[i];
}
return is;
}
};// @todo What if M / N = Eigen::Dynamic?
/**
* A simple scalar type.
*/
template<class _scalar = double>
struct Scalar {
enum {DOF = 1, TYP = 5, DIM=0};
typedef _scalar scalar;
scalar value;
Scalar(const scalar& value = scalar(0)) : value(value) {}
operator const scalar&() const { return value; }
operator scalar&() { return value; }
Scalar& operator=(const scalar& val) { value = val; return *this; }
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
value += scale * vec[0];
}
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
value += scale * vec[0];
}
void boxminus(MTK::vectview<scalar, DOF> res, const Scalar& other) const {
res[0] = *this - other;
}
};
/**
* Positive scalars.
* Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$.
*/
template<class _scalar = double>
struct PositiveScalar {
enum {DOF = 1, TYP = 6, DIM=0};
typedef _scalar scalar;
scalar value;
PositiveScalar(const scalar& value = scalar(1)) : value(value) {
assert(value > scalar(0));
}
operator const scalar&() const { return value; }
PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; }
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
value *= std::exp(scale * vec[0]);
}
void boxminus(MTK::vectview<scalar, DOF> res, const PositiveScalar& other) const {
res[0] = std::log(*this / other);
}
void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
value *= std::exp(scale * vec[0]);
}
void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
{
res = Eigen::Matrix<scalar, 3, 3>::Zero();
}
void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 2, 3>::Zero();
}
void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const scalar, 2> delta)
{
std::cerr << "wrong idx for S2" << std::endl;
std::exit(100);
res = Eigen::Matrix<scalar, 3, 2>::Zero();
}
friend std::istream& operator>>(std::istream &is, PositiveScalar<scalar>& s){
is >> s.value;
assert(s.value > 0);
return is;
}
};
template<class _scalar = double>
struct Complex : public std::complex<_scalar>{
enum {DOF = 2, TYP = 7, DIM=0};
typedef _scalar scalar;
typedef std::compl
gitextract_30l0tz1c/
├── .gitignore
├── FAST-LIO/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── Log/
│ │ ├── fast_lio_time_log_analysis.m
│ │ ├── guide.md
│ │ └── plot.py
│ ├── PCD/
│ │ └── 1
│ ├── README.md
│ ├── config/
│ │ ├── avia.yaml
│ │ ├── horizon.yaml
│ │ ├── ouster64.yaml
│ │ ├── ouster64_mulran.yaml
│ │ └── velodyne.yaml
│ ├── include/
│ │ ├── Exp_mat.h
│ │ ├── IKFoM_toolkit/
│ │ │ ├── esekfom/
│ │ │ │ ├── esekfom.hpp
│ │ │ │ └── util.hpp
│ │ │ └── mtk/
│ │ │ ├── build_manifold.hpp
│ │ │ ├── src/
│ │ │ │ ├── SubManifold.hpp
│ │ │ │ ├── mtkmath.hpp
│ │ │ │ └── vectview.hpp
│ │ │ ├── startIdx.hpp
│ │ │ └── types/
│ │ │ ├── S2.hpp
│ │ │ ├── SOn.hpp
│ │ │ ├── vect.hpp
│ │ │ └── wrapped_cv_mat.hpp
│ │ ├── common_lib.h
│ │ ├── ikd-Tree/
│ │ │ ├── README.md
│ │ │ ├── ikd_Tree.cpp
│ │ │ └── ikd_Tree.h
│ │ ├── matplotlibcpp.h
│ │ ├── so3_math.h
│ │ └── use-ikfom.hpp
│ ├── launch/
│ │ ├── gdb_debug_example.launch
│ │ ├── mapping_avia.launch
│ │ ├── mapping_horizon.launch
│ │ ├── mapping_ouster64.launch
│ │ ├── mapping_ouster64_mulran.launch
│ │ └── mapping_velodyne.launch
│ ├── msg/
│ │ └── Pose6D.msg
│ ├── package.xml
│ ├── rviz_cfg/
│ │ ├── .gitignore
│ │ └── loam_livox.rviz
│ └── src/
│ ├── IMU_Processing.hpp
│ ├── laserMapping.cpp
│ ├── preprocess.cpp
│ └── preprocess.h
├── README.md
└── SC-PGO/
├── CMakeLists.txt
├── README.md
├── docker/
│ ├── Dockerfile
│ ├── Makefile
│ └── run.sh
├── include/
│ ├── aloam_velodyne/
│ │ ├── common.h
│ │ └── tic_toc.h
│ └── scancontext/
│ ├── KDTreeVectorOfVectorsAdaptor.h
│ ├── Scancontext.cpp
│ ├── Scancontext.h
│ └── nanoflann.hpp
├── launch/
│ ├── aloam_mulran.launch
│ ├── aloam_velodyne_HDL_32.launch
│ ├── aloam_velodyne_HDL_64.launch
│ ├── aloam_velodyne_VLP_16.launch
│ ├── fastlio_ouster64.launch
│ └── kitti_helper.launch
├── package.xml
├── rviz_cfg/
│ └── aloam_velodyne.rviz
├── src/
│ ├── kittiHelper.cpp
│ ├── laserMapping.cpp
│ ├── laserOdometry.cpp
│ ├── laserPosegraphOptimization.cpp
│ ├── lidarFactor.hpp
│ └── scanRegistration.cpp
└── utils/
├── python/
│ ├── bone_table.npy
│ ├── jet_table.npy
│ ├── makeMergedMap.py
│ └── pypcdMyUtils.py
└── sample_data/
└── KAIST03/
├── Scans/
│ ├── 000000.pcd
│ ├── 000001.pcd
│ ├── 000002.pcd
│ ├── 000003.pcd
│ ├── 000004.pcd
│ ├── 000005.pcd
│ ├── 000006.pcd
│ ├── 000007.pcd
│ ├── 000008.pcd
│ ├── 000009.pcd
│ ├── 000010.pcd
│ ├── 000011.pcd
│ ├── 000012.pcd
│ ├── 000013.pcd
│ ├── 000014.pcd
│ ├── 000015.pcd
│ ├── 000016.pcd
│ ├── 000017.pcd
│ ├── 000018.pcd
│ ├── 000019.pcd
│ └── 000020.pcd
├── optimized_poses.txt
└── times.txt
SYMBOL INDEX (536 symbols across 31 files)
FILE: FAST-LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp
type esekfom (line 56) | namespace esekfom
type share_datastruct (line 65) | struct share_datastruct
type dyn_share_datastruct (line 79) | struct dyn_share_datastruct
type dyn_runtime_share_datastruct (line 94) | struct dyn_runtime_share_datastruct
class esekf (line 105) | class esekf
method esekf (line 139) | esekf(const state &x = state(),
method init (line 153) | void init(processModel f_in, processMatrix1 f_x_in, processMatrix2 f...
method init_dyn (line 175) | void init_dyn(processModel f_in, processMatrix1 f_x_in, processMatri...
method init_dyn_runtime (line 197) | void init_dyn_runtime(processModel f_in, processMatrix1 f_x_in, proc...
method init_share (line 219) | void init_share(processModel f_in, processMatrix1 f_x_in, processMat...
method init_dyn_share (line 240) | void init_dyn_share(processModel f_in, processMatrix1 f_x_in, proces...
method init_dyn_runtime_share (line 262) | void init_dyn_runtime_share(processModel f_in, processMatrix1 f_x_in...
method predict (line 280) | void predict(double &dt, processnoisecovariance &Q, const input &i_i...
method update_iterated (line 412) | void update_iterated(measurement &z, measurementnoisecovariance &R)
method update_iterated_share (line 640) | void update_iterated_share()
method update_iterated_dyn (line 872) | void update_iterated_dyn(Eigen::Matrix<scalar_type, Eigen::Dynamic, ...
method update_iterated_dyn_share (line 1093) | void update_iterated_dyn_share()
method update_iterated_dyn_runtime (line 1323) | void update_iterated_dyn_runtime(measurement_runtime z, measurementn...
method update_iterated_dyn_runtime_share (line 1550) | void update_iterated_dyn_runtime_share(measurement_runtime z, measur...
method update_iterated_dyn_share_modified (line 1777) | void update_iterated_dyn_share_modified(double R, double &solve_time)
method change_x (line 2123) | void change_x(state &input_state)
method change_P (line 2134) | void change_P(cov &input_cov)
method state (line 2139) | const state &get_x() const
method cov (line 2143) | const cov &get_P() const
method T (line 2178) | T check_safe_update(T _temp_vec)
FILE: FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp
type esekfom (line 40) | namespace esekfom {
class is_same (line 43) | class is_same {
class is_same<T1, T1> (line 50) | class is_same<T1, T1> {
class is_double (line 58) | class is_double {
class is_double<double> (line 66) | class is_double<double> {
function T (line 74) | static T
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp
type MTK (line 86) | namespace MTK {
type SubManifold (line 100) | struct SubManifold : public T
method SubManifold (line 108) | explicit
method SubManifold (line 113) | SubManifold(const T& t) : T(t) {}
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp
type MTK (line 92) | namespace MTK {
type internal (line 94) | namespace internal {
type traits (line 97) | struct traits {
type traits<float> (line 105) | struct traits<float> : traits<Scalar<float> > {}
type traits<double> (line 107) | struct traits<double> : traits<Scalar<double> > {}
function scalar (line 131) | inline scalar normalize(scalar x, scalar bound){ //not used
function cos_sinc_sqrt (line 143) | std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
function hat (line 177) | Eigen::Matrix<typename Base::scalar, 3, 3> hat(const Base& v) {
function A_inv_trans (line 186) | Eigen::Matrix<typename Base::scalar, 3, 3> A_inv_trans(const Base& v){
function A_inv (line 202) | Eigen::Matrix<typename Base::scalar, 3, 3> A_inv(const Base& v){
function S2_w_expw_ (line 218) | Eigen::Matrix<scalar, 2, 3> S2_w_expw_( Eigen::Matrix<scalar, 2, 1> v,...
function A_matrix (line 236) | Eigen::Matrix<typename Base::scalar, 3, 3> A_matrix(const Base & v){
function scalar (line 250) | scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, ...
function log (line 269) | void log(vectview<scalar, n> result,
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp
type MTK (line 45) | namespace MTK {
type internal (line 58) | namespace internal {
type CovBlock (line 60) | struct CovBlock {
type CovBlock_ (line 66) | struct CovBlock_ {
type CrossCovBlock (line 72) | struct CrossCovBlock {
type CrossCovBlock_ (line 78) | struct CrossCovBlock_ {
type VectviewBase (line 84) | struct VectviewBase {
type UnalignedType (line 91) | struct UnalignedType {
class vectview (line 97) | class vectview : public internal::VectviewBase<scalar, dim>::Type {
method vectview (line 105) | explicit
method vectview (line 108) | vectview(matrix_type& m) : base(m.data(), m.size()) {}
method vectview (line 110) | vectview(const vectview &v) : base(v) {}
method vectview (line 113) | vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef...
method vectview (line 115) | vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&blo...
method scalar (line 120) | scalar* data() {return const_cast<scalar*>(base::data());}
class vectview<const scalar, dim> (line 134) | class vectview<const scalar, dim> : public internal::VectviewBase<scal...
method vectview (line 142) | explicit
method vectview (line 146) | vectview(const Eigen::Matrix<scalar, dim, 1, options>& m) : base(m.d...
method vectview (line 149) | vectview(const Eigen::Matrix<scalar, 1, dim, options, phony>& m) : b...
method vectview (line 151) | vectview(vectview<scalar, dim> x) : base(x.data()) {}
method vectview (line 153) | vectview(const base &x) : base(x) {}
method vectview (line 159) | vectview(Eigen::VectorBlock<Base, dim> block) : base(&block.coeffRef...
method vectview (line 161) | vectview(Eigen::Block<Base, dim, 1, PacketAccess> block) : base(&blo...
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/startIdx.hpp
type MTK (line 85) | namespace MTK {
function getStartIdx (line 105) | int getStartIdx( MTK::SubManifold<T, idx, dim> Base::*)
function getStartIdx_ (line 111) | int getStartIdx_( MTK::SubManifold<T, idx, dim> Base::*)
function getDof (line 120) | int getDof( MTK::SubManifold<T, idx, dim> Base::*)
function getDim (line 125) | int getDim( MTK::SubManifold<T, idx, dim> Base::*)
function setDiagonal (line 134) | void setDiagonal(Eigen::Matrix<typename Base::scalar, Base::DOF, Base:...
function setDiagonal_ (line 141) | void setDiagonal_(Eigen::Matrix<typename Base::scalar, Base::DIM, Base...
function subblock (line 157) | typename MTK::internal::CovBlock<Base, T1, T2>::Type
function subblock_ (line 165) | typename MTK::internal::CovBlock_<Base, T1, T2>::Type
function subblock (line 173) | typename MTK::internal::CrossCovBlock<Base1, Base2, T1, T2>::Type
function subblock_ (line 180) | typename MTK::internal::CrossCovBlock_<Base1, Base2, T1, T2>::Type
function subblock_ (line 194) | typename MTK::internal::CovBlock_<Base, T, T>::Type
function subblock (line 202) | typename MTK::internal::CovBlock<Base, T, T>::Type
class get_cov (line 210) | class get_cov {
class get_cov_ (line 217) | class get_cov_ {
class get_cross_cov (line 224) | class get_cross_cov {
class get_cross_cov_ (line 231) | class get_cross_cov_ {
function subvector_impl_ (line 239) | vectview<typename Base::scalar, T::DIM>
function subvector_impl (line 246) | vectview<typename Base::scalar, T::DOF>
function subvector_ (line 256) | vectview<Scalar, T::DIM>
function subvector (line 263) | vectview<Scalar, T::DOF>
function subvector (line 273) | vectview<Scalar, T::DOF>
function subvector_ (line 280) | vectview<Scalar, T::DIM>
function subvector_ (line 287) | vectview<const Scalar, T::DIM>
function subvector (line 294) | vectview<const Scalar, T::DOF>
function subvector_impl (line 305) | vectview<const typename Base::scalar, T::DOF>
function subvector_impl_ (line 312) | vectview<const typename Base::scalar, T::DIM>
function subvector (line 319) | vectview<const Scalar, T::DOF>
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/S2.hpp
type MTK (line 89) | namespace MTK {
type S2 (line 98) | struct S2 {
method S2 (line 114) | S2() {
method S2 (line 119) | S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, ...
method S2 (line 124) | S2(const vect_type &_vec) : vec(_vec) {
method oplus (line 129) | void oplus(MTK::vectview<const scalar, 3> delta, scalar scale = 1)
method boxplus (line 136) | void boxplus(MTK::vectview<const scalar, 2> delta, scalar scale=1) {
method boxminus (line 144) | void boxminus(MTK::vectview<scalar, 2> res, const S2<scalar, den, nu...
method S2_hat (line 169) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method S2_Bx (line 179) | void S2_Bx(Eigen::Matrix<scalar, 3, 2> &res)
method S2_Nx (line 234) | void S2_Nx(Eigen::Matrix<scalar, 2, 3> &res, S2<scalar, den, num, S2...
method S2_Nx_yy (line 259) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 266) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
method vect_type (line 286) | const vect_type& get_vect() const {
method scalar (line 297) | scalar operator[](int idx) const {return vec[idx]; }
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp
type MTK (line 86) | namespace MTK {
type SO2 (line 95) | struct SO2 : public Eigen::Rotation2D<_scalar> {
method SO2 (line 103) | SO2(const scalar& angle = 0) : base(angle) { }
method SO2 (line 106) | SO2(const base& src) : base(src) {}
method SO2 (line 112) | SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}
method SO2 (line 116) | SO2 operator%(const base &r) const {
method vect_type (line 122) | vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
method SO2 (line 127) | SO2 operator/(const SO2 &r) const {
method S2_hat (line 135) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method S2_Nx_yy (line 140) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 147) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
method oplus (line 154) | void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method boxplus (line 158) | void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method boxminus (line 161) | void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& oth...
type SO3 (line 178) | struct SO3 : public Eigen::Quaternion<_scalar, Options> {
method EIGEN_STRONG_INLINE (line 186) | EIGEN_STRONG_INLINE
method vect_type (line 193) | vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
method EIGEN_STRONG_INLINE (line 198) | EIGEN_STRONG_INLINE
method SO3 (line 207) | SO3(const scalar& w, const scalar& x, const scalar& y, const scalar&...
method SO3 (line 215) | SO3(const base& src = base::Identity()) : base(src) {}
method SO3 (line 222) | SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}
method SO3 (line 229) | SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation...
method boxplus (line 233) | void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
method boxminus (line 237) | void boxminus(MTK::vectview<scalar, DOF> res, const SO3<scalar>& oth...
method oplus (line 242) | void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
method S2_hat (line 247) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method S2_Nx_yy (line 251) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 258) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
method SO3 (line 284) | static SO3 exp(const Eigen::Matrix<scalar, 3, 1>& dvec, scalar scale...
method log (line 293) | static typename base::Vector3 log(const SO3 &orient){
type internal (line 300) | namespace internal {
type UnalignedType<SO2<Scalar, Options > > (line 302) | struct UnalignedType<SO2<Scalar, Options > >{
type UnalignedType<SO3<Scalar, Options > > (line 307) | struct UnalignedType<SO3<Scalar, Options > >{
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/vect.hpp
type MTK (line 89) | namespace MTK {
type vect (line 100) | struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> {
method vect (line 108) | vect(const base &src = base::Zero()) : base(src) {}
method EIGEN_STRONG_INLINE (line 112) | EIGEN_STRONG_INLINE vect(const Eigen::DenseBase<OtherDerived>& other...
method vect (line 115) | vect(const scalar* src, int size = DOF) : base(base::Map(src, size))...
method boxplus (line 117) | void boxplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
method boxminus (line 120) | void boxminus(MTK::vectview<scalar, D> res, const vect<D, scalar>& o...
method oplus (line 124) | void oplus(MTK::vectview<const scalar, D> vec, scalar scale=1) {
method S2_hat (line 128) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method S2_Nx_yy (line 133) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 140) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
method tail (line 191) | vectview<scalar, dim> tail(){
method tail (line 196) | vectview<const scalar, dim> tail() const{
method head (line 201) | vectview<scalar, dim> head(){
method head (line 206) | vectview<const scalar, dim> head() const{
type matrix (line 219) | struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> {
method matrix (line 227) | matrix() {
method EIGEN_STRONG_INLINE (line 233) | EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase<OtherDerived>& ot...
method matrix (line 236) | matrix(const scalar* src) : base(src) { }
method boxplus (line 238) | void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method boxminus (line 241) | void boxminus(MTK::vectview<scalar, DOF> res, const matrix& other) c...
method S2_hat (line 245) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method oplus (line 250) | void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method S2_Nx_yy (line 254) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 261) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
type Scalar (line 288) | struct Scalar {
method Scalar (line 294) | Scalar(const scalar& value = scalar(0)) : value(value) {}
method Scalar (line 297) | Scalar& operator=(const scalar& val) { value = val; return *this; }
method S2_hat (line 299) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method S2_Nx_yy (line 304) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 311) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
method oplus (line 318) | void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
method boxplus (line 322) | void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
method boxminus (line 325) | void boxminus(MTK::vectview<scalar, DOF> res, const Scalar& other) c...
type PositiveScalar (line 335) | struct PositiveScalar {
method PositiveScalar (line 341) | PositiveScalar(const scalar& value = scalar(1)) : value(value) {
method PositiveScalar (line 345) | PositiveScalar& operator=(const scalar& val) { assert(val>0); value ...
method boxplus (line 347) | void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method boxminus (line 350) | void boxminus(MTK::vectview<scalar, DOF> res, const PositiveScalar& ...
method oplus (line 354) | void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method S2_hat (line 358) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method S2_Nx_yy (line 363) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 370) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
type Complex (line 386) | struct Complex : public std::complex<_scalar>{
method Complex (line 392) | Complex(const Base& value) : Base(value) {}
method Complex (line 393) | Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, i...
method Complex (line 394) | Complex(const MTK::vectview<const scalar, 2> &in) : Base(in[0], in[1...
method Complex (line 396) | Complex(const Eigen::DenseBase<Derived> &in) : Base(in[0], in[1]) {}
method boxplus (line 398) | void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method boxminus (line 402) | void boxminus(MTK::vectview<scalar, DOF> res, const Complex& other) ...
method S2_hat (line 407) | void S2_hat(Eigen::Matrix<scalar, 3, 3> &res)
method oplus (line 412) | void oplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
method S2_Nx_yy (line 417) | void S2_Nx_yy(Eigen::Matrix<scalar, 2, 3> &res)
method S2_Mx (line 424) | void S2_Mx(Eigen::Matrix<scalar, 3, 2> &res, MTK::vectview<const sca...
method scalar (line 431) | scalar squaredNorm() const {
method scalar (line 435) | const scalar& operator()(int i) const {
method scalar (line 439) | scalar& operator()(int i){
type internal (line 446) | namespace internal {
type UnalignedType<vect<dim, Scalar, Options > > (line 449) | struct UnalignedType<vect<dim, Scalar, Options > >{
FILE: FAST-LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp
type MTK (line 43) | namespace MTK {
type cv_f_type (line 46) | struct cv_f_type
type cv_f_type<double> (line 49) | struct cv_f_type<double>
type cv_f_type<float> (line 55) | struct cv_f_type<float>
class cv_mat (line 64) | class cv_mat : public matrix<rows, cols, f_type, cols==1 ? Eigen::ColM...
method cv_mat (line 71) | cv_mat()
method cv_mat (line 76) | cv_mat(const cv_mat& oth) : base_type(oth)
method cv_mat (line 82) | cv_mat(const Eigen::MatrixBase<Derived> &value) : base_type(value)
method cv_mat (line 88) | cv_mat& operator=(const Eigen::MatrixBase<Derived> &value)
method cv_mat (line 94) | cv_mat& operator=(const cv_mat& value)
method CvMat (line 101) | CvMat* operator&()
method CvMat (line 105) | const CvMat* operator&() const
FILE: FAST-LIO/include/common_lib.h
type fast_lio (line 36) | typedef fast_lio::Pose6D Pose6D;
type pcl (line 37) | typedef pcl::PointXYZINormal PointType;
type pcl (line 38) | typedef pcl::PointCloud<PointType> PointCloudXYZI;
type vector (line 39) | typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
type Vector3d (line 40) | typedef Vector3d V3D;
type Matrix3d (line 41) | typedef Matrix3d M3D;
type Vector3f (line 42) | typedef Vector3f V3F;
type Matrix3f (line 43) | typedef Matrix3f M3F;
type StatesGroup (line 67) | struct StatesGroup
function resetpose (line 141) | void resetpose()
function calc_dist (line 222) | float calc_dist(PointType p1, PointType p2)
FILE: FAST-LIO/include/ikd-Tree/ikd_Tree.cpp
function BoxPointType (line 118) | BoxPointType KD_TREE::tree_range()
type timespec (line 893) | struct timespec
type timespec (line 967) | struct timespec
type timespec (line 1056) | struct timespec
type timespec (line 1131) | struct timespec
function PointType_CMP (line 1893) | PointType_CMP MANUAL_HEAP::top()
function Operation_Logger_Type (line 1998) | Operation_Logger_Type MANUAL_Q::front()
function Operation_Logger_Type (line 2003) | Operation_Logger_Type MANUAL_Q::back()
FILE: FAST-LIO/include/ikd-Tree/ikd_Tree.h
type pcl (line 20) | typedef pcl::PointXYZINormal PointType;
type vector (line 21) | typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
type KD_TREE_NODE (line 25) | struct KD_TREE_NODE
type PointType_CMP (line 53) | struct PointType_CMP
type BoxPointType (line 71) | struct BoxPointType
type operation_set (line 77) | enum operation_set
type delete_point_storage_set (line 87) | enum delete_point_storage_set
type Operation_Logger_Type (line 94) | struct Operation_Logger_Type
function class (line 103) | class MANUAL_Q
function class (line 122) | class MANUAL_HEAP
function class (line 141) | class KD_TREE
FILE: FAST-LIO/include/matplotlibcpp.h
function namespace (line 42) | namespace matplotlibcpp {
function backend (line 265) | inline void backend(const std::string& name)
function annotate (line 270) | inline bool annotate(std::string annotation, double x, double y)
function namespace (line 296) | namespace detail {
function namespace (line 781) | namespace detail {
function fignum_exists (line 1520) | inline bool fignum_exists(long number)
function figure_size (line 1536) | inline void figure_size(size_t w, size_t h)
function legend (line 1558) | inline void legend()
function legend (line 1568) | inline void legend(const std::map<std::string, std::string>& keywords)
function xticks (line 1708) | void xticks(const std::vector<Numeric> &ticks, const std::map<std::strin...
function yticks (line 1757) | void yticks(const std::vector<Numeric> &ticks, const std::map<std::strin...
function margins (line 1762) | void margins(Numeric margin)
function margins (line 1777) | void margins(Numeric margin_x, Numeric margin_y)
function subplot (line 1820) | inline void subplot(long nrows, long ncols, long plot_number)
function axis (line 1906) | inline void axis(const std::string &axisstr)
function grid (line 2066) | inline void grid(bool flag)
function close (line 2108) | inline void close()
function xkcd (line 2121) | inline void xkcd() {
function draw (line 2138) | inline void draw()
function pause (line 2152) | void pause(Numeric interval)
function save (line 2166) | inline void save(const std::string& filename)
function clf (line 2183) | inline void clf() {
function cla (line 2195) | inline void cla() {
function ion (line 2207) | inline void ion() {
function tight_layout (line 2256) | inline void tight_layout() {
function namespace (line 2270) | namespace detail {
type Fallback (line 2287) | struct Fallback { void operator()(); }
type Derived (line 2288) | struct Derived
type dtype (line 2300) | typedef decltype(&Fallback::operator()) dtype;
type typename (line 2308) | typedef typename is_callable_impl<std::is_class<T>::value, T>::type type;
function false_type (line 2315) | struct plot_impl<std::false_type>
function true_type (line 2354) | struct plot_impl<std::true_type>
function class (line 2400) | class Plot
FILE: FAST-LIO/include/use-ikfom.hpp
function process_noise_cov (line 27) | MTK::get_cov<process_noise_ikfom>::type process_noise_cov()
function get_f (line 40) | Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
function df_dx (line 56) | Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in)
function df_dw (line 75) | Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in)
function vect3 (line 85) | vect3 SO3ToEuler(const SO3 &orient) //将SO3转为Euler角
FILE: FAST-LIO/src/IMU_Processing.hpp
function time_list (line 32) | const bool time_list(PointType &x, PointType &y) { return (x.curvature <...
class ImuProcess (line 35) | class ImuProcess
FILE: FAST-LIO/src/laserMapping.cpp
function SigHandle (line 160) | void SigHandle(int sig)
function dump_lio_state_to_log (line 168) | inline void dump_lio_state_to_log(FILE *fp)
function pointBodyToWorld_ikfom (line 185) | void pointBodyToWorld_ikfom(PointType const *const pi, PointType *const ...
function pointBodyToWorld (line 197) | void pointBodyToWorld(PointType const *const pi, PointType *const po)
function pointBodyToWorld (line 209) | void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po)
function RGBpointBodyToWorld (line 219) | void RGBpointBodyToWorld(PointType const *const pi, PointType *const po)
function RGBpointBodyLidarToIMU (line 230) | void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
function points_cache_collect (line 242) | void points_cache_collect()
function lasermap_fov_segment (line 253) | void lasermap_fov_segment()
function standard_pcl_cbk (line 322) | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
function livox_pcl_cbk (line 347) | void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
function imu_cbk (line 386) | void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
function sync_packages (line 415) | bool sync_packages(MeasureGroup &meas)
function map_incremental (line 466) | void map_incremental() //地图的增量更新，主要完成对ikd-tree的地图建立
function publish_frame_world (line 528) | void publish_frame_world(const ros::Publisher &pubLaserCloudFull)
function publish_frame_body (line 569) | void publish_frame_body(const ros::Publisher &pubLaserCloudFull_body)
function publish_frame_lidar (line 588) | void publish_frame_lidar(const ros::Publisher &pubLaserCloudFull_lidar)
function publish_effect_world (line 598) | void publish_effect_world(const ros::Publisher &pubLaserCloudEffect)
function publish_map (line 614) | void publish_map(const ros::Publisher &pubLaserCloudMap)
function set_posestamp (line 624) | void set_posestamp(T &out)
function publish_odometry (line 635) | void publish_odometry(const ros::Publisher &pubOdomAftMapped)
function publish_path (line 669) | void publish_path(const ros::Publisher pubPath)
function h_share_model (line 685) | void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double>...
function main (line 808) | int main(int argc, char **argv)
FILE: FAST-LIO/src/preprocess.h
type pcl (line 10) | typedef pcl::PointXYZINormal PointType;
type pcl (line 11) | typedef pcl::PointCloud<PointType> PointCloudXYZI;
type LID_TYPE (line 14) | enum LID_TYPE
type Feature (line 22) | enum Feature
type Surround (line 33) | enum Surround
type E_jump (line 40) | enum E_jump
type orgtype (line 49) | struct orgtype
function Point (line 73) | struct EIGEN_ALIGN16 Point
function Point (line 90) | struct EIGEN_ALIGN16 Point
FILE: SC-PGO/include/aloam_velodyne/common.h
type pcl (line 43) | typedef pcl::PointXYZI PointType;
function rad2deg (line 45) | inline double rad2deg(double radians)
function deg2rad (line 50) | inline double deg2rad(double degrees)
type Pose6D (line 55) | struct Pose6D {
FILE: SC-PGO/include/aloam_velodyne/tic_toc.h
function class (line 10) | class TicToc
function class (line 34) | class TicTocV2
function tic (line 48) | void tic()
function toc (line 53) | void toc( std::string _about_task )
FILE: SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h
type typename (line 53) | typedef typename Distance::template traits<num_t,self_t>::distance_t met...
type nanoflann (line 54) | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexTy...
function m_data (line 59) | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const Ve...
function num_t (line 103) | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
FILE: SC-PGO/include/scancontext/Scancontext.cpp
function coreImportTest (line 6) | void coreImportTest (void)
function rad2deg (line 12) | float rad2deg(float radians)
function deg2rad (line 17) | float deg2rad(float degrees)
function xy2theta (line 23) | float xy2theta( const float & _x, const float & _y )
function MatrixXd (line 39) | MatrixXd circshift( MatrixXd &_mat, int _num_shift )
function eig2stdvec (line 62) | std::vector<float> eig2stdvec( MatrixXd _eigmat )
function MatrixXd (line 151) | MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _sca...
function MatrixXd (line 198) | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
function MatrixXd (line 214) | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
FILE: SC-PGO/include/scancontext/Scancontext.h
function class (line 57) | class SCManager
FILE: SC-PGO/include/scancontext/nanoflann.hpp
function T (line 79) | T pi_const() {
type has_resize (line 87) | struct has_resize : std::false_type {}
type has_resize<T, decltype((void)std::declval<T>().resize(1), 0)> (line 90) | struct has_resize<T, decltype((void)std::declval<T>().resize(1), 0)>
type has_assign (line 93) | struct has_assign : std::false_type {}
type has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)> (line 96) | struct has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)>
function resize (line 103) | inline typename std::enable_if<has_resize<Container>::value, void>::type
function resize (line 113) | inline typename std::enable_if<!has_resize<Container>::value, void>::type
function assign (line 123) | inline typename std::enable_if<has_assign<Container>::value, void>::type
function assign (line 132) | inline typename std::enable_if<!has_assign<Container>::value, void>::type
class KNNResultSet (line 142) | class KNNResultSet {
method KNNResultSet (line 155) | inline KNNResultSet(CountType capacity_)
method init (line 158) | inline void init(IndexType *indices_, DistanceType *dists_) {
method CountType (line 166) | inline CountType size() const { return count; }
method full (line 168) | inline bool full() const { return count == capacity; }
method addPoint (line 175) | inline bool addPoint(DistanceType dist, IndexType index) {
type IndexDist_Sorter (line 208) | struct IndexDist_Sorter {
class RadiusResultSet (line 220) | class RadiusResultSet {
method RadiusResultSet (line 230) | inline RadiusResultSet(
method init (line 237) | inline void init() { clear(); }
method clear (line 238) | inline void clear() { m_indices_dists.clear(); }
method size (line 240) | inline size_t size() const { return m_indices_dists.size(); }
method full (line 242) | inline bool full() const { return true; }
method addPoint (line 249) | inline bool addPoint(DistanceType dist, IndexType index) {
method DistanceType (line 255) | inline DistanceType worstDist() const { return radius; }
method worst_item (line 261) | std::pair<IndexType, DistanceType> worst_item() const {
method save_value (line 279) | void save_value(FILE *stream, const T &value, size_t count = 1) {
method save_value (line 284) | void save_value(FILE *stream, const std::vector<T> &value) {
method load_value (line 291) | void load_value(FILE *stream, T &value, size_t count = 1) {
method load_value (line 298) | void load_value(FILE *stream, std::vector<T> &value) {
type Metric (line 315) | struct Metric {}
type L1_Adaptor (line 324) | struct L1_Adaptor {
method L1_Adaptor (line 330) | L1_Adaptor(const DataSource &_data_source) : data_source(_data_sourc...
method DistanceType (line 332) | inline DistanceType evalMetric(const T *a, const size_t b_idx, size_...
method DistanceType (line 363) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type L2_Adaptor (line 375) | struct L2_Adaptor {
method L2_Adaptor (line 381) | L2_Adaptor(const DataSource &_data_source) : data_source(_data_sourc...
method DistanceType (line 383) | inline DistanceType evalMetric(const T *a, const size_t b_idx, size_...
method DistanceType (line 411) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type L2_Simple_Adaptor (line 423) | struct L2_Simple_Adaptor {
method L2_Simple_Adaptor (line 429) | L2_Simple_Adaptor(const DataSource &_data_source)
method DistanceType (line 432) | inline DistanceType evalMetric(const T *a, const size_t b_idx,
method DistanceType (line 443) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type SO2_Adaptor (line 455) | struct SO2_Adaptor {
method SO2_Adaptor (line 461) | SO2_Adaptor(const DataSource &_data_source) : data_source(_data_sour...
method DistanceType (line 463) | inline DistanceType evalMetric(const T *a, const size_t b_idx,
method DistanceType (line 471) | inline DistanceType accum_dist(const U a, const V b, const size_t) c...
type SO3_Adaptor (line 490) | struct SO3_Adaptor {
method SO3_Adaptor (line 496) | SO3_Adaptor(const DataSource &_data_source)
method DistanceType (line 499) | inline DistanceType evalMetric(const T *a, const size_t b_idx,
method DistanceType (line 505) | inline DistanceType accum_dist(const U a, const V b, const size_t id...
type metric_L1 (line 511) | struct metric_L1 : public Metric {
type traits (line 512) | struct traits {
type metric_L2 (line 517) | struct metric_L2 : public Metric {
type traits (line 518) | struct traits {
type metric_L2_Simple (line 523) | struct metric_L2_Simple : public Metric {
type traits (line 524) | struct traits {
type metric_SO2 (line 529) | struct metric_SO2 : public Metric {
type traits (line 530) | struct traits {
type metric_SO3 (line 535) | struct metric_SO3 : public Metric {
type traits (line 536) | struct traits {
type KDTreeSingleIndexAdaptorParams (line 547) | struct KDTreeSingleIndexAdaptorParams {
method KDTreeSingleIndexAdaptorParams (line 548) | KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
type SearchParams (line 555) | struct SearchParams {
method SearchParams (line 558) | SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ ...
method T (line 579) | inline T *allocate(size_t count = 1) {
class PooledAllocator (line 602) | class PooledAllocator {
method internal_init (line 613) | void internal_init() {
method PooledAllocator (line 627) | PooledAllocator() { internal_init(); }
method free_all (line 635) | void free_all() {
method T (line 703) | T *allocate(const size_t count = 1) {
type array_or_vector_selector (line 716) | struct array_or_vector_selector {
type array_or_vector_selector<-1, T> (line 720) | struct array_or_vector_selector<-1, T> {
class KDTreeBaseClass (line 740) | class KDTreeBaseClass {
method freeIndex (line 745) | void freeIndex(Derived &obj) {
type Node (line 755) | struct Node {
type leaf (line 759) | struct leaf {
type nonleaf (line 762) | struct nonleaf {
type Interval (line 772) | struct Interval {
method size (line 814) | size_t size(const Derived &obj) const { return obj.m_size; }
method veclen (line 817) | size_t veclen(const Derived &obj) {
method ElementType (line 822) | inline ElementType dataset_get(const Derived &obj, size_t idx,
method usedMemory (line 831) | size_t usedMemory(Derived &obj) {
method computeMinMax (line 837) | void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,
method NodePtr (line 858) | NodePtr divideTree(Derived &obj, const IndexType left, const IndexTy...
method middleSplit_ (line 910) | void middleSplit_(Derived &obj, IndexType *ind, IndexType count,
method planeSplit (line 968) | void planeSplit(Derived &obj, IndexType *ind, const IndexType count,
method DistanceType (line 1006) | DistanceType computeInitialDistances(const Derived &obj,
method save_tree (line 1025) | void save_tree(Derived &obj, FILE *stream, NodePtr tree) {
method load_tree (line 1035) | void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {
method saveIndex_ (line 1051) | void saveIndex_(Derived &obj, FILE *stream) {
method loadIndex_ (line 1065) | void loadIndex_(Derived &obj, FILE *stream) {
class KDTreeSingleIndexAdaptor (line 1117) | class KDTreeSingleIndexAdaptor
method KDTreeSingleIndexAdaptor (line 1123) | KDTreeSingleIndexAdaptor(
method KDTreeSingleIndexAdaptor (line 1171) | KDTreeSingleIndexAdaptor(const int dimensionality,
method buildIndex (line 1191) | void buildIndex() {
method findNeighbors (line 1222) | bool findNeighbors(RESULTSET &result, const ElementType *vec,
method knnSearch (line 1253) | size_t knnSearch(const ElementType *query_point, const size_t num_cl...
method radiusSearch (line 1278) | size_t
method radiusSearchCustomCallback (line 1296) | size_t radiusSearchCustomCallback(
method init_vind (line 1308) | void init_vind() {
method computeBoundingBox (line 1317) | void computeBoundingBox(BoundingBox &bbox) {
method searchLevel (line 1347) | bool searchLevel(RESULTSET &result_set, const ElementType *vec,
method saveIndex (line 1418) | void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
method loadIndex (line 1425) | void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
class KDTreeSingleIndexDynamicAdaptor_ (line 1467) | class KDTreeSingleIndexDynamicAdaptor_
method KDTreeSingleIndexDynamicAdaptor_ (line 1518) | KDTreeSingleIndexDynamicAdaptor_(
method KDTreeSingleIndexDynamicAdaptor_ (line 1535) | KDTreeSingleIndexDynamicAdaptor_
method buildIndex (line 1554) | void buildIndex() {
method findNeighbors (line 1583) | bool findNeighbors(RESULTSET &result, const ElementType *vec,
method knnSearch (line 1613) | size_t knnSearch(const ElementType *query_point, const size_t num_cl...
method radiusSearch (line 1638) | size_t
method radiusSearchCustomCallback (line 1656) | size_t radiusSearchCustomCallback(
method computeBoundingBox (line 1666) | void computeBoundingBox(BoundingBox &bbox) {
method searchLevel (line 1696) | void searchLevel(RESULTSET &result_set, const ElementType *vec,
method saveIndex (line 1762) | void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
method loadIndex (line 1769) | void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
class KDTreeSingleIndexDynamicAdaptor (line 1788) | class KDTreeSingleIndexDynamicAdaptor {
method First0Bit (line 1822) | int First0Bit(IndexType num) {
method init (line 1832) | void init() {
method KDTreeSingleIndexDynamicAdaptor (line 1857) | KDTreeSingleIndexDynamicAdaptor(const int dimensionality,
method KDTreeSingleIndexDynamicAdaptor (line 1878) | KDTreeSingleIndexDynamicAdaptor(
method addPoints (line 1883) | void addPoints(IndexType start, IndexType end) {
method removePoint (line 1906) | void removePoint(size_t idx) {
method findNeighbors (line 1926) | bool findNeighbors(RESULTSET &result, const ElementType *vec,
type KDTreeEigenMatrixAdaptor (line 1954) | struct KDTreeEigenMatrixAdaptor {
method KDTreeEigenMatrixAdaptor (line 1968) | KDTreeEigenMatrixAdaptor(const size_t dimensionality,
method KDTreeEigenMatrixAdaptor (line 1987) | KDTreeEigenMatrixAdaptor(const self_t &) = delete;
method query (line 1999) | inline void query(const num_t *query_point, const size_t num_closest,
method self_t (line 2010) | const self_t &derived() const { return *this; }
method self_t (line 2011) | self_t &derived() { return *this; }
method kdtree_get_point_count (line 2014) | inline size_t kdtree_get_point_count() const {
method num_t (line 2019) | inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {
method kdtree_get_bbox (line 2028) | bool kdtree_get_bbox(BBOX & /*bb*/) const {
FILE: SC-PGO/src/kittiHelper.cpp
function read_lidar_data (line 25) | std::vector<float> read_lidar_data(const std::string lidar_data_path)
function main (line 37) | int main(int argc, char** argv)
FILE: SC-PGO/src/laserMapping.cpp
function transformAssociateToMap (line 143) | void transformAssociateToMap()
function transformUpdate (line 149) | void transformUpdate()
function pointAssociateToMap (line 155) | void pointAssociateToMap(PointType const *const pi, PointType *const po)
function pointAssociateTobeMapped (line 166) | void pointAssociateTobeMapped(PointType const *const pi, PointType *cons...
function laserCloudCornerLastHandler (line 176) | void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr ...
function laserCloudSurfLastHandler (line 183) | void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &l...
function laserCloudFullResHandler (line 190) | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &la...
function laserOdometryHandler (line 198) | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdome...
function process (line 232) | void process()
function main (line 908) | int main(int argc, char **argv)
FILE: SC-PGO/src/laserOdometry.cpp
function TransformToStart (line 111) | void TransformToStart(PointType const *const pi, PointType *const po)
function TransformToEnd (line 133) | void TransformToEnd(PointType const *const pi, PointType *const po)
function laserCloudSharpHandler (line 150) | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &corn...
function laserCloudLessSharpHandler (line 157) | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &...
function laserCloudFlatHandler (line 164) | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfP...
function laserCloudLessFlatHandler (line 171) | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &s...
function laserCloudFullResHandler (line 179) | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &la...
function main (line 186) | int main(int argc, char **argv)
FILE: SC-PGO/src/laserPosegraphOptimization.cpp
function padZeros (line 137) | std::string padZeros(int val, int num_digits = 6) {
function Pose6DtoGTSAMPose3 (line 143) | gtsam::Pose3 Pose6DtoGTSAMPose3(const Pose6D& p)
function saveOdometryVerticesKITTIformat (line 148) | void saveOdometryVerticesKITTIformat(std::string _filename)
function saveOptimizedVerticesKITTIformat (line 166) | void saveOptimizedVerticesKITTIformat(gtsam::Values _estimates, std::str...
function laserOdometryHandler (line 191) | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &_laserOdom...
function laserCloudFullResHandler (line 198) | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &_l...
function gpsHandler (line 205) | void gpsHandler(const sensor_msgs::NavSatFix::ConstPtr &_gps)
function initNoises (line 214) | void initNoises( void )
function Pose6D (line 242) | Pose6D getOdom(nav_msgs::Odometry::ConstPtr _odom)
function Pose6D (line 255) | Pose6D diffTransformation(const Pose6D& _p1, const Pose6D& _p2)
function local2global (line 268) | pcl::PointCloud<PointType>::Ptr local2global(const pcl::PointCloud<Point...
function pubPath (line 291) | void pubPath( void )
function updatePoses (line 338) | void updatePoses(void)
function runISAM2opt (line 363) | void runISAM2opt(void)
function transformPointCloud (line 376) | pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<Poin...
function loopFindNearKeyframesCloud (line 402) | void loopFindNearKeyframesCloud( pcl::PointCloud<PointType>::Ptr& nearKe...
function doICPVirtualRelative (line 427) | std::optional<gtsam::Pose3> doICPVirtualRelative( int _loop_kf_idx, int ...
function process_pg (line 480) | void process_pg()
function performSCLoopClosure (line 637) | void performSCLoopClosure(void)
function process_lcd (line 656) | void process_lcd(void)
function process_icp (line 668) | void process_icp(void)
function process_viz_path (line 701) | void process_viz_path(void)
function process_isam (line 713) | void process_isam(void)
function pubMap (line 731) | void pubMap(void)
function process_viz_map (line 757) | void process_viz_map(void)
function main (line 770) | int main(int argc, char **argv)
FILE: SC-PGO/src/lidarFactor.hpp
type LidarEdgeFactor (line 12) | struct LidarEdgeFactor
method LidarEdgeFactor (line 14) | LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_poin...
type LidarPlaneFactor (line 57) | struct LidarPlaneFactor
method LidarPlaneFactor (line 59) | LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_poi...
type LidarPlaneNormFactor (line 106) | struct LidarPlaneNormFactor
method LidarPlaneNormFactor (line 109) | LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plan...
type LidarDistanceFactor (line 141) | struct LidarDistanceFactor
method LidarDistanceFactor (line 144) | LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d close...
FILE: SC-PGO/src/scanRegistration.cpp
function comp (line 73) | bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
function removeClosedPointCloud (line 88) | void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
function laserCloudHandler (line 116) | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserClou...
function main (line 475) | int main(int argc, char **argv)
FILE: SC-PGO/utils/python/pypcdMyUtils.py
function make_xyzi_point_cloud (line 5) | def make_xyzi_point_cloud(xyzl, label_type='f'):
Condensed preview — 99 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,300K chars).
[
{
"path": ".gitignore",
"chars": 150,
"preview": "build\nFAST-LIO/Log/*.png\nFAST-LIO/Log/*.txt\nFAST-LIO/Log/*.csv\nFAST-LIO/Log/*.pdf\n.vscode/c_cpp_properties.json\n.vscode/"
},
{
"path": "FAST-LIO/CMakeLists.txt",
"chars": 2206,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(fast_lio)\n\nSET(CMAKE_BUILD_TYPE \"Debug\")\n\nADD_COMPILE_OPTIONS(-std=c++14 )"
},
{
"path": "FAST-LIO/LICENSE",
"chars": 18092,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 2, June 1991\n\n Copyright (C) 1989, 1991 Fr"
},
{
"path": "FAST-LIO/Log/fast_lio_time_log_analysis.m",
"chars": 5182,
"preview": "clear\nclose all\n\nColor_red = [0.6350 0.0780 0.1840];\nColor_blue = [0 0.4470 0.7410];\nColor_orange = [0.8500 0.3250 0.098"
},
{
"path": "FAST-LIO/Log/guide.md",
"chars": 156,
"preview": "Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DE"
},
{
"path": "FAST-LIO/Log/plot.py",
"chars": 3072,
"preview": "# import matplotlib\n# matplotlib.use('Agg')\nimport numpy as np\nimport matplotlib.pyplot as plt\n\n\n#######for ikfom\nfig, a"
},
{
"path": "FAST-LIO/PCD/1",
"chars": 2,
"preview": "1\n"
},
{
"path": "FAST-LIO/README.md",
"chars": 10016,
"preview": "## Related Works\n\n1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search."
},
{
"path": "FAST-LIO/config/avia.yaml",
"chars": 916,
"preview": "common:\n lid_topic: \"/livox/lidar\"\n imu_topic: \"/livox/imu\"\n time_sync_en: false # ONLY turn on when "
},
{
"path": "FAST-LIO/config/horizon.yaml",
"chars": 880,
"preview": "common:\n lid_topic: \"/livox/lidar\"\n imu_topic: \"/livox/imu\"\n time_sync_en: false # ONLY turn on when "
},
{
"path": "FAST-LIO/config/ouster64.yaml",
"chars": 882,
"preview": "common:\n lid_topic: \"/os_cloud_node/points\"\n imu_topic: \"/os_cloud_node/imu\"\n time_sync_en: false # O"
},
{
"path": "FAST-LIO/config/ouster64_mulran.yaml",
"chars": 863,
"preview": "common:\n lid_topic: \"/os1_points\"\n imu_topic: \"/imu/data_raw\"\n time_sync_en: false # ONLY turn on whe"
},
{
"path": "FAST-LIO/config/velodyne.yaml",
"chars": 869,
"preview": "common:\n lid_topic: \"/velodyne_points\"\n imu_topic: \"/imu/data\"\n time_sync_en: false # ONLY turn on wh"
},
{
"path": "FAST-LIO/include/Exp_mat.h",
"chars": 2743,
"preview": "#ifndef EXP_MAT_H\n#define EXP_MAT_H\n\n#include <math.h>\n#include <Eigen/Core>\n#include <opencv2/core.hpp>\n// #include <co"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp",
"chars": 71990,
"preview": "/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Author: Dongjiao HE <hdj658"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/esekfom/util.hpp",
"chars": 2363,
"preview": "/*\n * Copyright (c) 2019--2023, The University of Hong Kong\n * All rights reserved.\n *\n * Author: Dongjiao HE <hdj658"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp",
"chars": 9436,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wag"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp",
"chars": 4886,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wag"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp",
"chars": 10176,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wag"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp",
"chars": 6540,
"preview": "\n/*\n * Copyright (c) 2008--2011, Universitaet Bremen\n * All rights reserved.\n *\n * Author: Christoph Hertzberg <chtz@"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/startIdx.hpp",
"chars": 12515,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wag"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/S2.hpp",
"chars": 9655,
"preview": "// This is a NEW implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wagner, "
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp",
"chars": 10624,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wag"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/vect.hpp",
"chars": 14226,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// C. Hertzberg, R. Wag"
},
{
"path": "FAST-LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp",
"chars": 3236,
"preview": "/*\n * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH\n * All rights reserved.\n *\n * Author: Rene Wagner <"
},
{
"path": "FAST-LIO/include/common_lib.h",
"chars": 8103,
"preview": "#ifndef COMMON_LIB_H\n#define COMMON_LIB_H\n\n#include <so3_math.h>\n#include <Eigen/Eigen>\n#include <pcl/point_types.h>\n#in"
},
{
"path": "FAST-LIO/include/ikd-Tree/README.md",
"chars": 73,
"preview": "# ikd-Tree\nikd-Tree is an incremental k-d tree for robotic applications.\n"
},
{
"path": "FAST-LIO/include/ikd-Tree/ikd_Tree.cpp",
"chars": 76427,
"preview": "#include \"ikd_Tree.h\"\n\n/*\nDescription: ikd-Tree: an incremental k-d tree for robotic applications\nAuthor: Yixi Cai\nemail"
},
{
"path": "FAST-LIO/include/ikd-Tree/ikd_Tree.h",
"chars": 7405,
"preview": "#pragma once\n#include <pcl/point_types.h>\n#include <Eigen/StdVector>\n#include <Eigen/Geometry>\n#include <stdio.h>\n#inclu"
},
{
"path": "FAST-LIO/include/matplotlibcpp.h",
"chars": 81837,
"preview": "#pragma once\n\n// Python headers must be included before any system headers, since\n// they define _POSIX_C_SOURCE\n#includ"
},
{
"path": "FAST-LIO/include/so3_math.h",
"chars": 2936,
"preview": "#ifndef SO3_MATH_H\n#define SO3_MATH_H\n\n#include <math.h>\n#include <Eigen/Core>\n\n#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1]"
},
{
"path": "FAST-LIO/include/use-ikfom.hpp",
"chars": 5078,
"preview": "#ifndef USE_IKFOM_H\n#define USE_IKFOM_H\n\n#include <IKFoM_toolkit/esekfom/esekfom.hpp>\n\ntypedef MTK::vect<3, double> vect"
},
{
"path": "FAST-LIO/launch/gdb_debug_example.launch",
"chars": 962,
"preview": "<launch>\n\n <arg name=\"rviz\" default=\"true\" />\n\n <node pkg=\"fast_lio\" type=\"fastlio_mapping\" name=\"laserMapping\" output"
},
{
"path": "FAST-LIO/launch/mapping_avia.launch",
"chars": 1076,
"preview": "<launch>\n<!-- Launch file for Livox AVIA LiDAR -->\n\n\t<!--- Sim Time -->\n <param name=\"/use_sim_time\" value=\"true\" />\n"
},
{
"path": "FAST-LIO/launch/mapping_horizon.launch",
"chars": 1013,
"preview": "<launch>\n<!-- Launch file for Livox Horizon LiDAR -->\n\n\t<arg name=\"rviz\" default=\"true\" />\n\n\t<rosparam command=\"load\" fi"
},
{
"path": "FAST-LIO/launch/mapping_ouster64.launch",
"chars": 1056,
"preview": "<launch>\n<!-- Launch file for ouster OS2-64 LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command=\"lo"
},
{
"path": "FAST-LIO/launch/mapping_ouster64_mulran.launch",
"chars": 1066,
"preview": "<launch>\n<!-- Launch file for ouster OS2-64 LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command=\"lo"
},
{
"path": "FAST-LIO/launch/mapping_velodyne.launch",
"chars": 1063,
"preview": "<launch>\n <!-- Launch file for velodyne16 VLP-16 LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam comma"
},
{
"path": "FAST-LIO/msg/Pose6D.msg",
"chars": 597,
"preview": "# the preintegrated Lidar states at the time of IMU measurements in a frame\nfloat64 offset_time # the offset time of IM"
},
{
"path": "FAST-LIO/package.xml",
"chars": 1465,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>fast_lio</name>\n <version>0.0.0</version>\n\n <description>\n This is a modifi"
},
{
"path": "FAST-LIO/rviz_cfg/.gitignore",
"chars": 0,
"preview": ""
},
{
"path": "FAST-LIO/rviz_cfg/loam_livox.rviz",
"chars": 11122,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "FAST-LIO/src/IMU_Processing.hpp",
"chars": 17137,
"preview": "#include <cmath>\n#include <math.h>\n#include <deque>\n#include <mutex>\n#include <thread>\n#include <fstream>\n#include <csig"
},
{
"path": "FAST-LIO/src/laserMapping.cpp",
"chars": 52658,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// J. Zhang and S. Singh. "
},
{
"path": "FAST-LIO/src/preprocess.cpp",
"chars": 28330,
"preview": "#include \"preprocess.h\"\n\n#define RETURN0 0x00\n#define RETURN0AND1 0x10\n\nPreprocess::Preprocess()\n : feature_enabled(0"
},
{
"path": "FAST-LIO/src/preprocess.h",
"chars": 4654,
"preview": "#include <ros/ros.h>\n#include <pcl_conversions/pcl_conversions.h>\n#include <sensor_msgs/PointCloud2.h>\n#include <livox_r"
},
{
"path": "README.md",
"chars": 3166,
"preview": "# FAST_LIO2_Noted\n\n## 中文注释说明\n\n本项目是基于[FAST_LIO_SLAM](https://github.com/GDUT-Kyle/FAST_LIO_SLAM)项目扩展来的，主要讲述了Fast-LIO2主程序代"
},
{
"path": "SC-PGO/CMakeLists.txt",
"chars": 1685,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(aloam_velodyne)\n\nset(CMAKE_BUILD_TYPE \"Release\")\n# set(CMAKE_CXX_FLAGS \"-s"
},
{
"path": "SC-PGO/README.md",
"chars": 5858,
"preview": "# SC-A-LOAM\n\n## What is SC-A-LOAM? \n- A real-time LiDAR SLAM package that integrates A-LOAM and ScanContext. \n - **A-"
},
{
"path": "SC-PGO/docker/Dockerfile",
"chars": 1846,
"preview": "FROM ros:kinetic-perception\n\nENV CERES_VERSION=\"1.12.0\"\nENV PCL_VERSION=\"1.8.0\"\nENV CATKIN_WS=/root/catkin_ws\n\n # set"
},
{
"path": "SC-PGO/docker/Makefile",
"chars": 335,
"preview": "all: help\n\nhelp:\n\t@echo \"\"\n\t@echo \"-- Help Menu\"\n\t@echo \"\"\n\t@echo \" 1. make build - build all images\"\n\t# "
},
{
"path": "SC-PGO/docker/run.sh",
"chars": 2018,
"preview": "#!/bin/bash\ntrap : SIGTERM SIGINT\n\nfunction abspath() {\n # generate absolute path from relative path\n # $1 : r"
},
{
"path": "SC-PGO/include/aloam_velodyne/common.h",
"chars": 2374,
"preview": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOA"
},
{
"path": "SC-PGO/include/aloam_velodyne/tic_toc.h",
"chars": 1343,
"preview": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#pragma"
},
{
"path": "SC-PGO/include/scancontext/KDTreeVectorOfVectorsAdaptor.h",
"chars": 5448,
"preview": "/***********************************************************************\n * Software License Agreement (BSD License)\n *\n"
},
{
"path": "SC-PGO/include/scancontext/Scancontext.cpp",
"chars": 14621,
"preview": "#include \"scancontext/Scancontext.h\"\n\n// namespace SC2\n// {\n\nvoid coreImportTest (void)\n{\n cout << \"scancontext lib i"
},
{
"path": "SC-PGO/include/scancontext/Scancontext.h",
"chars": 5210,
"preview": "#pragma once\n\n#include <ctime>\n#include <cassert>\n#include <cmath>\n#include <utility>\n#include <vector>\n#include <algori"
},
{
"path": "SC-PGO/include/scancontext/nanoflann.hpp",
"chars": 73146,
"preview": "/***********************************************************************\n * Software License Agreement (BSD License)\n *\n"
},
{
"path": "SC-PGO/launch/aloam_mulran.launch",
"chars": 1946,
"preview": "<launch>\n \n <param name=\"scan_line\" type=\"int\" value=\"64\" />\n\n <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 "
},
{
"path": "SC-PGO/launch/aloam_velodyne_HDL_32.launch",
"chars": 1640,
"preview": "<launch>\n \n <param name=\"scan_line\" type=\"int\" value=\"32\" />\n\n <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 "
},
{
"path": "SC-PGO/launch/aloam_velodyne_HDL_64.launch",
"chars": 1671,
"preview": "<launch>\n \n <param name=\"scan_line\" type=\"int\" value=\"64\" />\n\n <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 "
},
{
"path": "SC-PGO/launch/aloam_velodyne_VLP_16.launch",
"chars": 1822,
"preview": "<launch>\n \n <param name=\"scan_line\" type=\"int\" value=\"16\" />\n\n <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 "
},
{
"path": "SC-PGO/launch/fastlio_ouster64.launch",
"chars": 1981,
"preview": "<launch>\n\n <param name=\"scan_line\" type=\"int\" value=\"64\" />\n\n <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. "
},
{
"path": "SC-PGO/launch/kitti_helper.launch",
"chars": 510,
"preview": "<launch>\n \n\n <node name=\"kittiHelper\" pkg=\"aloam_velodyne\" type=\"kittiHelper\" output=\"screen\"> \n <param nam"
},
{
"path": "SC-PGO/package.xml",
"chars": 1289,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>aloam_velodyne</name>\n <version>0.0.0</version>\n\n <description>\n This is an"
},
{
"path": "SC-PGO/rviz_cfg/aloam_velodyne.rviz",
"chars": 16809,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "SC-PGO/src/kittiHelper.cpp",
"chars": 7319,
"preview": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#includ"
},
{
"path": "SC-PGO/src/laserMapping.cpp",
"chars": 36726,
"preview": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOA"
},
{
"path": "SC-PGO/src/laserOdometry.cpp",
"chars": 29318,
"preview": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOA"
},
{
"path": "SC-PGO/src/laserPosegraphOptimization.cpp",
"chars": 33672,
"preview": "#include <fstream>\n#include <math.h>\n#include <vector>\n#include <mutex>\n#include <queue>\n#include <thread>\n#include <ios"
},
{
"path": "SC-PGO/src/lidarFactor.hpp",
"chars": 6157,
"preview": "// Author: Tong Qin qintonguav@gmail.com\n// \t Shaozu Cao \t\t saozu.cao@connect.ust.hk\n\n#includ"
},
{
"path": "SC-PGO/src/scanRegistration.cpp",
"chars": 19070,
"preview": "// This is an advanced implementation of the algorithm described in the following paper:\n// J. Zhang and S. Singh. LOA"
},
{
"path": "SC-PGO/utils/python/makeMergedMap.py",
"chars": 5350,
"preview": "import os \nimport sys\nimport time \nimport copy \nfrom io import StringIO\n\nimport pypcd # for the install, use this comman"
},
{
"path": "SC-PGO/utils/python/pypcdMyUtils.py",
"chars": 1244,
"preview": "import numpy as np\nimport pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --us"
},
{
"path": "SC-PGO/utils/sample_data/KAIST03/optimized_poses.txt",
"chars": 367898,
"preview": "1 -8.3363e-15 -7.00722e-10 -1.25902e-17 8.33638e-15 1 1.06392e-10 7.48517e-18 7.00722e-10 -1.06392e-10 1 -2.90588e-12\n0."
},
{
"path": "SC-PGO/utils/sample_data/KAIST03/times.txt",
"chars": 60035,
"preview": "1567410201.7452047\n1567410207.7531185\n1567410208.4532168\n1567410208.9530807\n1567410209.3450341\n1567410209.6453834\n156741"
}
]
// ... and 23 more files (download for full content)
About this extraction
This page contains the full source code of the lovelyyoshino/FAST_LIO2_Noted GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 99 files (1.2 MB), approximately 465.4k tokens, and a symbol index with 536 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.