JYC/25RC_MID360_R2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first_commit

Browse Source
This commit is contained in:
hara 2024-11-17 21:20:53 +08:00
parent 6113041bd0
commit f7b901b1f6
80 changed files with 16163 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/CMakeLists.txt Symbolic link
View File

@ -0,0 +1 @@
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

1
src/FAST_LIO Submodule

@ -0,0 +1 @@
Subproject commit 7cc4175de6f8ba2edf34bab02a42195b141027e9

10
src/FAST_LIO_LOCALIZATION/.gitignore vendored Normal file
View File

@ -0,0 +1,10 @@
build
Log/*.png
Log/*.txt
Log/*.csv
Log/*.pdf
.vscode/c_cpp_properties.json
.vscode/settings.json
PCD/*.pcd
.idea
cmake-build-debug

4
src/FAST_LIO_LOCALIZATION/.gitmodules vendored Normal file
View File

@ -0,0 +1,4 @@
[submodule "include/ikd-Tree"]
path = include/ikd-Tree
url = https://github.com/hku-mars/ikd-Tree.git
branch = fast_lio

97
src/FAST_LIO_LOCALIZATION/CMakeLists.txt Normal file
View File

@ -0,0 +1,97 @@
cmake_minimum_required(VERSION 2.8.3)
project(fast_lio_localization)
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_driver2
message_generation
eigen_conversions
genmsg
)
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_mapping1 src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
add_executable(flag src/flag.cpp)
add_executable(send src/send.cpp src/mbot_linux_serial.cpp)
target_link_libraries(fastlio_mapping1 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_link_libraries(flag ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_link_libraries(send ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
target_include_directories(fastlio_mapping1 PRIVATE ${PYTHON_INCLUDE_DIRS})
target_include_directories(flag PRIVATE ${PYTHON_INCLUDE_DIRS})
target_include_directories(send PRIVATE ${PYTHON_INCLUDE_DIRS})
add_dependencies(fastlio_mapping fast_lio_generate_messages_cpp)

339
src/FAST_LIO_LOCALIZATION/LICENSE Normal file
View File

@ -0,0 +1,339 @@
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.

135
src/FAST_LIO_LOCALIZATION/Log/fast_lio_time_log_analysis.m Normal file
View File

@ -0,0 +1,135 @@
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')

1
src/FAST_LIO_LOCALIZATION/Log/guide.md Normal file
View File

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

94
src/FAST_LIO_LOCALIZATION/Log/plot.py Normal file
View File

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

1
src/FAST_LIO_LOCALIZATION/PCD/1 Normal file
View File

@ -0,0 +1 @@
1

BIN
src/FAST_LIO_LOCALIZATION/PCD/2.pgm Normal file
View File

Binary file not shown.

7
src/FAST_LIO_LOCALIZATION/PCD/2.yaml Normal file
View File

@ -0,0 +1,7 @@
image: /home/cmrt/mid360/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/2.pgm
resolution: 0.100000
origin: [-17.800000, -11.000000, -nan]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

BIN
src/FAST_LIO_LOCALIZATION/PCD/M3.pgm Executable file
View File

Binary file not shown.

7
src/FAST_LIO_LOCALIZATION/PCD/M3.yaml Normal file
View File

@ -0,0 +1,7 @@
image: /home/cmrt/mid360/ws_livox/src/FAST_LIO_LOCALIZATION/PCD.pgm
resolution: 0.050000
origin: [-18.653002, -10.328159, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

BIN
src/FAST_LIO_LOCALIZATION/PCD/scans.pgm Normal file
View File

Binary file not shown.

7
src/FAST_LIO_LOCALIZATION/PCD/scans.yaml Normal file
View File

@ -0,0 +1,7 @@
image: /home/cmrt/mid360/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/m4.pgm
resolution: 0.100000
origin: [-13.700000, -7.900000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

149
src/FAST_LIO_LOCALIZATION/README.md Normal file
View File

@ -0,0 +1,149 @@
# FAST-LIO-LOCALIZATION
A simple localization framework that can re-localize in built maps based on [FAST-LIO](https://github.com/hku-mars/FAST_LIO).
## News
- **2021-08-11:** Add **Open3D 0.7** support.
- **2021-08-09:** Migrate to **Open3D** for better performance.
## 1. Features
- Realtime 3D global localization in a pre-built point cloud map.
By fusing low-frequency global localization (about 0.5~0.2Hz), and high-frequency odometry from FAST-LIO, the entire system is computationally efficient.
<div align="center"><img src="doc/demo.GIF" width=90% /></div>
- Eliminate the accumulative error of the odometry.
<div align="center"><img src="doc/demo_accu.GIF" width=90% /></div>
- The initial localization can be provided either by rough manual estimation from RVIZ, or pose from another sensor/algorithm.
<!-- ![image](doc/real_experiment2.gif) -->
<!-- [![Watch the video](doc/real_exp_2.png)](https://youtu.be/2OvjGnxszf8) -->
<div align="center">
<img src="doc/demo_init.GIF" width=49.6% />
<img src="doc/demo_init_2.GIF" width = 49.6% >
</div>
## 2. Prerequisites
### 2.1 Dependencies for FAST-LIO
Technically, if you have built and run FAST-LIO before, you may skip section 2.1.
This part of dependency is consistent with FAST-LIO, please refer to the documentation https://github.com/hku-mars/FAST_LIO#1-prerequisites
### 2.2 Dependencies for localization module
- python 2.7
- [ros_numpy](https://github.com/eric-wieser/ros_numpy)
```shell
sudo apt install ros-$ROS_DISTRO-ros-numpy
```
- [Open3D](http://www.open3d.org/docs/0.9.0/getting_started.html)
```shell
pip install open3d==0.9
```
Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**:
```shell
ERROR: Package 'pyrsistent' requires a different Python: 2.7.18 not in '>=3.5'
```
you may firstly install **pyrsistent**:
```shell
pip install pyrsistent==0.15
```
Then
```shell
pip install open3d==0.9
```
## 3. Build
Clone the repository and catkin_make:
```
cd ~/$A_ROS_DIR$/src
git clone https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
cd FAST_LIO_LOCALIZATION
git submodule update --init
cd ../..
catkin_make
source devel/setup.bash
```
- Remember to source the livox_ros_driver before build (follow [livox_ros_driver](https://github.com/hku-mars/FAST_LIO#13-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}```
## 4. Run Localization
### 4.1 Sample Dataset
Demo rosbag in a large underground garage:
[Google Drive](https://drive.google.com/file/d/15ZZAcz84mDxaWviwFPuALpkoeK-KAh-4/view?usp=sharing) | [Baidu Pan (Code: ne8d)](https://pan.baidu.com/s/1ceBiIAUqHa1vY3QjWpxwNA);
Corresponding map: [Google Drive](https://drive.google.com/file/d/1X_mhPlSCNj-1erp_DStCQZfkY7l4w7j8/view?usp=sharing) | [Baidu Pan (Code: kw6f)](https://pan.baidu.com/s/1Yw4vY3kEK8x2g-AsBi6VCw)
The map can be built using LIO-SAM or FAST-LIO-SLAM.
### 4.2 Run
1. First, please make sure you're using the **Python 2.7** environment;
2. Run localization, here we take Livox AVIA as an example:
```shell
roslaunch fast_lio_localization localization_avia.launch map:=/path/to/your/map.pcd
```
Please modify `/path/to/your/map.pcd` to your own map point cloud file path.
Wait for 3~5 seconds until the map cloud shows up in RVIZ;
3. If you are testing with the sample rosbag data:
```shell
rosbag play localization_test_scene_1.bag
```
Or if you are running realtime
```shell
roslaunch livox_ros_driver livox_lidar_msg.launch
```
Please set the **publish_freq** in **livox_lidar_rviz.launch** to **10Hz**, to ensure there are enough points for global localization in a single scan.
Support for higher frequency is coming soon.
4. Provide initial pose
```shell
rosrun fast_lio_localization publish_initial_pose.py 14.5 -7.5 0 -0.25 0 0
```
The numerical value **14.5 -7.5 0 -0.25 0 0** denotes 6D pose **x y z yaw pitch roll** in map frame,
which is a rough initial guess for **localization_test_scene_1.bag**.
The initial guess can also be provided by the '2D Pose Estimate' Tool in RVIZ.
Note that, during the initialization stage, it's better to keep the robot still. Or if you play bags, fistly play the bag for about 0.5s, and then pause the bag until the initialization succeed.
## Related Works
1. [FAST-LIO](https://github.com/hku-mars/FAST_LIO): A computationally efficient and robust LiDAR-inertial odometry (LIO) package
2. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search.
3. [FAST-LIO-SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM): The integration of FAST-LIO with [Scan-Context](https://github.com/irapkaist/scancontext) **loop closure** module.
4. [LIO-SAM_based_relocalization](https://github.com/Gaochao-hit/LIO-SAM_based_relocalization): A simple system that can relocalize a robot on a built map based on LIO-SAM.
## Acknowledgments
Thanks for the authors of [FAST-LIO](https://github.com/hku-mars/FAST_LIO) and [LIO-SAM_based_relocalization](https://github.com/Gaochao-hit/LIO-SAM_based_relocalization).
## TODO
1. Go over the timestamp issue of the published odometry and tf;
2. Using integrated points for global localization;
3. Fuse global localization with the state estimation of FAST-LIO, and smooth the localization trajectory;
4. Updating...

26
src/FAST_LIO_LOCALIZATION/config/avia.yaml Normal file
View File

@ -0,0 +1,26 @@
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: 90
det_range: 450.0
extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
scan_publish_en: 1 # 'false' will close all the point cloud output
dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame

26
src/FAST_LIO_LOCALIZATION/config/horizon.yaml Normal file
View File

@ -0,0 +1,26 @@
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: 1 # 'false' will close all the point cloud output
dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame

35
src/FAST_LIO_LOCALIZATION/config/mid360.yaml Normal file
View File

@ -0,0 +1,35 @@
common:
lid_topic: "/livox/lidar"
imu_topic: "/livox/imu"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
# This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
preprocess:
lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 4
blind: 0.5
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 360
det_range: 100.0
extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic
extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
extrinsic_R: [ 1, 0, 0,
0, 1, 0,
0, 0, 1]
publish:
path_en: false
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

26
src/FAST_LIO_LOCALIZATION/config/ouster64.yaml Normal file
View File

@ -0,0 +1,26 @@
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: 1 # 'false' will close all the point cloud output
dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame

26
src/FAST_LIO_LOCALIZATION/config/velodyne.yaml Normal file
View File

@ -0,0 +1,26 @@
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: 1 # 'false' will close all the point cloud output
dense_publish_en: 1 # false will low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: 1 # output the point cloud scans in IMU-body-frame

BIN
src/FAST_LIO_LOCALIZATION/doc/demo.GIF Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 MiB

BIN
src/FAST_LIO_LOCALIZATION/doc/demo_accu.GIF Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.7 MiB

BIN
src/FAST_LIO_LOCALIZATION/doc/demo_init.GIF Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.0 MiB

BIN
src/FAST_LIO_LOCALIZATION/doc/demo_init_2.GIF Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.2 MiB

103
src/FAST_LIO_LOCALIZATION/include/Exp_mat.h Normal file
View File

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

2005
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/esekfom/esekfom.hpp Executable file
View File

File diff suppressed because it is too large Load Diff

82
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/esekfom/util.hpp Executable file
View File

@ -0,0 +1,82 @@
/*
* 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__

229
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/build_manifold.hpp Executable file
View File

@ -0,0 +1,229 @@
// 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_*/

123
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/src/SubManifold.hpp Executable file
View File

@ -0,0 +1,123 @@
// 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_ */

294
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/src/mtkmath.hpp Executable file
View File

@ -0,0 +1,294 @@
// 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_ */

168
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/src/vectview.hpp Executable file
View File

@ -0,0 +1,168 @@
/*
* 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_ */

328
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/startIdx.hpp Executable file
View File

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

316
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/types/S2.hpp Executable file
View File

@ -0,0 +1,316 @@
// 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_*/

317
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/types/SOn.hpp Executable file
View File

@ -0,0 +1,317 @@
// 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_*/

461
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/types/vect.hpp Executable file
View File

@ -0,0 +1,461 @@
// 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::complex<scalar> Base;
Complex(const Base& value) : Base(value) {}
Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {}
Complex(const MTK::vectview<const scalar, 2> &in) : Base(in[0], in[1]) {}
template<class Derived>
Complex(const Eigen::DenseBase<Derived> &in) : Base(in[0], in[1]) {}
void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale = 1) {
Base::real() += scale * vec[0];
Base::imag() += scale * vec[1];
};
void boxminus(MTK::vectview<scalar, DOF> res, const Complex& other) const {
Complex diff = *this - other;
res << diff.real(), diff.imag();
}
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) {
Base::real() += scale * vec[0];
Base::imag() += scale * vec[1];
};
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();
}
scalar squaredNorm() const {
return std::pow(Base::real(),2) + std::pow(Base::imag(),2);
}
const scalar& operator()(int i) const {
assert(0<=i && i<2 && "Index out of range");
return i==0 ? Base::real() : Base::imag();
}
scalar& operator()(int i){
assert(0<=i && i<2 && "Index out of range");
return i==0 ? Base::real() : Base::imag();
}
};
namespace internal {
template<int dim, class Scalar, int Options>
struct UnalignedType<vect<dim, Scalar, Options > >{
typedef vect<dim, Scalar, Options | Eigen::DontAlign> type;
};
} // namespace internal
} // namespace MTK
#endif /*VECT_H_*/

113
src/FAST_LIO_LOCALIZATION/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp Executable file
View File

@ -0,0 +1,113 @@
/*
* Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH
* All rights reserved.
*
* Author: Rene Wagner <rene.wagner@dfki.de>
* 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 DFKI GmbH
* 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 WRAPPED_CV_MAT_HPP_
#define WRAPPED_CV_MAT_HPP_
#include <Eigen/Core>
#include <opencv/cv.h>
namespace MTK {
template<class f_type>
struct cv_f_type;
template<>
struct cv_f_type<double>
{
enum {value = CV_64F};
};
template<>
struct cv_f_type<float>
{
enum {value = CV_32F};
};
/**
* cv_mat wraps a CvMat around an Eigen Matrix
*/
template<int rows, int cols, class f_type = double>
class cv_mat : public matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor>
{
typedef matrix<rows, cols, f_type, cols==1 ? Eigen::ColMajor : Eigen::RowMajor> base_type;
enum {type_ = cv_f_type<f_type>::value};
CvMat cv_mat_;
public:
cv_mat()
{
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
}
cv_mat(const cv_mat& oth) : base_type(oth)
{
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
}
template<class Derived>
cv_mat(const Eigen::MatrixBase<Derived> &value) : base_type(value)
{
cv_mat_ = cvMat(rows, cols, type_, base_type::data());
}
template<class Derived>
cv_mat& operator=(const Eigen::MatrixBase<Derived> &value)
{
base_type::operator=(value);
return *this;
}
cv_mat& operator=(const cv_mat& value)
{
base_type::operator=(value);
return *this;
}
// FIXME: Maybe overloading operator& is not a good idea ...
CvMat* operator&()
{
return &cv_mat_;
}
const CvMat* operator&() const
{
return &cv_mat_;
}
};
} // namespace MTK
#endif /* WRAPPED_CV_MAT_HPP_ */

258
src/FAST_LIO_LOCALIZATION/include/common_lib.h Normal file
View File

@ -0,0 +1,258 @@
#ifndef COMMON_LIB_H
#define COMMON_LIB_H
#include <so3_math.h>
#include <Eigen/Eigen>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <fast_lio/Pose6D.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
using namespace std;
using namespace Eigen;
#define USE_IKFOM
#define PI_M (3.14159265358)
#define G_m_s2 (9.81) // Gravaty const in GuangDong/China
#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3)
#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3)
#define CUBE_LEN (6.0)
#define LIDAR_SP_LEN (2)
#define INIT_COV (1)
#define NUM_MATCH_POINTS (5)
#define MAX_MEAS_DIM (10000)
#define VEC_FROM_ARRAY(v) v[0],v[1],v[2]
#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8]
#define CONSTRAIN(v,min,max) ((v>min)?((v<max)?v:max):min)
#define ARRAY_FROM_EIGEN(mat) mat.data(), mat.data() + mat.rows() * mat.cols()
#define STD_VEC_FROM_EIGEN(mat) vector<decltype(mat)::Scalar> (mat.data(), mat.data() + mat.rows() * mat.cols())
#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name))
typedef fast_lio::Pose6D Pose6D;
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
typedef Vector3d V3D;
typedef Matrix3d M3D;
typedef Vector3f V3F;
typedef Matrix3f M3F;
#define MD(a,b) Matrix<double, (a), (b)>
#define VD(a) Matrix<double, (a), 1>
#define MF(a,b) Matrix<float, (a), (b)>
#define VF(a) Matrix<float, (a), 1>
M3D Eye3d(M3D::Identity());
M3F Eye3f(M3F::Identity());
V3D Zero3d(0, 0, 0);
V3F Zero3f(0, 0, 0);
struct MeasureGroup // Lidar data and imu dates for the curent process
{
MeasureGroup()
{
lidar_beg_time = 0.0;
this->lidar.reset(new PointCloudXYZI());
};
double lidar_beg_time;
PointCloudXYZI::Ptr lidar;
deque<sensor_msgs::Imu::ConstPtr> imu;
};
struct StatesGroup
{
StatesGroup() {
this->rot_end = M3D::Identity();
this->pos_end = Zero3d;
this->vel_end = Zero3d;
this->bias_g = Zero3d;
this->bias_a = Zero3d;
this->gravity = Zero3d;
this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV;
this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001;
};
StatesGroup(const StatesGroup& b) {
this->rot_end = b.rot_end;
this->pos_end = b.pos_end;
this->vel_end = b.vel_end;
this->bias_g = b.bias_g;
this->bias_a = b.bias_a;
this->gravity = b.gravity;
this->cov = b.cov;
};
StatesGroup& operator=(const StatesGroup& b)
{
this->rot_end = b.rot_end;
this->pos_end = b.pos_end;
this->vel_end = b.vel_end;
this->bias_g = b.bias_g;
this->bias_a = b.bias_a;
this->gravity = b.gravity;
this->cov = b.cov;
return *this;
};
StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
{
StatesGroup a;
a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
a.pos_end = this->pos_end + state_add.block<3,1>(3,0);
a.vel_end = this->vel_end + state_add.block<3,1>(6,0);
a.bias_g = this->bias_g + state_add.block<3,1>(9,0);
a.bias_a = this->bias_a + state_add.block<3,1>(12,0);
a.gravity = this->gravity + state_add.block<3,1>(15,0);
a.cov = this->cov;
return a;
};
StatesGroup& operator+=(const Matrix<double, DIM_STATE, 1> &state_add)
{
this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0));
this->pos_end += state_add.block<3,1>(3,0);
this->vel_end += state_add.block<3,1>(6,0);
this->bias_g += state_add.block<3,1>(9,0);
this->bias_a += state_add.block<3,1>(12,0);
this->gravity += state_add.block<3,1>(15,0);
return *this;
};
Matrix<double, DIM_STATE, 1> operator-(const StatesGroup& b)
{
Matrix<double, DIM_STATE, 1> a;
M3D rotd(b.rot_end.transpose() * this->rot_end);
a.block<3,1>(0,0) = Log(rotd);
a.block<3,1>(3,0) = this->pos_end - b.pos_end;
a.block<3,1>(6,0) = this->vel_end - b.vel_end;
a.block<3,1>(9,0) = this->bias_g - b.bias_g;
a.block<3,1>(12,0) = this->bias_a - b.bias_a;
a.block<3,1>(15,0) = this->gravity - b.gravity;
return a;
};
void resetpose()
{
this->rot_end = M3D::Identity();
this->pos_end = Zero3d;
this->vel_end = Zero3d;
}
M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point
V3D pos_end; // the estimated position at the end lidar point (world frame)
V3D vel_end; // the estimated velocity at the end lidar point (world frame)
V3D bias_g; // gyroscope bias
V3D bias_a; // accelerator bias
V3D gravity; // the estimated gravity acceleration
Matrix<double, DIM_STATE, DIM_STATE> cov; // states covariance
};
template<typename T>
T rad2deg(T radians)
{
return radians * 180.0 / PI_M;
}
template<typename T>
T deg2rad(T degrees)
{
return degrees * PI_M / 180.0;
}
template<typename T>
auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, \
const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p, const Matrix<T, 3, 3> &R)
{
Pose6D rot_kp;
rot_kp.offset_time = t;
for (int i = 0; i < 3; i++)
{
rot_kp.acc[i] = a(i);
rot_kp.gyr[i] = g(i);
rot_kp.vel[i] = v(i);
rot_kp.pos[i] = p(i);
for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j);
}
return move(rot_kp);
}
/* comment
plane equation: Ax + By + Cz + D = 0
convert to: A/D*x + B/D*y + C/D*z = -1
solve: A0*x0 = b0
where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T
normvec: normalized x0
*/
template<typename T>
bool esti_normvector(Matrix<T, 3, 1> &normvec, const PointVector &point, const T &threshold, const int &point_num)
{
MatrixXf A(point_num, 3);
MatrixXf b(point_num, 1);
b.setOnes();
b *= -1.0f;
for (int j = 0; j < point_num; j++)
{
A(j,0) = point[j].x;
A(j,1) = point[j].y;
A(j,2) = point[j].z;
}
normvec = A.colPivHouseholderQr().solve(b);
for (int j = 0; j < point_num; j++)
{
if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold)
{
return false;
}
}
normvec.normalize();
return true;
}
float calc_dist(PointType p1, PointType p2){
float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
return d;
}
template<typename T>
bool esti_plane(Matrix<T, 4, 1> &pca_result, const PointVector &point, const T &threshold)
{
Matrix<T, NUM_MATCH_POINTS, 3> A;
Matrix<T, NUM_MATCH_POINTS, 1> b;
A.setZero();
b.setOnes();
b *= -1.0f;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
A(j,0) = point[j].x;
A(j,1) = point[j].y;
A(j,2) = point[j].z;
}
Matrix<T, 3, 1> normvec = A.colPivHouseholderQr().solve(b);
T n = normvec.norm();
pca_result(0) = normvec(0) / n;
pca_result(1) = normvec(1) / n;
pca_result(2) = normvec(2) / n;
pca_result(3) = 1.0 / n;
for (int j = 0; j < NUM_MATCH_POINTS; j++)
{
if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold)
{
return false;
}
}
return true;
}
#endif

2
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/README.md Normal file
View File

@ -0,0 +1,2 @@
# ikd-Tree
ikd-Tree is an incremental k-d tree for robotic applications.

1406
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

187
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.h Normal file
View File

@ -0,0 +1,187 @@
#pragma once
#include <pcl/point_types.h>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <stdio.h>
#include <queue>
#include <pthread.h>
#include <chrono>
#include <time.h>
#define EPSS 1e-6
#define Minimal_Unbalanced_Tree_Size 10
#define Multi_Thread_Rebuild_Point_Num 1500
#define DOWNSAMPLE_SWITCH true
#define ForceRebuildPercentage 0.2
#define Q_LEN 1000000
using namespace std;
typedef pcl::PointXYZINormal PointType;
typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVector;
const PointType ZeroP;
struct KD_TREE_NODE
{
PointType point;
int division_axis;
int TreeSize = 1;
int invalid_point_num = 0;
int down_del_num = 0;
bool point_deleted = false;
bool tree_deleted = false;
bool point_downsample_deleted = false;
bool tree_downsample_deleted = false;
bool need_push_down_to_left = false;
bool need_push_down_to_right = false;
bool working_flag = false;
pthread_mutex_t push_down_mutex_lock;
float node_range_x[2], node_range_y[2], node_range_z[2];
KD_TREE_NODE *left_son_ptr = nullptr;
KD_TREE_NODE *right_son_ptr = nullptr;
KD_TREE_NODE *father_ptr = nullptr;
// For paper data record
float alpha_del;
float alpha_bal;
};
struct PointType_CMP{
PointType point;
float dist = 0.0;
PointType_CMP (PointType p = ZeroP, float d = INFINITY){
this->point = p;
this->dist = d;
};
bool operator < (const PointType_CMP &a)const{
if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x;
else return dist < a.dist;
}
};
struct BoxPointType{
float vertex_min[3];
float vertex_max[3];
};
enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN};
enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC};
struct Operation_Logger_Type{
PointType point;
BoxPointType boxpoint;
bool tree_deleted, tree_downsample_deleted;
operation_set op;
};
class MANUAL_Q{
private:
int head = 0,tail = 0, counter = 0;
Operation_Logger_Type q[Q_LEN];
bool is_empty;
public:
void pop();
Operation_Logger_Type front();
Operation_Logger_Type back();
void clear();
void push(Operation_Logger_Type op);
bool empty();
int size();
};
class MANUAL_HEAP
{
public:
MANUAL_HEAP(int max_capacity = 100);
~MANUAL_HEAP();
void pop();
PointType_CMP top();
void push(PointType_CMP point);
int size();
void clear();
private:
PointType_CMP * heap;
void MoveDown(int heap_index);
void FloatUp(int heap_index);
int heap_size = 0;
int cap = 0;
};
class KD_TREE
{
private:
// Multi-thread Tree Rebuild
bool termination_flag = false;
bool rebuild_flag = false;
pthread_t rebuild_thread;
pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex;
pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
// queue<Operation_Logger_Type> Rebuild_Logger;
MANUAL_Q Rebuild_Logger;
PointVector Rebuild_PCL_Storage;
KD_TREE_NODE ** Rebuild_Ptr = nullptr;
int search_mutex_counter = 0;
static void * multi_thread_ptr(void *arg);
void multi_thread_rebuild();
void start_thread();
void stop_thread();
void run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation);
// KD Tree Functions and augmented variables
int Treesize_tmp = 0, Validnum_tmp = 0;
float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;
float delete_criterion_param = 0.5f;
float balance_criterion_param = 0.7f;
float downsample_size = 0.2f;
bool Delete_Storage_Disabled = false;
KD_TREE_NODE * STATIC_ROOT_NODE = nullptr;
PointVector Points_deleted;
PointVector Downsample_Storage;
PointVector Multithread_Points_deleted;
void InitTreeNode(KD_TREE_NODE * root);
void Test_Lock_States(KD_TREE_NODE *root);
void BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage);
void Rebuild(KD_TREE_NODE ** root);
int Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample);
void Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild);
void Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis);
void Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild);
void Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist);//priority_queue<PointType_CMP>
void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage);
bool Criterion_Check(KD_TREE_NODE * root);
void Push_Down(KD_TREE_NODE * root);
void Update(KD_TREE_NODE * root);
void delete_tree_nodes(KD_TREE_NODE ** root);
void downsample(KD_TREE_NODE ** root);
bool same_point(PointType a, PointType b);
float calc_dist(PointType a, PointType b);
float calc_box_dist(KD_TREE_NODE * node, PointType point);
static bool point_cmp_x(PointType a, PointType b);
static bool point_cmp_y(PointType a, PointType b);
static bool point_cmp_z(PointType a, PointType b);
public:
KD_TREE(float delete_param = 0.5, float balance_param = 0.6 , float box_length = 0.2);
~KD_TREE();
void Set_delete_criterion_param(float delete_param);
void Set_balance_criterion_param(float balance_param);
void set_downsample_param(float box_length);
void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2);
int size();
int validnum();
void root_alpha(float &alpha_bal, float &alpha_del);
void Build(PointVector point_cloud);
void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector<float> & Point_Distance, double max_dist = INFINITY);
int Add_Points(PointVector & PointToAdd, bool downsample_on);
void Add_Point_Boxes(vector<BoxPointType> & BoxPoints);
void Delete_Points(PointVector & PointToDel);
int Delete_Point_Boxes(vector<BoxPointType> & BoxPoints);
void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type);
void acquire_removed_points(PointVector & removed_points);
BoxPointType tree_range();
PointVector PCL_Storage;
KD_TREE_NODE * Root_Node = nullptr;
int max_queue_size = 0;
};

2499
src/FAST_LIO_LOCALIZATION/include/matplotlibcpp.h Normal file
View File

File diff suppressed because it is too large Load Diff

111
src/FAST_LIO_LOCALIZATION/include/so3_math.h Normal file
View File

@ -0,0 +1,111 @@
#ifndef SO3_MATH_H
#define SO3_MATH_H
#include <math.h>
#include <Eigen/Core>
#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> skew_sym_mat(const Eigen::Matrix<T, 3, 1> &v)
{
Eigen::Matrix<T, 3, 3> skew_sym_mat;
skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0;
return skew_sym_mat;
}
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 = (R.trace() > 3.0 - 1e-6) ? 0.0 : 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>
Eigen::Matrix<T, 3, 1> RotMtoEuler(const Eigen::Matrix<T, 3, 3> &rot)
{
T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0));
bool singular = sy < 1e-6;
T x, y, z;
if(!singular)
{
x = atan2(rot(2, 1), rot(2, 2));
y = atan2(-rot(2, 0), sy);
z = atan2(rot(1, 0), rot(0, 0));
}
else
{
x = atan2(-rot(1, 2), rot(1, 1));
y = atan2(-rot(2, 0), sy);
z = 0;
}
Eigen::Matrix<T, 3, 1> ang(x, y, z);
return ang;
}
#endif

126
src/FAST_LIO_LOCALIZATION/include/use-ikfom.hpp Normal file
View File

@ -0,0 +1,126 @@
#ifndef USE_IKFOM_H
#define USE_IKFOM_H
#include <IKFoM_toolkit/esekfom/esekfom.hpp>
typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2;
typedef MTK::vect<1, double> vect1;
typedef MTK::vect<2, double> vect2;
MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((S2, grav))
);
MTK_BUILD_MANIFOLD(input_ikfom,
((vect3, acc))
((vect3, gyro))
);
MTK_BUILD_MANIFOLD(process_noise_ikfom,
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);
MTK::get_cov<process_noise_ikfom>::type process_noise_cov()
{
MTK::get_cov<process_noise_ikfom>::type cov = MTK::get_cov<process_noise_ikfom>::type::Zero();
MTK::setDiagonal<process_noise_ikfom, vect3, 0>(cov, &process_noise_ikfom::ng, 0.0001);// 0.03
MTK::setDiagonal<process_noise_ikfom, vect3, 3>(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05
MTK::setDiagonal<process_noise_ikfom, vect3, 6>(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01
MTK::setDiagonal<process_noise_ikfom, vect3, 9>(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01
return cov;
}
//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia
//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3);
Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
vect3 omega;
in.gyro.boxminus(omega, s.bg);
vect3 a_inertial = s.rot * (in.acc-s.ba);
for(int i = 0; i < 3; i++ ){
res(i) = s.vel[i];
res(i + 3) = omega[i];
res(i + 12) = a_inertial[i] + s.grav[i];
}
return res;
}
Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 23> cov = Eigen::Matrix<double, 24, 23>::Zero();
cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity();
vect3 acc_;
in.acc.boxminus(acc_, s.ba);
vect3 omega;
in.gyro.boxminus(omega, s.bg);
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_);
cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix();
Eigen::Matrix<state_ikfom::scalar, 2, 1> vec = Eigen::Matrix<state_ikfom::scalar, 2, 1>::Zero();
Eigen::Matrix<state_ikfom::scalar, 3, 2> grav_matrix;
s.S2_Mx(grav_matrix, vec, 21);
cov.template block<3, 2>(12, 21) = grav_matrix;
cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity();
return cov;
}
Eigen::Matrix<double, 24, 12> df_dw(state_ikfom &s, const input_ikfom &in)
{
Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix();
cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity();
cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity();
cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity();
return cov;
}
vect3 SO3ToEuler(const SO3 &orient)
{
Eigen::Matrix<double, 3, 1> _ang;
Eigen::Vector4d q_data = orient.coeffs().transpose();
//scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2];
double sqw = q_data[3]*q_data[3];
double sqx = q_data[0]*q_data[0];
double sqy = q_data[1]*q_data[1];
double sqz = q_data[2]*q_data[2];
double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor
double test = q_data[3]*q_data[1] - q_data[2]*q_data[0];
if (test > 0.49999*unit) { // singularity at north pole
_ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0;
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
if (test < -0.49999*unit) { // singularity at south pole
_ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0;
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
return euler_ang;
}
_ang <<
std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw),
std::asin (2*test/unit),
std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw);
double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3};
vect3 euler_ang(temp, 3);
// euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw
return euler_ang;
}
#endif

23
src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch Normal file
View File

@ -0,0 +1,23 @@
<launch>
<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/cloud_registered"/>
<rosparam>
transform_tolerance: 0.01
min_height: 0.0
max_height: 1.0
angle_min: -3.14159 # -M_PI/2
angle_max: 3.14159 # M_PI/2
angle_increment: 0.0087 # M_PI/360.0
scan_time: 10
range_min: 0.05
range_max: 30.0
use_inf: true
inf_epsilon: 1.0
concurrency_level: 1
</rosparam>
</node>
</launch>

16
src/FAST_LIO_LOCALIZATION/launch/align_pcd_map.launch Normal file
View File

@ -0,0 +1,16 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" />
<!-- load 2d map -->
<arg name="2dmap" default="scans.yaml" />
<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio)/PCD/scans.yaml /map:=prior_map"/>
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</launch>

22
src/FAST_LIO_LOCALIZATION/launch/gdb_debug_example.launch Normal file
View File

@ -0,0 +1,22 @@
<launch>
<arg name="rviz" default="true" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" required="true" launch-prefix="gdb -ex run --args">
<param name="imu_topic" type="string" value="/livox/imu" />
<param name="map_file_path" type="string" value=" " />
<param name="max_iteration" type="int" value="4" />
<param name="dense_map_enable" type="bool" value="1" />
<param name="fov_degree" type="double" value="75" />
<param name="filter_size_corner" type="double" value="0.2" />
<param name="filter_size_surf" type="double" value="0.2" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="runtime_pos_log_enable" type="bool" value="1" />
<param name="cube_side_length" type="double" value="2000" />
</node>
<!-- <group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
</group> -->
</launch>

47
src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch Normal file
View File

@ -0,0 +1,47 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<!-- <arg name="map" default="/home/rm/ws_sentry/src/FAST_LIO/PCD(305_10-20)/scans.pcd" /> -->
<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
<!-- loalization-->
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
<!-- transform fusion-->
<node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
<!-- build 2d map -->
<!-- <include file="$(find fast_lio)/launch/Pointcloud2Map.launch">
</include> -->
<!-- load 2d map -->
<arg name="2dmap" default="scans.yaml" />
<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio)/PCD/m4.yaml /map:=prior_map"/>
<!-- pointscloud2 to laserscans -->
<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">
</include>
</launch>

35
src/FAST_LIO_LOCALIZATION/launch/localization_avia.launch Normal file
View File

@ -0,0 +1,35 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio_localization)/config/avia.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="1"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
<arg name="map" default="/home/rm/ws_sentry/src/FAST_LIO/PCD(305_10-20)/scans.pcd" />
<!-- loalization-->
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
<!-- transform fusion-->
<node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
</launch>

34
src/FAST_LIO_LOCALIZATION/launch/localization_horizon.launch Normal file
View File

@ -0,0 +1,34 @@
<launch>
<!-- Launch file for Livox Horizon LiDAR -->
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio_localization)/config/horizon.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="dense_publish_enable" type="bool" value="1" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
<arg name="map" default="" />
<!-- loalization-->
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
<!-- transform fusion-->
<node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
</launch>

34
src/FAST_LIO_LOCALIZATION/launch/localization_ouster64.launch Normal file
View File

@ -0,0 +1,34 @@
<launch>
<!-- Launch file for ouster OS2-64 LiDAR -->
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio_localization)/config/ouster64.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="4"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
<arg name="map" default="" />
<!-- loalization-->
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
<!-- transform fusion-->
<node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
</launch>

34
src/FAST_LIO_LOCALIZATION/launch/localization_velodyne.launch Normal file
View File

@ -0,0 +1,34 @@
<launch>
<!-- Launch file for velodyne16 VLP-16 LiDAR -->
<arg name="rviz" default="true" />
<rosparam command="load" file="$(find fast_lio_localization)/config/velodyne.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="4"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name="pcd_save_enable" type="bool" value="0" />
<node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
<arg name="map" default="" />
<!-- loalization-->
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
<!-- transform fusion-->
<node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
</launch>

48
src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch Normal file
View File

@ -0,0 +1,48 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<arg name="use_sim_time" value="true"/>
<arg name="map" default="$(find fast_lio)/PCD/scanss.pcd" />
<!-- fast_lio -->
<rosparam command="load" file="$(find fast_lio)/config/mid360.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<!-- loalization-->
<!-- <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" /> -->
<!-- transform fusion-->
<!-- <node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" /> -->
<!-- glbal map-->
<!-- <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" /> -->
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/sentry_build_map.rviz" />
</group>
<!-- build 2d map -->
<include file="$(find fast_lio)/launch/Pointcloud2Map.launch">
</include>
<!-- pointscloud2 to laserscans -->
<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">
</include>
<!-- 发布一个雷达body到机器人足端body_foot的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_1" args="-0.10 -0.11 0 0 0 0 body body_foot" />
<!-- 发布一个雷达初始位置camera_init到机器人足端初始位置body_init的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_2" args="-0.10 -0.11 0 0 0 0 camera_init robot_foot_init" />
</launch>

53
src/FAST_LIO_LOCALIZATION/launch/sentry_localize.launch Normal file
View File

@ -0,0 +1,53 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<arg name="use_sim_time" value="true"/>
<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
<!-- fast_lio -->
<rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name= "pcd_save_en" type="bool" value="false"/>
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<!-- loalization-->
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
<!-- <node pkg="fast_lio_localization" type="flag" name="flag" /> -->
<!-- transform fusion-->
<node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" />
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
<!-- load 2d map -->
<arg name="2dmap" default="m4.yaml" />
<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio)/PCD/m4.yaml /map:=prior_map"/>
<!-- pointscloud2 to laserscans -->
<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">
</include>
<!-- 发布一个雷达body到机器人足端body_foot的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_1" args="-0.10 -0.11 0 0 0 0 body body_foot" />
<!-- 发布一个雷达初始位置camera_init到机器人足端初始位置body_init的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_2" args="-0.10 -0.11 0 0 0 0 camera_init robot_foot_init" />
<!-- 发布一个雷达初始位置camera_init到map的静态映射 -->
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_3" args="0 0 0 0 0 0 map camera_init" /> -->
</launch>

51
src/FAST_LIO_LOCALIZATION/launch/sentry_localize_odom.launch Normal file
View File

@ -0,0 +1,51 @@
<launch>
<!-- Launch file for Livox AVIA LiDAR -->
<arg name="rviz" default="true" />
<arg name="use_sim_time" value="true"/>
<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
<!-- fast_lio -->
<rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />
<param name="feature_extract_enable" type="bool" value="0"/>
<param name="point_filter_num" type="int" value="3"/>
<param name="max_iteration" type="int" value="3" />
<param name="filter_size_surf" type="double" value="0.5" />
<param name="filter_size_map" type="double" value="0.5" />
<param name="cube_side_length" type="double" value="1000" />
<param name="runtime_pos_log_enable" type="bool" value="0" />
<param name= "pcd_save_en" type="bool" value="false"/>
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
<!-- loalization-->
<!-- transform fusion-->
<!-- glbal map-->
<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" />
<!-- load 2d map -->
<arg name="2dmap" default="scans.yaml" />
<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio)/PCD/2.yaml /map:=prior_map"/>
<group if="$(arg rviz)">
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
</group>
<!-- pointscloud2 to laserscans -->
<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">
</include>
<!-- 发布一个雷达body到机器人足端body_foot的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_1" args="-0.10 -0.11 0 0 0 0 body body_foot" />
<!-- 发布一个雷达初始位置camera_init到机器人足端初始位置body_init的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_2" args="-0.10 -0.11 0 0 0 0 camera_init robot_foot_init" />
<!-- 发布一个雷达初始位置camera_init到map的静态映射 -->
<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_3" args="0 0 0 0 0 0 map camera_init" />
</launch>

BIN
src/FAST_LIO_LOCALIZATION/m4.pgm Normal file
View File

Binary file not shown.

7
src/FAST_LIO_LOCALIZATION/msg/Pose6D.msg Normal file
View File

@ -0,0 +1,7 @@
# the preintegrated Lidar states at the time of IMU measurements in a frame
float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point
float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin
float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin
float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin
float64[3] pos # the preintegrated position (global frame) at the Lidar origin
float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin

47
src/FAST_LIO_LOCALIZATION/package.xml Normal file
View File

@ -0,0 +1,47 @@
<?xml version="1.0"?>
<package>
<name>fast_lio_localization</name>
<version>0.0.0</version>
<description>
This is a modified version of LOAM which is original algorithm
is described in the following paper:
J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
</description>
<maintainer email="dev@livoxtech.com">claydergc</maintainer>
<license>BSD</license>
<author email="zhangji@cmu.edu">Ji Zhang</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>livox_ros_driver2</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>livox_ros_driver2</run_depend>
<run_depend>message_runtime</run_depend>
<test_depend>rostest</test_depend>
<test_depend>rosbag</test_depend>
<export>
</export>
</package>

0
src/FAST_LIO_LOCALIZATION/rviz_cfg/.gitignore vendored Normal file
View File

366
src/FAST_LIO_LOCALIZATION/rviz_cfg/loam_livox.rviz Normal file
View File

@ -0,0 +1,366 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /mapping1
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/currPoints1/Autocompute Value Bounds1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Odometry1/Odometry1/Covariance1
- /Odometry1/Odometry1/Covariance1/Position1
- /Odometry1/Odometry1/Covariance1/Orientation1
- /MarkerArray1/Namespaces1
Splitter Ratio: 0.6432291865348816
Tree Height: 1139
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 1000
Class: rviz/Grid
Color: 160; 160; 164
Enabled: false
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 40
Reference Frame: <Fixed Frame>
Value: false
- Class: rviz/Axes
Enabled: false
Length: 0.699999988079071
Name: Axes
Radius: 0.05999999865889549
Reference Frame: <Fixed Frame>
Value: false
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 70
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 151
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: mapping
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.009999999776482582
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.20000000298023224
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /Odometry
Unreliable: false
Value: true
Enabled: true
Name: Odometry
- Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 0
Buffer Length: 2
Class: rviz/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.20000000298023224
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 239; 41; 41
Max Intensity: 0
Min Color: 239; 41; 41
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 4
Size (m): 0.30000001192092896
Style: Spheres
Topic: /cloud_effected
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 13.139549255371094
Min Value: -32.08251953125
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 138; 226; 52
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 138; 226; 52
Max Intensity: 248
Min Color: 138; 226; 52
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /MarkerArray
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: camera_init
Frame Rate: 10
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 19.453998565673828
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.0753980875015259
Target Frame: body
Value: Orbit (rviz)
Yaw: 2.4953973293304443
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1385
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001c8000004b3fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000004b3000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000004b3000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009c100000052fc0100000002fb0000000800540069006d00650100000000000009c10000031300fffffffb0000000800540069006d006501000000000000045000000000000000000000069b000004b300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2497
X: 63
Y: 27

566
src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz Normal file
View File

@ -0,0 +1,566 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/currPoints1/Autocompute Value Bounds1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Odometry1/Odometry1/Covariance1
- /Odometry1/Odometry1/Covariance1/Position1
- /Odometry1/Odometry1/Covariance1/Orientation1
- /Localization1/localization1
- /Localization1/localization1/Shape1
- /Path1
- /MarkerArray1/Namespaces1
Splitter Ratio: 0.6382352709770203
Tree Height: 417
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 0.20000000298023224
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: body
Value: true
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 0.699999988079071
Name: Axes
Radius: 0.05999999865889549
Reference Frame: body_2d
Show Trail: false
Value: false
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 70
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: false
Name: mapping
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.009999999776482582
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.20000000298023224
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /Odometry
Unreliable: false
Value: true
Enabled: false
Name: Odometry
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 252; 233; 79
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: cur_scan_in_map
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /cur_scan_in_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.5
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 138; 226; 52
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: submap_in_FOV
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /submap
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 25; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: map
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: localization
Position Tolerance: 0.10000000149011612
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 138; 226; 52
Head Length: 0.20000000298023224
Head Radius: 0.10000000149011612
Shaft Length: 0.4000000059604645
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /localization
Unreliable: false
Value: true
Enabled: false
Name: Localization
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: body
Show Trail: false
Value: false
- Alpha: 0
Buffer Length: 3
Class: rviz/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.10000000149011612
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /move_base1/DWAPlannerROS/local_plan
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /MarkerArray
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /projected_map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0
Min Value: 0
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 138; 226; 52
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /prior_map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /move_base1/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Pose
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Arrow
Topic: /move_base_simple/goal
Unreliable: false
Value: true
- Class: rviz/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
body:
Value: true
body_2d:
Value: true
body_foot:
Value: true
camera_init:
Value: true
map:
Value: true
robot_foot_init:
Value: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
map:
body_2d:
{}
camera_init:
body:
body_foot:
{}
robot_foot_init:
{}
Update Interval: 0
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /move_base1/NavfnROS/plan
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: map
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 9.772259712219238
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 1.974967360496521
Y: -0.12288086116313934
Z: -0.07930535078048706
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.0747967958450317
Target Frame: <Fixed Frame>
Yaw: 3.1467628479003906
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 656
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000001defc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000001defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001de000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b800000052fc0100000002fb0000000800540069006d00650100000000000004b8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000035c000001de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1208
X: 72
Y: 27

521
src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz Normal file
View File

@ -0,0 +1,521 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Grid1
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/currPoints1/Autocompute Value Bounds1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Odometry1/Odometry1/Covariance1
- /Odometry1/Odometry1/Covariance1/Position1
- /Odometry1/Odometry1/Covariance1/Orientation1
- /Localization1/map1
- /Localization1/localization1
- /Localization1/localization1/Shape1
- /MarkerArray1/Namespaces1
- /Axes3/Status1
Splitter Ratio: 0.6382352709770203
Tree Height: 467
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: surround
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 0.20000000298023224
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: body
Value: true
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1000
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 70
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Name: mapping
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.009999999776482582
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.20000000298023224
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /Odometry
Unreliable: false
Value: true
Enabled: false
Name: Odometry
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 252; 233; 79
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: cur_scan_in_map
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /cur_scan_in_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.5
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 138; 226; 52
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: submap_in_FOV
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /submap
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 25; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: map
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 500
Name: localization
Position Tolerance: 0.10000000149011612
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 138; 226; 52
Head Length: 0.30000001192092896
Head Radius: 0.20000000298023224
Shaft Length: 1
Shaft Radius: 0.10000000149011612
Value: Arrow
Topic: /localization
Unreliable: false
Value: true
Enabled: false
Name: Localization
- Alpha: 0
Buffer Length: 2
Class: rviz/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.20000000298023224
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /MarkerArray
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /projected_map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0
Min Value: 0
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /prior_map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: camera_init
Show Trail: false
Value: false
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 0.699999988079071
Name: Axes
Radius: 0.05999999865889549
Reference Frame: body
Show Trail: false
Value: false
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: map
Show Trail: false
Value: false
- Class: rviz/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
body:
Value: true
body_foot:
Value: true
camera_init:
Value: true
robot_foot_init:
Value: true
Marker Alpha: 1
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
camera_init:
body:
body_foot:
{}
robot_foot_init:
{}
Update Interval: 0
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: robot_foot_init
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 27.20014190673828
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 3.642894744873047
Y: 0.2535877823829651
Z: -0.6714627742767334
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.279795527458191
Target Frame: body
Yaw: 0.9917507767677307
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 706
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000015600000210fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000004b3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044e00000052fc0100000002fb0000000800540069006d006501000000000000044e000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f20000021000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1102
X: 191
Y: 33

481
src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_localize.rviz Normal file
View File

@ -0,0 +1,481 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Grid1
- /mapping1/surround1
- /mapping1/currPoints1
- /mapping1/currPoints1/Autocompute Value Bounds1
- /Odometry1/Odometry1
- /Odometry1/Odometry1/Shape1
- /Odometry1/Odometry1/Covariance1
- /Odometry1/Odometry1/Covariance1/Position1
- /Odometry1/Odometry1/Covariance1/Orientation1
- /Localization1/map1
- /Localization1/localization1
- /Localization1/localization1/Shape1
- /MarkerArray1/Namespaces1
Splitter Ratio: 0.6382352709770203
Tree Height: 777
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: map
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 1
Cell Size: 0.20000000298023224
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: body
Value: true
- Alpha: 1
Class: rviz/Axes
Enabled: false
Length: 0.699999988079071
Name: Axes
Radius: 0.05999999865889549
Reference Frame: <Fixed Frame>
Show Trail: false
Value: false
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 238; 238; 236
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: surround
Position Transformer: XYZ
Queue Size: 1
Selectable: false
Size (Pixels): 1
Size (m): 0.05000000074505806
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 15
Min Value: -5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 1000
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 70
Min Color: 0; 0; 0
Min Intensity: 0
Name: currPoints
Position Transformer: XYZ
Queue Size: 100000
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /cloud_registered
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /Laser_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: false
Name: mapping
- Class: rviz/Group
Displays:
- Angle Tolerance: 0.009999999776482582
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1
Name: Odometry
Position Tolerance: 0.0010000000474974513
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.20000000298023224
Color: 255; 85; 0
Head Length: 0
Head Radius: 0
Shaft Length: 0.05000000074505806
Shaft Radius: 0.05000000074505806
Value: Axes
Topic: /Odometry
Unreliable: false
Value: true
Enabled: false
Name: Odometry
- Class: rviz/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 252; 233; 79
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: cur_scan_in_map
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /cur_scan_in_map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.5
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 138; 226; 52
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: submap_in_FOV
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /submap
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.6000000238418579
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 25; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: map
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 1
Size (m): 0.009999999776482582
Style: Points
Topic: /map
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 500
Name: localization
Position Tolerance: 0.10000000149011612
Queue Size: 10
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 138; 226; 52
Head Length: 0.30000001192092896
Head Radius: 0.20000000298023224
Shaft Length: 1
Shaft Radius: 0.10000000149011612
Value: Arrow
Topic: /localization
Unreliable: false
Value: true
Enabled: true
Name: Localization
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 0.699999988079071
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: true
- Alpha: 0
Buffer Length: 2
Class: rviz/Path
Color: 25; 255; 255
Enabled: true
Head Diameter: 0
Head Length: 0
Length: 0.30000001192092896
Line Style: Billboards
Line Width: 0.20000000298023224
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 25; 255; 255
Pose Style: None
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.4000000059604645
Shaft Length: 0.4000000059604645
Topic: /path
Unreliable: false
Value: true
- Class: rviz/MarkerArray
Enabled: false
Marker Topic: /MarkerArray
Name: MarkerArray
Namespaces:
{}
Queue Size: 100
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /projected_map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0
Min Value: 0
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /prior_map
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Global Options:
Background Color: 0; 0; 0
Default Light: true
Fixed Frame: map
Frame Rate: 20
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 24.35710334777832
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: -0.18300682306289673
Y: 0.05401577427983284
Z: -0.6143645644187927
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.9147980213165283
Target Frame: body
Yaw: 1.3385640382766724
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000015600000346fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000346000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f000004b3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1848
X: 72
Y: 27

260
src/FAST_LIO_LOCALIZATION/scripts/global_localization.py Executable file
View File

@ -0,0 +1,260 @@
#!/usr/bin/python3
# coding=utf8
from __future__ import print_function, division, absolute_import
import copy
import _thread
import time
import open3d as o3d
import rospy
import ros_numpy
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion
from nav_msgs.msg import Odometry
from sensor_msgs.msg import PointCloud2
import numpy as np
import tf
import tf.transformations
global_map = None
initialized = False
T_map_to_odom = np.eye(4)
cur_odom = None
cur_scan = None
def pose_to_mat(pose_msg):
return np.matmul(
tf.listener.xyz_to_mat44(pose_msg.pose.pose.position),
tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation),
)
def msg_to_array(pc_msg):
pc_array = ros_numpy.numpify(pc_msg)
pc = np.zeros([len(pc_array), 3])
pc[:, 0] = pc_array['x']
pc[:, 1] = pc_array['y']
pc[:, 2] = pc_array['z']
return pc
def registration_at_scale(pc_scan, pc_map, initial, scale):
result_icp = o3d.pipelines.registration.registration_icp(
voxel_down_sample(pc_scan, SCAN_VOXEL_SIZE * scale), voxel_down_sample(pc_map, MAP_VOXEL_SIZE * scale),
1.0 * scale, initial,
o3d.pipelines.registration.TransformationEstimationPointToPoint(),
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20)
)
return result_icp.transformation, result_icp.fitness
def inverse_se3(trans):
trans_inverse = np.eye(4)
# R
trans_inverse[:3, :3] = trans[:3, :3].T
# t
trans_inverse[:3, 3] = -np.matmul(trans[:3, :3].T, trans[:3, 3])
return trans_inverse
def publish_point_cloud(publisher, header, pc):
data = np.zeros(len(pc), dtype=[
('x', np.float32),
('y', np.float32),
('z', np.float32),
('intensity', np.float32),
])
data['x'] = pc[:, 0]
data['y'] = pc[:, 1]
data['z'] = pc[:, 2]
if pc.shape[1] == 4:
data['intensity'] = pc[:, 3]
msg = ros_numpy.msgify(PointCloud2, data)
msg.header = header
publisher.publish(msg)
def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom):
# 当前scan原点的位姿
T_odom_to_base_link = pose_to_mat(cur_odom)
T_map_to_base_link = np.matmul(pose_estimation, T_odom_to_base_link)
T_base_link_to_map = inverse_se3(T_map_to_base_link)
# 把地图转换到lidar系下
global_map_in_map = np.array(global_map.points)
global_map_in_map = np.column_stack([global_map_in_map, np.ones(len(global_map_in_map))])
global_map_in_base_link = np.matmul(T_base_link_to_map, global_map_in_map.T).T
# 将视角内的地图点提取出来
if FOV > 3.14:
# 环状lidar 仅过滤距离
indices = np.where(
(global_map_in_base_link[:, 0] < FOV_FAR) &
(np.abs(np.arctan2(global_map_in_base_link[:, 1], global_map_in_base_link[:, 0])) < FOV / 2.0)
)
else:
# 非环状lidar 保前视范围
# FOV_FAR>x>0 且角度小于FOV
indices = np.where(
(global_map_in_base_link[:, 0] > 0) &
(global_map_in_base_link[:, 0] < FOV_FAR) &
(np.abs(np.arctan2(global_map_in_base_link[:, 1], global_map_in_base_link[:, 0])) < FOV / 2.0)
)
global_map_in_FOV = o3d.geometry.PointCloud()
global_map_in_FOV.points = o3d.utility.Vector3dVector(np.squeeze(global_map_in_map[indices, :3]))
# 发布fov内点云
header = cur_odom.header
header.frame_id = 'map'
publish_point_cloud(pub_submap, header, np.array(global_map_in_FOV.points)[::10])
return global_map_in_FOV
def global_localization(pose_estimation):
global global_map, cur_scan, cur_odom, T_map_to_odom
# 用icp配准
# print(global_map, cur_scan, T_map_to_odom)
rospy.loginfo('Global localization by scan-to-map matching......')
# TODO 这里注意线程安全
scan_tobe_mapped = copy.copy(cur_scan)
tic = time.time()
global_map_in_FOV = crop_global_map_in_FOV(global_map, pose_estimation, cur_odom)
# 粗配准
transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=5)
# 精配准
transformation, fitness = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=transformation,
scale=1)
toc = time.time()
rospy.loginfo('Time: {}'.format(toc - tic))
rospy.loginfo('')
# 当全局定位成功时才更新map2odom
if fitness > LOCALIZATION_TH:
# T_map_to_odom = np.matmul(transformation, pose_estimation)
T_map_to_odom = transformation
# 发布map_to_odom
map_to_odom = Odometry()
xyz = tf.transformations.translation_from_matrix(T_map_to_odom)
quat = tf.transformations.quaternion_from_matrix(T_map_to_odom)
map_to_odom.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
map_to_odom.header.stamp = cur_odom.header.stamp
map_to_odom.header.frame_id = 'map'
pub_map_to_odom.publish(map_to_odom)
return True
else:
rospy.logwarn('Not match!!!!')
rospy.logwarn('{}'.format(transformation))
rospy.logwarn('fitness score:{}'.format(fitness))
return False
def voxel_down_sample(pcd, voxel_size):
try:
pcd_down = pcd.voxel_down_sample(voxel_size)
except:
# for opend3d 0.7 or lower
pcd_down = o3d.geometry.voxel_down_sample(pcd, voxel_size)
return pcd_down
def initialize_global_map(pc_msg):
global global_map
global_map = o3d.geometry.PointCloud()
global_map.points = o3d.utility.Vector3dVector(msg_to_array(pc_msg)[:, :3])
global_map = voxel_down_sample(global_map, MAP_VOXEL_SIZE)
rospy.loginfo('Global map received.')
def cb_save_cur_odom(odom_msg):
global cur_odom
cur_odom = odom_msg
def cb_save_cur_scan(pc_msg):
global cur_scan
# 注意这里fastlio直接将scan转到odom系下了 不是lidar局部系
pc_msg.header.frame_id = 'camera_init'
pc_msg.header.stamp = rospy.Time().now()
pub_pc_in_map.publish(pc_msg)
# 转换为pcd
# fastlio给的field有问题 处理一下
pc_msg.fields = [pc_msg.fields[0], pc_msg.fields[1], pc_msg.fields[2],
pc_msg.fields[4], pc_msg.fields[5], pc_msg.fields[6],
pc_msg.fields[3], pc_msg.fields[7]]
pc = msg_to_array(pc_msg)
cur_scan = o3d.geometry.PointCloud()
cur_scan.points = o3d.utility.Vector3dVector(pc[:, :3])
def thread_localization():
global T_map_to_odom
while True:
# 每隔一段时间进行全局定位
rospy.sleep(1 / FREQ_LOCALIZATION)
# TODO 由于这里Fast lio发布的scan是已经转换到odom系下了 所以每次全局定位的初始解就是上一次的map2odom 不需要再拿odom了
global_localization(T_map_to_odom)
if __name__ == '__main__':
MAP_VOXEL_SIZE = 0.4
SCAN_VOXEL_SIZE = 0.1
# Global localization frequency (HZ)
FREQ_LOCALIZATION = 0.5
# The threshold of global localization,
# only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
LOCALIZATION_TH = 0.95
# FOV(rad), modify this according to your LiDAR type
FOV = 6.28
# The farthest distance(meters) within FOV
FOV_FAR = 30
rospy.init_node('fast_lio_localization')
rospy.loginfo('Localization Node Inited...')
# publisher
pub_pc_in_map = rospy.Publisher('/cur_scan_in_map', PointCloud2, queue_size=1)
pub_submap = rospy.Publisher('/submap', PointCloud2, queue_size=1)
pub_map_to_odom = rospy.Publisher('/map_to_odom', Odometry, queue_size=1)
rospy.Subscriber('/cloud_registered', PointCloud2, cb_save_cur_scan, queue_size=1)
rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1)
# 初始化全局地图
rospy.logwarn('Waiting for global map......')
initialize_global_map(rospy.wait_for_message('/map', PointCloud2))
# 初始化
while not initialized:
rospy.logwarn('Waiting for initial pose....')
# 等待初始位姿
pose_msg = rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
initial_pose = pose_to_mat(pose_msg)
if cur_scan:
initialized = global_localization(initial_pose)
else:
rospy.logwarn('First scan not received!!!!!')
rospy.loginfo('')
rospy.loginfo('Initialize successfully!!!!!!')
rospy.loginfo('')
# 开始定期全局定位
_thread.start_new_thread(thread_localization, ())
rospy.spin()

35
src/FAST_LIO_LOCALIZATION/scripts/publish_initial_pose.py Executable file
View File

@ -0,0 +1,35 @@
#!/usr/bin/python3
# coding=utf8
from __future__ import print_function, division, absolute_import
import argparse
import rospy
import tf.transformations
from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('x', type=float)
parser.add_argument('y', type=float)
parser.add_argument('z', type=float)
parser.add_argument('yaw', type=float)
parser.add_argument('pitch', type=float)
parser.add_argument('roll', type=float)
args = parser.parse_args()
rospy.init_node('publish_initial_pose')
pub_pose = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)
# 转换为pose
quat = tf.transformations.quaternion_from_euler(args.roll, args.pitch, args.yaw)
xyz = [args.x, args.y, args.z]
initial_pose = PoseWithCovarianceStamped()
initial_pose.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
initial_pose.header.stamp = rospy.Time().now()
initial_pose.header.frame_id = 'map'
rospy.sleep(1)
rospy.loginfo('Initial Pose: {} {} {} {} {} {}'.format(
args.x, args.y, args.z, args.yaw, args.pitch, args.roll, ))
pub_pose.publish(initial_pose)

88
src/FAST_LIO_LOCALIZATION/scripts/transform_fusion.py Executable file
View File

@ -0,0 +1,88 @@
#!/usr/bin/python3
# coding=utf8
from __future__ import print_function, division, absolute_import
import copy
import _thread
import time
import numpy as np
import rospy
import tf
import tf.transformations
from geometry_msgs.msg import Pose, Point, Quaternion
from nav_msgs.msg import Odometry
cur_odom_to_baselink = None
cur_map_to_odom = None
def pose_to_mat(pose_msg):
return np.matmul(
tf.listener.xyz_to_mat44(pose_msg.pose.pose.position),
tf.listener.xyzw_to_mat44(pose_msg.pose.pose.orientation),
)
def transform_fusion():
global cur_odom_to_baselink, cur_map_to_odom
br = tf.TransformBroadcaster()
while True:
time.sleep(1 / FREQ_PUB_LOCALIZATION)
# TODO 这里注意线程安全
cur_odom = copy.copy(cur_odom_to_baselink)
if cur_map_to_odom is not None:
T_map_to_odom = pose_to_mat(cur_map_to_odom)
else:
T_map_to_odom = np.eye(4)
br.sendTransform(tf.transformations.translation_from_matrix(T_map_to_odom),
tf.transformations.quaternion_from_matrix(T_map_to_odom),
rospy.Time.now(),
'camera_init', 'map')
if cur_odom is not None:
# 发布全局定位的odometry
localization = Odometry()
T_odom_to_base_link = pose_to_mat(cur_odom)
# 这里T_map_to_odom短时间内变化缓慢 暂时不考虑与T_odom_to_base_link时间同步
T_map_to_base_link = np.matmul(T_map_to_odom, T_odom_to_base_link)
xyz = tf.transformations.translation_from_matrix(T_map_to_base_link)
quat = tf.transformations.quaternion_from_matrix(T_map_to_base_link)
localization.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
localization.twist = cur_odom.twist
localization.header.stamp = cur_odom.header.stamp
localization.header.frame_id = 'map'
localization.child_frame_id = 'body'
# rospy.loginfo_throttle(1, '{}'.format(np.matmul(T_map_to_odom, T_odom_to_base_link)))
pub_localization.publish(localization)
def cb_save_cur_odom(odom_msg):
global cur_odom_to_baselink
cur_odom_to_baselink = odom_msg
def cb_save_map_to_odom(odom_msg):
global cur_map_to_odom
cur_map_to_odom = odom_msg
if __name__ == '__main__':
# tf and localization publishing frequency (HZ)
FREQ_PUB_LOCALIZATION = 50
rospy.init_node('transform_fusion')
rospy.loginfo('Transform Fusion Node Inited...')
rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1)
rospy.Subscriber('/map_to_odom', Odometry, cb_save_map_to_odom, queue_size=1)
pub_localization = rospy.Publisher('/localization', Odometry, queue_size=1)
# 发布定位消息
_thread.start_new_thread(transform_fusion, ())
rospy.spin()

376
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.hpp Normal file
View File

@ -0,0 +1,376 @@
#include <cmath>
#include <math.h>
#include <deque>
#include <mutex>
#include <thread>
#include <fstream>
#include <csignal>
#include <ros/ros.h>
#include <so3_math.h>
#include <Eigen/Eigen>
#include <common_lib.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <condition_variable>
#include <nav_msgs/Odometry.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <tf/transform_broadcaster.h>
#include <eigen_conversions/eigen_msg.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Vector3.h>
#include "use-ikfom.hpp"
/// *************Preconfiguration
#define MAX_INI_COUNT (20)
const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
/// *************IMU Process and undistortion
class ImuProcess
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ImuProcess();
~ImuProcess();
void Reset();
void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu);
void set_extrinsic(const V3D &transl, const M3D &rot);
void set_extrinsic(const V3D &transl);
void set_extrinsic(const MD(4,4) &T);
void set_gyr_cov(const V3D &scaler);
void set_acc_cov(const V3D &scaler);
void set_gyr_bias_cov(const V3D &b_g);
void set_acc_bias_cov(const V3D &b_a);
Eigen::Matrix<double, 12, 12> Q;
void Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);
ofstream fout_imu;
V3D cov_acc;
V3D cov_gyr;
V3D cov_acc_scale;
V3D cov_gyr_scale;
V3D cov_bias_gyr;
V3D cov_bias_acc;
double first_lidar_time;
private:
void IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N);
void UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_in_out);
PointCloudXYZI::Ptr cur_pcl_un_;
sensor_msgs::ImuConstPtr last_imu_;
deque<sensor_msgs::ImuConstPtr> v_imu_;
vector<Pose6D> IMUpose;
vector<M3D> v_rot_pcl_;
M3D Lidar_R_wrt_IMU;
V3D Lidar_T_wrt_IMU;
V3D mean_acc;
V3D mean_gyr;
V3D angvel_last;
V3D acc_s_last;
double start_timestamp_;
double last_lidar_end_time_;
int init_iter_num = 1;
bool b_first_frame_ = true;
bool imu_need_init_ = true;
};
ImuProcess::ImuProcess()
: b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1)
{
init_iter_num = 1;
Q = process_noise_cov();
cov_acc = V3D(0.1, 0.1, 0.1);
cov_gyr = V3D(0.1, 0.1, 0.1);
cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001);
cov_bias_acc = V3D(0.0001, 0.0001, 0.0001);
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
Lidar_T_wrt_IMU = Zero3d;
Lidar_R_wrt_IMU = Eye3d;
last_imu_.reset(new sensor_msgs::Imu());
}
ImuProcess::~ImuProcess() {}
void ImuProcess::Reset()
{
// ROS_WARN("Reset ImuProcess");
mean_acc = V3D(0, 0, -1.0);
mean_gyr = V3D(0, 0, 0);
angvel_last = Zero3d;
imu_need_init_ = true;
start_timestamp_ = -1;
init_iter_num = 1;
v_imu_.clear();
IMUpose.clear();
last_imu_.reset(new sensor_msgs::Imu());
cur_pcl_un_.reset(new PointCloudXYZI());
}
void ImuProcess::set_extrinsic(const MD(4,4) &T)
{
Lidar_T_wrt_IMU = T.block<3,1>(0,3);
Lidar_R_wrt_IMU = T.block<3,3>(0,0);
}
void ImuProcess::set_extrinsic(const V3D &transl)
{
Lidar_T_wrt_IMU = transl;
Lidar_R_wrt_IMU.setIdentity();
}
void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot)
{
Lidar_T_wrt_IMU = transl;
Lidar_R_wrt_IMU = rot;
}
void ImuProcess::set_gyr_cov(const V3D &scaler)
{
cov_gyr_scale = scaler;
}
void ImuProcess::set_acc_cov(const V3D &scaler)
{
cov_acc_scale = scaler;
}
void ImuProcess::set_gyr_bias_cov(const V3D &b_g)
{
cov_bias_gyr = b_g;
}
void ImuProcess::set_acc_bias_cov(const V3D &b_a)
{
cov_bias_acc = b_a;
}
void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, int &N)
{
/** 1. initializing the gravity, gyro bias, acc and gyro covariance
** 2. normalize the acceleration measurenments to unit gravity **/
V3D cur_acc, cur_gyr;
if (b_first_frame_)
{
Reset();
N = 1;
b_first_frame_ = false;
const auto &imu_acc = meas.imu.front()->linear_acceleration;
const auto &gyr_acc = meas.imu.front()->angular_velocity;
mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
first_lidar_time = meas.lidar_beg_time;
}
for (const auto &imu : meas.imu)
{
const auto &imu_acc = imu->linear_acceleration;
const auto &gyr_acc = imu->angular_velocity;
cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
mean_acc += (cur_acc - mean_acc) / N;
mean_gyr += (cur_gyr - mean_gyr) / N;
cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N);
cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N);
// cout<<"acc norm: "<<cur_acc.norm()<<" "<<mean_acc.norm()<<endl;
N ++;
}
state_ikfom init_state = kf_state.get_x();
init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);
//state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
init_state.bg = mean_gyr;
init_state.offset_T_L_I = Lidar_T_wrt_IMU;
init_state.offset_R_L_I = Lidar_R_wrt_IMU;
kf_state.change_x(init_state);
esekfom::esekf<state_ikfom, 12, input_ikfom>::cov init_P = kf_state.get_P();
init_P.setIdentity();
init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001;
init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;
init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;
init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;
init_P(21,21) = init_P(22,22) = 0.00001;
kf_state.change_P(init_P);
last_imu_ = meas.imu.back();
}
void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI &pcl_out)
{
/*** add the imu of the last frame-tail to the of current frame-head ***/
auto v_imu = meas.imu;
v_imu.push_front(last_imu_);
const double &imu_beg_time = v_imu.front()->header.stamp.toSec();
const double &imu_end_time = v_imu.back()->header.stamp.toSec();
const double &pcl_beg_time = meas.lidar_beg_time;
/*** sort point clouds by offset time ***/
pcl_out = *(meas.lidar);
sort(pcl_out.points.begin(), pcl_out.points.end(), time_list);
const double &pcl_end_time = pcl_beg_time + pcl_out.points.back().curvature / double(1000);
// cout<<"[ IMU Process ]: Process lidar from "<<pcl_beg_time<<" to "<<pcl_end_time<<", " \
// <<meas.imu.size()<<" imu msgs from "<<imu_beg_time<<" to "<<imu_end_time<<endl;
/*** Initialize IMU pose ***/
state_ikfom imu_state = kf_state.get_x();
IMUpose.clear();
IMUpose.push_back(set_pose6d(0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
/*** forward propagation at each imu point ***/
V3D angvel_avr, acc_avr, acc_imu, vel_imu, pos_imu;
M3D R_imu;
double dt = 0;
input_ikfom in;
for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++)
{
auto &&head = *(it_imu);
auto &&tail = *(it_imu + 1);
if (tail->header.stamp.toSec() < last_lidar_end_time_) continue;
angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x),
0.5 * (head->angular_velocity.y + tail->angular_velocity.y),
0.5 * (head->angular_velocity.z + tail->angular_velocity.z);
acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x),
0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y),
0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z);
// fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl;
acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba;
if(head->header.stamp.toSec() < last_lidar_end_time_)
{
dt = tail->header.stamp.toSec() - last_lidar_end_time_;
// dt = tail->header.stamp.toSec() - pcl_beg_time;
}
else
{
dt = tail->header.stamp.toSec() - head->header.stamp.toSec();
}
in.acc = acc_avr;
in.gyro = angvel_avr;
Q.block<3, 3>(0, 0).diagonal() = cov_gyr;
Q.block<3, 3>(3, 3).diagonal() = cov_acc;
Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr;
Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;
kf_state.predict(dt, Q, in);
/* save the poses at each IMU measurements */
imu_state = kf_state.get_x();
angvel_last = angvel_avr - imu_state.bg;
acc_s_last = imu_state.rot * (acc_avr - imu_state.ba);
for(int i=0; i<3; i++)
{
acc_s_last[i] += imu_state.grav[i];
}
double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;
IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix()));
}
/*** calculated the pos and attitude prediction at the frame-end ***/
double note = pcl_end_time > imu_end_time ? 1.0 : -1.0;
dt = note * (pcl_end_time - imu_end_time);
kf_state.predict(dt, Q, in);
imu_state = kf_state.get_x();
last_imu_ = meas.imu.back();
last_lidar_end_time_ = pcl_end_time;
/*** undistort each lidar point (backward propagation) ***/
auto it_pcl = pcl_out.points.end() - 1;
for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--)
{
auto head = it_kp - 1;
auto tail = it_kp;
R_imu<<MAT_FROM_ARRAY(head->rot);
// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;
vel_imu<<VEC_FROM_ARRAY(head->vel);
pos_imu<<VEC_FROM_ARRAY(head->pos);
acc_imu<<VEC_FROM_ARRAY(tail->acc);
angvel_avr<<VEC_FROM_ARRAY(tail->gyr);
for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --)
{
dt = it_pcl->curvature / double(1000) - head->offset_time;
/* Transform to the 'end' frame, using only the rotation
* Note: Compensation direction is INVERSE of Frame's moving direction
* So if we want to compensate a point at timestamp-i to the frame-e
* P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */
M3D R_i(R_imu * Exp(angvel_avr, dt));
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);
V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
// save Undistorted points and their rotation
it_pcl->x = P_compensate(0);
it_pcl->y = P_compensate(1);
it_pcl->z = P_compensate(2);
if (it_pcl == pcl_out.points.begin()) break;
}
}
}
void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr cur_pcl_un_)
{
double t1,t2,t3;
t1 = omp_get_wtime();
if(meas.imu.empty()) {return;};
ROS_ASSERT(meas.lidar != nullptr);
if (imu_need_init_)
{
/// The very first lidar frame
IMU_init(meas, kf_state, init_iter_num);
imu_need_init_ = true;
last_imu_ = meas.imu.back();
state_ikfom imu_state = kf_state.get_x();
if (init_iter_num > MAX_INI_COUNT)
{
cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2);
imu_need_init_ = false;
cov_acc = cov_acc_scale;
cov_gyr = cov_gyr_scale;
ROS_INFO("IMU Initial Done");
// ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\
// imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]);
fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out);
}
return;
}
UndistortPcl(meas, kf_state, *cur_pcl_un_);
t2 = omp_get_wtime();
t3 = omp_get_wtime();
// cout<<"[ IMU Process ]: Time: "<<t3 - t1<<endl;
}

55
src/FAST_LIO_LOCALIZATION/src/flag.cpp Normal file
View File

@ -0,0 +1,55 @@
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// tf chage https://zhuanlan.zhihu.com/p/340016739
int main(int argc, char** argv){
ros::init(argc, argv, "flag");
ros::NodeHandle nh;
ros::Publisher pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/pose_controller/filtered_pose", 10);
ros::NodeHandle node;
geometry_msgs::PoseStamped pose;
tf::TransformListener listener;
tf::TransformBroadcaster broadcaster;
tf::Transform transform_broadcaster;
ros::Duration(1.0).sleep();
ros::Rate rate(1000);
while (node.ok()){
tf::StampedTransform transform_listener;
try{
listener.lookupTransform("robot_foot_init", "body",
ros::Time(0), transform_listener);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
float robot_pose_x=transform_listener.getOrigin().x();
float robot_pose_y=transform_listener.getOrigin().y();
float robot_pose_z=transform_listener.getOrigin().z();
float robot_oriation_x=transform_listener.getRotation().getX();
float robot_oriation_y=transform_listener.getRotation().getY();
float robot_oriation_z=transform_listener.getRotation().getZ();
float robot_oriation_w=transform_listener.getRotation().getW();
pose.header.stamp = ros::Time::now();
pose.header.frame_id = "now";
pose.pose.position.x = robot_pose_x;
pose.pose.position.y = robot_pose_y;
pose.pose.position.z = robot_pose_z;
pose.pose.orientation.x = robot_oriation_x;
pose.pose.orientation.y = robot_oriation_y;
pose.pose.orientation.z = robot_oriation_z;
pose.pose.orientation.w = robot_oriation_w;
pose_publisher.publish(pose);
transform_broadcaster.setOrigin( tf::Vector3(2,2, 0.0) );
transform_broadcaster.setRotation( tf::Quaternion(0, 0, 0,1) );
//broadcaster.sendTransform(tf::StampedTransform(transform_broadcaster, ros::Time::now(), "map", "flag"));
rate.sleep();
}
return 0;
};

1000
src/FAST_LIO_LOCALIZATION/src/laserMapping.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

266
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp Normal file
View File

@ -0,0 +1,266 @@
#include "mbot_linux_serial.h"
#include <string>
#include <unistd.h>
/*===================================================================
程序功能：串口通信测试程序
程序编写：公众号：小白学移动机器人
其他 ：如果对代码有任何疑问，可以私信小编，一定会回复的。
=====================================================================
------------------关注公众号，获得更多有趣的分享---------------------
===================================================================*/
using namespace std;
using namespace boost::asio;
//串口相关对象
boost::system::error_code err;
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev);
uint8_t usart_start = 0xFF;
uint8_t usart_id = 0x06;
uint8_t usart_end = 0xFE;
/********************************************************
串口发送接收相关常量、变量、共用体对象
********************************************************/
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};
//发送左右轮速控制速度共用体
union sendData
{
short d;
unsigned char data[2];
}leftVelSet,rightVelSet;
union sendDatelf
{
float d;
unsigned char data[4];
}X,Y,YAW;
//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
union receiveData
{
short d;
unsigned char data[2];
}leftVelNow,rightVelNow,angleNow;
/********************************************************
函数功能：串口参数初始化
入口参数：无
出口参数：
********************************************************/
void serialInit()
{
sp.open("/dev/ttyUSB0", err);
if(err){
std::cout << "Error: " << err << std::endl;
std::cout << "请检查您的串口/dev/ttyUSB0 是否已经准备好：\n 1.读写权限是否打开（默认不打开) \n 2.串口名称是否正确" << std::endl;
return ;
}
sp.set_option(serial_port::baud_rate(115200));
sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
sp.set_option(serial_port::parity(serial_port::parity::none));
sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp.set_option(serial_port::character_size(8));
iosev.run();
}
/********************************************************
函数功能：将对机器人的左右轮子控制速度，打包发送给下位机
入口参数：机器人线速度、角速度
出口参数：
********************************************************/
void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag)
{
unsigned char buf[18] = {0};//
int i, length = 0;
leftVelSet.d = Left_v;//mm/s
rightVelSet.d = Right_v;
// 设置消息头
for(i = 0; i < 2; i++)
buf[i] = header[i]; //buf[0] buf[1]
// 设置机器人左右轮速度
length = 5;
buf[2] = length; //buf[2]
for(i = 0; i < 2; i++)
{
buf[i + 3] = leftVelSet.data[i]; //buf[3] buf[4]
buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
}
// 预留控制指令
buf[3 + length - 1] = ctrlFlag; //buf[7]
// 设置校验值、消息尾
buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
buf[3 + length + 1] = ender[0]; //buf[9]
buf[3 + length + 2] = ender[1]; //buf[10]
// 通过串口下发数据
boost::asio::write(sp, boost::asio::buffer(buf));
}
void write3float(float x,float y,float yaw)
{
unsigned char buf[18] = {0};//
int i, length = 0;
X.d = x;//mm/s
Y.d = y;
YAW.d = yaw;
// 设置消息头
for(i = 0; i < 2; i++)
buf[i] = header[i]; //buf[0] buf[1]
// 设置机器人左右轮速度
length = 12;
buf[2] = length; //buf[2]
for(i = 0; i < 4; i++)
{
buf[i + 3] = X.data[i]; //3456
buf[i + 7] = Y.data[i]; //78910
buf[i + 11] = YAW.data[i];//11121314
}
// 设置校验值、消息尾
buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
buf[3 + length + 1] = ender[0]; //buf[9]
buf[3 + length + 2] = ender[1]; //buf[10]
// 通过串口下发数据
boost::asio::write(sp, boost::asio::buffer(buf));
}
std::string string2hex(const std::string& input)
{
static const char* const lut = "0123456789ABCDEF";
size_t len = input.length();
std::string output;
output.reserve(2 * len);
for (size_t i = 0; i < len; ++i)
{
const unsigned char c = input[i];
output.push_back(lut[c >> 4]);
output.push_back(lut[c & 15]);
}
return output;
}
/********************************************************
函数功能：从下位机读取数据
入口参数：机器人左轮轮速、右轮轮速、角度，预留控制位
出口参数：bool
********************************************************/
bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag)
{
char length = 0;
unsigned char checkSum;
unsigned char buf[15]={0};
bool succeedReadFlag = false;
//=========================================================
// 此段代码可以读数据的结尾，进而来进行读取数据的头部
try
{
boost::asio::streambuf response;
boost::asio::read_until(sp, response, "\r\n", err); // 第一次分割数据 根据数据尾"\r\n"
std::string str;
std::istream is(&response);
while(response.size() != 0)
{
std::getline(is, str, (char)header[0]); // 第二次分割数据 根据数据头 这个会丢 0x55
// std::cout <<"筛选前："<<" {"<< string2hex(str) << "} " <<std::endl;
// std::cout << "str size = " << str.size() << std::endl;
if(str.size() == 12)
{
std::string finalStr(1, header[0]);
finalStr = finalStr + str;
// std::cout <<"筛选后："<<" {"<< string2hex(finalStr) << "} " <<std::endl;
for (size_t i = 0; i < finalStr.size(); i++)
{
buf[i] = finalStr[i];
}
succeedReadFlag = true;
break;
}
else
{
continue;
}
}
}
catch(boost::system::system_error &err)
{
ROS_ERROR("read_until error");
}
//=========================================================
// 检查信息头
if (buf[0]!= header[0] || buf[1] != header[1]) //buf[0] buf[1]
{
ROS_ERROR("Received message header error!");
return false;
}
// 数据长度
length = buf[2]; //buf[2]
// 检查信息校验值
checkSum = getCrc8(buf, 3 + length); //buf[10] 计算得出
if (checkSum != buf[3 + length]) //buf[10] 串口接收
{
ROS_ERROR("Received data check sum error!");
return false;
}
// 读取速度值
for(int i = 0; i < 2; i++)
{
leftVelNow.data[i] = buf[i + 3]; //buf[3] buf[4]
rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
angleNow.data[i] = buf[i + 7]; //buf[7] buf[8]
}
// 读取控制标志位
ctrlFlag = buf[9];
Left_v =leftVelNow.d;
Right_v =rightVelNow.d;
Angle =angleNow.d;
return true;
}
/********************************************************
函数功能：获得8位循环冗余校验值
入口参数：数组地址、长度
出口参数：校验值
********************************************************/
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
unsigned char crc;
unsigned char i;
crc = 0;
while(len--)
{
crc ^= *ptr++;
for(i = 0; i < 8; i++)
{
if(crc&0x01)
crc=(crc>>1)^0x8C;
else
crc >>= 1;
}
}
return crc;
}

18
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef MBOT_LINUX_SERIAL_H
#define MBOT_LINUX_SERIAL_H
#include <ros/ros.h>
#include <ros/time.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>
#include <geometry_msgs/Twist.h>
extern void serialInit();
extern void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag);
extern void write3float(float x, float y, float yaw);
extern bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag);
unsigned char getCrc8(unsigned char *ptr, unsigned short len);
#endif

916
src/FAST_LIO_LOCALIZATION/src/preprocess.cpp Normal file
View File

@ -0,0 +1,916 @@
#include "preprocess.h"
#define RETURN0 0x00
#define RETURN0AND1 0x10
Preprocess::Preprocess()
:feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
{
inf_bound = 10;
N_SCANS = 6;
group_size = 8;
disA = 0.01;
disA = 0.1; // B?
p2l_ratio = 225;
limit_maxmid =6.25;
limit_midmin =6.25;
limit_maxmin = 3.24;
jump_up_limit = 170.0;
jump_down_limit = 8.0;
cos160 = 160.0;
edgea = 2;
edgeb = 0.1;
smallp_intersect = 172.5;
smallp_ratio = 1.2;
given_offset_time = false;
jump_up_limit = cos(jump_up_limit/180*M_PI);
jump_down_limit = cos(jump_down_limit/180*M_PI);
cos160 = cos(cos160/180*M_PI);
smallp_intersect = cos(smallp_intersect/180*M_PI);
}
Preprocess::~Preprocess() {}
void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
{
feature_enabled = feat_en;
lidar_type = lid_type;
blind = bld;
point_filter_num = pfilt_num;
}
void Preprocess::process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{
avia_handler(msg);
*pcl_out = pl_surf;
}
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{
switch (lidar_type)
{
case OUST64:
oust64_handler(msg);
break;
case VELO16:
velodyne_handler(msg);
break;
default:
printf("Error LiDAR Type");
break;
}
*pcl_out = pl_surf;
}
void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
double t1 = omp_get_wtime();
int plsize = msg->point_num;
// cout<<"plsie: "<<plsize<<endl;
pl_corn.reserve(plsize);
pl_surf.reserve(plsize);
pl_full.resize(plsize);
for(int i=0; i<N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
uint valid_num = 0;
if (feature_enabled)
{
for(uint i=1; i<plsize; i++)
{
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
{
pl_full[i].x = msg->points[i].x;
pl_full[i].y = msg->points[i].y;
pl_full[i].z = msg->points[i].z;
pl_full[i].intensity = msg->points[i].reflectivity;
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
bool is_new = false;
if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
|| (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
|| (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
{
pl_buff[msg->points[i].line].push_back(pl_full[i]);
}
}
}
static int count = 0;
static double time = 0.0;
count ++;
double t0 = omp_get_wtime();
for(int j=0; j<N_SCANS; j++)
{
if(pl_buff[j].size() <= 5) continue;
pcl::PointCloud<PointType> &pl = pl_buff[j];
plsize = pl.size();
vector<orgtype> &types = typess[j];
types.clear();
types.resize(plsize);
plsize--;
for(uint i=0; i<plsize; i++)
{
types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
give_feature(pl, types);
// pl_surf += pl;
}
time += omp_get_wtime() - t0;
printf("Feature extraction time: %lf \n", time / count);
}
else
{
for(uint i=1; i<plsize; i++)
{
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
{
valid_num ++;
if (valid_num % point_filter_num == 0)
{
pl_full[i].x = msg->points[i].x;
pl_full[i].y = msg->points[i].y;
pl_full[i].z = msg->points[i].z;
pl_full[i].intensity = msg->points[i].reflectivity;
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7)
|| (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
|| (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)
&& (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind))
{
if(pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y < max_scan_range * max_scan_range){
pl_surf.push_back(pl_full[i]);
}
// pl_surf.push_back(pl_full[i]);
}
}
}
}
}
}
void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<ouster_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.size();
pl_corn.reserve(plsize);
pl_surf.reserve(plsize);
if (feature_enabled)
{
for (int i = 0; i < N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
for (uint i = 0; i < plsize; i++)
{
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
if (range < blind) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
if (yaw_angle <= -180.0)
yaw_angle += 360.0;
added_pt.curvature = pl_orig.points[i].t / 1e6;
if(pl_orig.points[i].ring < N_SCANS)
{
pl_buff[pl_orig.points[i].ring].push_back(added_pt);
}
}
for (int j = 0; j < N_SCANS; j++)
{
PointCloudXYZI &pl = pl_buff[j];
int linesize = pl.size();
vector<orgtype> &types = typess[j];
types.clear();
types.resize(linesize);
linesize--;
for (uint i = 0; i < linesize; i++)
{
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
give_feature(pl, types);
}
}
else
{
double time_stamp = msg->header.stamp.toSec();
// cout << "===================================" << endl;
// printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
for (int i = 0; i < pl_orig.points.size(); i++)
{
if (i % point_filter_num != 0) continue;
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
if (range < blind) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
if (yaw_angle <= -180.0)
yaw_angle += 360.0;
added_pt.curvature = pl_orig.points[i].t / 1e6;
pl_surf.points.push_back(added_pt);
}
}
// pub_func(pl_surf, pub_full, msg->header.stamp);
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}
void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<velodyne_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.points.size();
pl_surf.reserve(plsize);
/*** These variables only works when no point timestamps given ***/
double omega_l=3.61; // scan angular velocity
std::vector<bool> is_first(N_SCANS,true);
std::vector<double> yaw_fp(N_SCANS, 0.0); // yaw of first scan point
std::vector<float> yaw_last(N_SCANS, 0.0); // yaw of last scan point
std::vector<float> time_last(N_SCANS, 0.0); // last offset time
/*****************************************************************/
if (pl_orig.points[plsize - 1].time > 0)
{
given_offset_time = true;
}
else
{
given_offset_time = false;
double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
double yaw_end = yaw_first;
int layer_first = pl_orig.points[0].ring;
for (uint i = plsize - 1; i > 0; i--)
{
if (pl_orig.points[i].ring == layer_first)
{
yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
break;
}
}
}
if(feature_enabled)
{
for (int i = 0; i < N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
for (int i = 0; i < plsize; i++)
{
PointType added_pt;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
int layer = pl_orig.points[i].ring;
if (layer >= N_SCANS) continue;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms
if (!given_offset_time)
{
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
if (is_first[layer])
{
// printf("layer: %d; is first: %d", layer, is_first[layer]);
yaw_fp[layer]=yaw_angle;
is_first[layer]=false;
added_pt.curvature = 0.0;
yaw_last[layer]=yaw_angle;
time_last[layer]=added_pt.curvature;
continue;
}
if (yaw_angle <= yaw_fp[layer])
{
added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
}
else
{
added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
}
if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l;
yaw_last[layer] = yaw_angle;
time_last[layer]=added_pt.curvature;
}
pl_buff[layer].points.push_back(added_pt);
}
for (int j = 0; j < N_SCANS; j++)
{
PointCloudXYZI &pl = pl_buff[j];
int linesize = pl.size();
if (linesize < 2) continue;
vector<orgtype> &types = typess[j];
types.clear();
types.resize(linesize);
linesize--;
for (uint i = 0; i < linesize; i++)
{
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
give_feature(pl, types);
}
}
else
{
for (int i = 0; i < plsize; i++)
{
PointType added_pt;
// cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.curvature = pl_orig.points[i].time / 1000.0;
if (!given_offset_time)
{
int layer = pl_orig.points[i].ring;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
if (is_first[layer])
{
// printf("layer: %d; is first: %d", layer, is_first[layer]);
yaw_fp[layer]=yaw_angle;
is_first[layer]=false;
added_pt.curvature = 0.0;
yaw_last[layer]=yaw_angle;
time_last[layer]=added_pt.curvature;
continue;
}
// compute offset time
if (yaw_angle <= yaw_fp[layer])
{
added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
}
else
{
added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
}
if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l;
yaw_last[layer] = yaw_angle;
time_last[layer]=added_pt.curvature;
}
if (i % point_filter_num == 0)
{
if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind)
{
pl_surf.points.push_back(added_pt);
}
}
}
}
}
void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
{
int plsize = pl.size();
int plsize2;
if(plsize == 0)
{
printf("something wrong\n");
return;
}
uint head = 0;
while(types[head].range < blind)
{
head++;
}
// Surf
plsize2 = (plsize > group_size) ? (plsize - group_size) : 0;
Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero());
Eigen::Vector3d last_direct(Eigen::Vector3d::Zero());
uint i_nex = 0, i2;
uint last_i = 0; uint last_i_nex = 0;
int last_state = 0;
int plane_type;
for(uint i=head; i<plsize2; i++)
{
if(types[i].range < blind)
{
continue;
}
i2 = i;
plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
if(plane_type == 1)
{
for(uint j=i; j<=i_nex; j++)
{
if(j!=i && j!=i_nex)
{
types[j].ftype = Real_Plane;
}
else
{
types[j].ftype = Poss_Plane;
}
}
// if(last_state==1 && fabs(last_direct.sum())>0.5)
if(last_state==1 && last_direct.norm()>0.1)
{
double mod = last_direct.transpose() * curr_direct;
if(mod>-0.707 && mod<0.707)
{
types[i].ftype = Edge_Plane;
}
else
{
types[i].ftype = Real_Plane;
}
}
i = i_nex - 1;
last_state = 1;
}
else // if(plane_type == 2)
{
i = i_nex;
last_state = 0;
}
// else if(plane_type == 0)
// {
// if(last_state == 1)
// {
// uint i_nex_tem;
// uint j;
// for(j=last_i+1; j<=last_i_nex; j++)
// {
// uint i_nex_tem2 = i_nex_tem;
// Eigen::Vector3d curr_direct2;
// uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
// if(ttem != 1)
// {
// i_nex_tem = i_nex_tem2;
// break;
// }
// curr_direct = curr_direct2;
// }
// if(j == last_i+1)
// {
// last_state = 0;
// }
// else
// {
// for(uint k=last_i_nex; k<=i_nex_tem; k++)
// {
// if(k != i_nex_tem)
// {
// types[k].ftype = Real_Plane;
// }
// else
// {
// types[k].ftype = Poss_Plane;
// }
// }
// i = i_nex_tem-1;
// i_nex = i_nex_tem;
// i2 = j-1;
// last_state = 1;
// }
// }
// }
last_i = i2;
last_i_nex = i_nex;
last_direct = curr_direct;
}
plsize2 = plsize > 3 ? plsize - 3 : 0;
for(uint i=head+3; i<plsize2; i++)
{
if(types[i].range<blind || types[i].ftype>=Real_Plane)
{
continue;
}
if(types[i-1].dista<1e-16 || types[i].dista<1e-16)
{
continue;
}
Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z);
Eigen::Vector3d vecs[2];
for(int j=0; j<2; j++)
{
int m = -1;
if(j == 1)
{
m = 1;
}
if(types[i+m].range < blind)
{
if(types[i].range > inf_bound)
{
types[i].edj[j] = Nr_inf;
}
else
{
types[i].edj[j] = Nr_blind;
}
continue;
}
vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z);
vecs[j] = vecs[j] - vec_a;
types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm();
if(types[i].angle[j] < jump_up_limit)
{
types[i].edj[j] = Nr_180;
}
else if(types[i].angle[j] > jump_down_limit)
{
types[i].edj[j] = Nr_zero;
}
}
types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista)
{
if(types[i].intersect > cos160)
{
if(edge_jump_judge(pl, types, i, Prev))
{
types[i].ftype = Edge_Jump;
}
}
}
else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista)
{
if(types[i].intersect > cos160)
{
if(edge_jump_judge(pl, types, i, Next))
{
types[i].ftype = Edge_Jump;
}
}
}
else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf)
{
if(edge_jump_judge(pl, types, i, Prev))
{
types[i].ftype = Edge_Jump;
}
}
else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor)
{
if(edge_jump_judge(pl, types, i, Next))
{
types[i].ftype = Edge_Jump;
}
}
else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor)
{
if(types[i].ftype == Nor)
{
types[i].ftype = Wire;
}
}
}
plsize2 = plsize-1;
double ratio;
for(uint i=head+1; i<plsize2; i++)
{
if(types[i].range<blind || types[i-1].range<blind || types[i+1].range<blind)
{
continue;
}
if(types[i-1].dista<1e-8 || types[i].dista<1e-8)
{
continue;
}
if(types[i].ftype == Nor)
{
if(types[i-1].dista > types[i].dista)
{
ratio = types[i-1].dista / types[i].dista;
}
else
{
ratio = types[i].dista / types[i-1].dista;
}
if(types[i].intersect<smallp_intersect && ratio < smallp_ratio)
{
if(types[i-1].ftype == Nor)
{
types[i-1].ftype = Real_Plane;
}
if(types[i+1].ftype == Nor)
{
types[i+1].ftype = Real_Plane;
}
types[i].ftype = Real_Plane;
}
}
}
int last_surface = -1;
for(uint j=head; j<plsize; j++)
{
if(types[j].ftype==Poss_Plane || types[j].ftype==Real_Plane)
{
if(last_surface == -1)
{
last_surface = j;
}
if(j == uint(last_surface+point_filter_num-1))
{
PointType ap;
ap.x = pl[j].x;
ap.y = pl[j].y;
ap.z = pl[j].z;
ap.intensity = pl[j].intensity;
ap.curvature = pl[j].curvature;
pl_surf.push_back(ap);
last_surface = -1;
}
}
else
{
if(types[j].ftype==Edge_Jump || types[j].ftype==Edge_Plane)
{
pl_corn.push_back(pl[j]);
}
if(last_surface != -1)
{
PointType ap;
for(uint k=last_surface; k<j; k++)
{
ap.x += pl[k].x;
ap.y += pl[k].y;
ap.z += pl[k].z;
ap.intensity += pl[k].intensity;
ap.curvature += pl[k].curvature;
}
ap.x /= (j-last_surface);
ap.y /= (j-last_surface);
ap.z /= (j-last_surface);
ap.intensity /= (j-last_surface);
ap.curvature /= (j-last_surface);
pl_surf.push_back(ap);
}
last_surface = -1;
}
}
}
void Preprocess::pub_func(PointCloudXYZI &pl, const ros::Time &ct)
{
pl.height = 1; pl.width = pl.size();
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(pl, output);
output.header.frame_id = "livox";
output.header.stamp = ct;
}
int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct)
{
double group_dis = disA*types[i_cur].range + disB;
group_dis = group_dis * group_dis;
// i_nex = i_cur;
double two_dis;
vector<double> disarr;
disarr.reserve(20);
for(i_nex=i_cur; i_nex<i_cur+group_size; i_nex++)
{
if(types[i_nex].range < blind)
{
curr_direct.setZero();
return 2;
}
disarr.push_back(types[i_nex].dista);
}
for(;;)
{
if((i_cur >= pl.size()) || (i_nex >= pl.size())) break;
if(types[i_nex].range < blind)
{
curr_direct.setZero();
return 2;
}
vx = pl[i_nex].x - pl[i_cur].x;
vy = pl[i_nex].y - pl[i_cur].y;
vz = pl[i_nex].z - pl[i_cur].z;
two_dis = vx*vx + vy*vy + vz*vz;
if(two_dis >= group_dis)
{
break;
}
disarr.push_back(types[i_nex].dista);
i_nex++;
}
double leng_wid = 0;
double v1[3], v2[3];
for(uint j=i_cur+1; j<i_nex; j++)
{
if((j >= pl.size()) || (i_cur >= pl.size())) break;
v1[0] = pl[j].x - pl[i_cur].x;
v1[1] = pl[j].y - pl[i_cur].y;
v1[2] = pl[j].z - pl[i_cur].z;
v2[0] = v1[1]*vz - vy*v1[2];
v2[1] = v1[2]*vx - v1[0]*vz;
v2[2] = v1[0]*vy - vx*v1[1];
double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2];
if(lw > leng_wid)
{
leng_wid = lw;
}
}
if((two_dis*two_dis/leng_wid) < p2l_ratio)
{
curr_direct.setZero();
return 0;
}
uint disarrsize = disarr.size();
for(uint j=0; j<disarrsize-1; j++)
{
for(uint k=j+1; k<disarrsize; k++)
{
if(disarr[j] < disarr[k])
{
leng_wid = disarr[j];
disarr[j] = disarr[k];
disarr[k] = leng_wid;
}
}
}
if(disarr[disarr.size()-2] < 1e-16)
{
curr_direct.setZero();
return 0;
}
if(lidar_type==AVIA)
{
double dismax_mid = disarr[0]/disarr[disarrsize/2];
double dismid_min = disarr[disarrsize/2]/disarr[disarrsize-2];
if(dismax_mid>=limit_maxmid || dismid_min>=limit_midmin)
{
curr_direct.setZero();
return 0;
}
}
else
{
double dismax_min = disarr[0] / disarr[disarrsize-2];
if(dismax_min >= limit_maxmin)
{
curr_direct.setZero();
return 0;
}
}
curr_direct << vx, vy, vz;
curr_direct.normalize();
return 1;
}
bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir)
{
if(nor_dir == 0)
{
if(types[i-1].range<blind || types[i-2].range<blind)
{
return false;
}
}
else if(nor_dir == 1)
{
if(types[i+1].range<blind || types[i+2].range<blind)
{
return false;
}
}
double d1 = types[i+nor_dir-1].dista;
double d2 = types[i+3*nor_dir-2].dista;
double d;
if(d1<d2)
{
d = d1;
d1 = d2;
d2 = d;
}
d1 = sqrt(d1);
d2 = sqrt(d2);
if(d1>edgea*d2 || (d1-d2)>edgeb)
{
return false;
}
return true;
}

123
src/FAST_LIO_LOCALIZATION/src/preprocess.h Normal file
View File

@ -0,0 +1,123 @@
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <livox_ros_driver2/CustomMsg.h>
using namespace std;
#define IS_VALID(a) ((abs(a)>1e8) ? true : false)
typedef pcl::PointXYZINormal PointType;
typedef pcl::PointCloud<PointType> PointCloudXYZI;
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
enum Surround{Prev, Next};
enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
struct orgtype
{
double range;
double dista;
double angle[2];
double intersect;
E_jump edj[2];
Feature ftype;
orgtype()
{
range = 0;
edj[Prev] = Nr_nor;
edj[Next] = Nr_nor;
ftype = Nor;
intersect = 2;
}
};
namespace velodyne_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
float time;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace velodyne_ros
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(float, time, time)
(uint16_t, ring, ring)
)
namespace ouster_ros {
struct EIGEN_ALIGN16 Point {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t ambient;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace ouster_ros
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint8_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)
class Preprocess
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preprocess();
~Preprocess();
void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::PointCloud2::ConstPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf;
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
vector<orgtype> typess[128]; //maximum 128 line lidar
int lidar_type, point_filter_num, N_SCANS;;
double blind;
double max_scan_range;
bool feature_enabled, given_offset_time;
ros::Publisher pub_full, pub_surf, pub_corn;
private:
void avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg);
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct);
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
int group_size;
double disA, disB, inf_bound;
double limit_maxmid, limit_midmin, limit_maxmin;
double p2l_ratio;
double jump_up_limit, jump_down_limit;
double cos160;
double edgea, edgeb;
double smallp_intersect, smallp_ratio;
double vx, vy, vz;
};

160
src/FAST_LIO_LOCALIZATION/src/send.cpp Normal file
View File

@ -0,0 +1,160 @@
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Int64.h>
#include <std_msgs/String.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <vector>
#include <sensor_msgs/LaserScan.h>
#include "mbot_linux_serial.h"
class PIDController {
public:
PIDController(double kp, double ki, double kd)
: kp_(kp), ki_(ki), kd_(kd), integral_(0.0), prev_error_(0.0) {}
double control(double setpoint, double pv) {
double error = setpoint - pv;
integral_ += error;
double derivative = error - prev_error_;
prev_error_ = error;
return kp_ * error + ki_ * integral_ + kd_ * derivative;
}//pid
private:
double kp_, ki_, kd_, integral_, prev_error_;
};
struct TargetPose {
double x, y, yaw;
};
class PoseController {
public:
PoseController()
: nh_("~"),
x_controller_(0.12, 0.0, 0.0),
y_controller_(0.15, 0.0, 0.0),
yaw_controller_(0.12, 0.0, 0.0),
robot_mode_(0) {
target_poses_ = {
{1.582, 0.00, 0.0},
{0.000, 1.60, -M_PI/2},
{0.705, 1.60, -M_PI/2},
{1.482, 1.60, -M_PI/2},
{2.234, 1.60, -M_PI/2},
{2.981, 1.60, -M_PI/2},
};
target_ = target_poses_[0];
pose_subscriber_ = nh_.subscribe("filtered_pose", 10, &PoseController::pose_callback, this);
cmd_pid_publisher_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_pid", 10);
//position_id_subscriber_ = nh_.subscribe("/position_id", 10, &PoseController::position_id_callback, this);
pid_status_publisher_ = nh_.advertise<std_msgs::String>("/pid_status", 10);
//robot_mode_subscriber_ = nh_.subscribe("robot_mode", 10, &PoseController::robot_mode_callback, this);
goal_subscriber_ = nh_.subscribe("/move_base_simple/goal", 10, &PoseController::goal_callback, this);
serial_write3float_=nh_.subscribe("cmd_vel_pid", 10, &PoseController::serial_write3double_callback, this);
//scan_sub_=nh_.subscribe("/scan",10,&PoseController::distant,this);
}
private:
// void robot_mode_callback(const std_msgs::Int64::ConstPtr& msg) {
// robot_mode_ = msg->data;
// if (msg->data == 1) {
// target_ = target_poses_[0];
// }
// }
// void position_id_callback(const std_msgs::Int64::ConstPtr& msg) {
// size_t id = static_cast<size_t>(msg->data);
// if (id < target_poses_.size()) {
// target_ = target_poses_[id];
// } else {
// ROS_WARN("Invalid position id: %ld", id);
// }
// }
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
double x = msg->pose.position.x;
double y = msg->pose.position.y;
tf2::Quaternion quat;
tf2::fromMsg(msg->pose.orientation, quat);
tf2::Matrix3x3 m(quat);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
TargetPose target_local = transformTargetPose(x, y, yaw);//算位姿
calculateControl(target_local);//检验
}
void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
double x = msg->pose.position.x;
double y = msg->pose.position.y;
tf2::Quaternion quat;
tf2::fromMsg(msg->pose.orientation, quat);
tf2::Matrix3x3 m(quat);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
target_={x,y,yaw};
}//目标位置位姿
TargetPose transformTargetPose(double x, double y, double yaw) {
TargetPose target_local;
target_local.x = cos(yaw) * (target_.x - x) + sin(yaw) * (target_.y - y);
target_local.y = -sin(yaw) * (target_.x - x) + cos(yaw) * (target_.y - y);
target_local.yaw = target_.yaw - yaw;
if (target_local.yaw > M_PI)
target_local.yaw -= 2 * M_PI;
else if (target_local.yaw < -M_PI)
target_local.yaw += 2 * M_PI;
return target_local;
}//算位姿
void calculateControl(const TargetPose &target_local) {
double error_threshold = 0.05;
double control_x = 0.0, control_y = 0.0, control_yaw = 0.0;
if (std::abs(target_local.x) > error_threshold || std::abs(target_local.y) > error_threshold || std::abs(target_local.yaw) > error_threshold) {
//校零
control_x = x_controller_.control(0.0, target_local.x);
control_y = y_controller_.control(0.0, target_local.y);
control_yaw = yaw_controller_.control(0.0, target_local.yaw);
control_x = std::min(control_x, 0.2);
control_y = std::min(control_y, 0.2);//限速
} else {
std_msgs::String msg;
msg.data = (robot_mode_ == 6) ? "OKK" : "OK";
pid_status_publisher_.publish(msg);
}
geometry_msgs::Twist cmd;
cmd.linear.x = control_x;
cmd.linear.y = -control_y;
cmd.angular.z = control_yaw;
cmd_pid_publisher_.publish(cmd);//发送pid
}
void serial_write3double_callback(const geometry_msgs::Twist::ConstPtr& msg) {
write3float(msg->linear.x, msg->linear.y, msg->angular.z);
}
ros::NodeHandle nh_;
ros::Subscriber pose_subscriber_;
ros::Subscriber goal_subscriber_;
ros::Publisher cmd_pid_publisher_;
ros::Publisher pid_status_publisher_;
ros::Subscriber robot_mode_subscriber_;
ros::Subscriber position_id_subscriber_;
ros::Subscriber serial_write3float_;
ros::Subscriber scan_sub_;
std::vector<TargetPose> target_poses_;
PIDController x_controller_, y_controller_, yaw_controller_;
TargetPose target_;
int robot_mode_;
};
int main(int argc, char **argv) {
ros::init(argc, argv, "pose_controller");
serialInit();
PoseController node;
ros::spin();
return 0;
}

1
src/livox_ros_driver2 Submodule

@ -0,0 +1 @@
Subproject commit 6b9356cadf77084619ba406e6a0eb41163b08039

39
src/pcd2pgm/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.0.2)
project(pcd2pgm)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
nav_msgs
pcl_ros
roscpp
rospy
std_msgs
geometry_msgs
sensor_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES pcd2pgm
# CATKIN_DEPENDS nav_msgs pcl roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
find_package(PCL REQUIRED)
include_directories(
include
${PCL_INCLUDE_DIRS})
add_executable(pcd2pgm src/pcd2pgm.cpp)
target_link_libraries(pcd2pgm ${catkin_LIBRARIES} ${PCL_LIBRARIES} )

24
src/pcd2pgm/launch/run.launch Executable file
View File

@ -0,0 +1,24 @@
<!-- -->
<launch>
<node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
<!-- 存放pcd文件的路径-->
<param name="file_directory" value= "/home/cmrt/mid360/ws_livox/src/FAST_LIO/PCD/" />
<!-- pcd文件名称-->
<param name="file_name" value= "scans" />
<!-- 选取的范围　最小的高度-->
<param name="thre_z_min" value= "0.1" />
<!-- 选取的范围　最大的高度-->
<param name="thre_z_max" value= "1.5" />
<!--0 选取高度范围内的，１选取高度范围外的-->
<param name="flag_pass_through" value= "0" />
<!-- 半径滤波的半径-->
<param name="thre_radius" value= "0.5" />
<!-- 半径滤波的要求点数个数-->
<param name="thres_point_count" value= "10" />
<!-- 存储的栅格map的分辨率-->
<param name="map_resolution" value= "0.05" />
<!-- 转换后发布的二维地图的topic，默认使用map即可，可使用map_server保存-->
<param name="map_topic_name" value= "map" />
</node>
</launch>

78
src/pcd2pgm/package.xml Normal file
View File

@ -0,0 +1,78 @@
<?xml version="1.0"?>
<package format="2">
<name>pcd2pgm</name>
<version>0.0.0</version>
<description>The pcd2pgm package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="MotherRussia@facker.com">facker</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/pcd_to_topic</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>pcl_ros</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

219
src/pcd2pgm/src/pcd2pgm.cpp Normal file
View File

@ -0,0 +1,219 @@
#include <ros/ros.h>
#include <nav_msgs/GetMap.h>
#include <nav_msgs/OccupancyGrid.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/conditional_removal.h> //条件滤波器头文件
#include <pcl/filters/passthrough.h> //直通滤波器头文件
#include <pcl/filters/radius_outlier_removal.h> //半径滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/voxel_grid.h> //体素滤波器头文件
#include <pcl/point_types.h>
std::string file_directory;
std::string file_name;
std::string pcd_file;
std::string map_topic_name;
const std::string pcd_format = ".pcd";
nav_msgs::OccupancyGrid map_topic_msg;
//最小和最大高度
double thre_z_min = 0.3;
double thre_z_max = 2.0;
int flag_pass_through = 0;
double map_resolution = 0.05;
double thre_radius = 0.1;
//半径滤波的点数阈值
int thres_point_count = 10;
//直通滤波后数据指针
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
//半径滤波后数据指针
pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud_after_Radius(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr
pcd_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//直通滤波
void PassThroughFilter(const double &thre_low, const double &thre_high,
const bool &flag_in);
//半径滤波
void RadiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcd_cloud,
const double &radius, const int &thre_count);
//转换为栅格地图数据并发布
void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
nav_msgs::OccupancyGrid &msg);
int main(int argc, char **argv) {
ros::init(argc, argv, "pcl_filters");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
ros::Rate loop_rate(1.0);
private_nh.param("file_directory", file_directory, std::string("/home/"));
private_nh.param("file_name", file_name, std::string("map"));
pcd_file = file_directory + file_name + pcd_format;
private_nh.param("thre_z_min", thre_z_min, 0.2);
private_nh.param("thre_z_max", thre_z_max, 2.0);
private_nh.param("flag_pass_through", flag_pass_through, 0);
private_nh.param("thre_radius", thre_radius, 0.5);
private_nh.param("map_resolution", map_resolution, 0.05);
private_nh.param("thres_point_count", thres_point_count, 10);
private_nh.param("map_topic_name", map_topic_name, std::string("map"));
ros::Publisher map_topic_pub =
nh.advertise<nav_msgs::OccupancyGrid>(map_topic_name, 1);
// 下载pcd文件
if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_file, *pcd_cloud) == -1) {
PCL_ERROR("Couldn't read file: %s \n", pcd_file.c_str());
return (-1);
}
std::cout << "初始点云数据点数：" << pcd_cloud->points.size() << std::endl;
//对数据进行直通滤波
PassThroughFilter(thre_z_min, thre_z_max, bool(flag_pass_through));
//对数据进行半径滤波
RadiusOutlierFilter(cloud_after_PassThrough, thre_radius, thres_point_count);
//转换为栅格地图数据并发布
SetMapTopicMsg(cloud_after_Radius, map_topic_msg);
// SetMapTopicMsg(cloud_after_PassThrough, map_topic_msg);
while (ros::ok()) {
map_topic_pub.publish(map_topic_msg);
loop_rate.sleep();
ros::spinOnce();
}
return 0;
}
//直通滤波器对点云进行过滤，获取设定高度范围内的数据
void PassThroughFilter(const double &thre_low, const double &thre_high,
const bool &flag_in) {
// 创建滤波器对象
pcl::PassThrough<pcl::PointXYZ> passthrough;
//输入点云
passthrough.setInputCloud(pcd_cloud);
//设置对z轴进行操作
passthrough.setFilterFieldName("z");
//设置滤波范围
passthrough.setFilterLimits(thre_low, thre_high);
// true表示保留滤波范围外，false表示保留范围内
passthrough.setFilterLimitsNegative(flag_in);
//执行滤波并存储
passthrough.filter(*cloud_after_PassThrough);
// test 保存滤波后的点云到文件
pcl::io::savePCDFile<pcl::PointXYZ>(file_directory + "map_filter.pcd",
*cloud_after_PassThrough);
std::cout << "直通滤波后点云数据点数："
<< cloud_after_PassThrough->points.size() << std::endl;
}
//半径滤波
void RadiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcd_cloud0,
const double &radius, const int &thre_count) {
//创建滤波器
pcl::RadiusOutlierRemoval<pcl::PointXYZ> radiusoutlier;
//设置输入点云
radiusoutlier.setInputCloud(pcd_cloud0);
//设置半径,在该范围内找临近点
radiusoutlier.setRadiusSearch(radius);
//设置查询点的邻域点集数，小于该阈值的删除
radiusoutlier.setMinNeighborsInRadius(thre_count);
radiusoutlier.filter(*cloud_after_Radius);
// test 保存滤波后的点云到文件
pcl::io::savePCDFile<pcl::PointXYZ>(file_directory + "map_radius_filter.pcd",
*cloud_after_Radius);
std::cout << "半径滤波后点云数据点数：" << cloud_after_Radius->points.size()
<< std::endl;
}
//转换为栅格地图数据并发布
void SetMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
nav_msgs::OccupancyGrid &msg) {
msg.header.seq = 0;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.info.map_load_time = ros::Time::now();
msg.info.resolution = map_resolution;
double x_min, x_max, y_min, y_max;
double z_max_grey_rate = 0.05;
double z_min_grey_rate = 0.95;
//? ? ??
double k_line =
(z_max_grey_rate - z_min_grey_rate) / (thre_z_max - thre_z_min);
double b_line =
(thre_z_max * z_min_grey_rate - thre_z_min * z_max_grey_rate) /
(thre_z_max - thre_z_min);
if (cloud->points.empty()) {
ROS_WARN("pcd is empty!\n");
return;
}
for (int i = 0; i < cloud->points.size() - 1; i++) {
if (i == 0) {
x_min = x_max = cloud->points[i].x;
y_min = y_max = cloud->points[i].y;
}
double x = cloud->points[i].x;
double y = cloud->points[i].y;
if (x < x_min)
x_min = x;
if (x > x_max)
x_max = x;
if (y < y_min)
y_min = y;
if (y > y_max)
y_max = y;
}
// origin的确定
msg.info.origin.position.x = x_min;
msg.info.origin.position.y = y_min;
msg.info.origin.position.z = 0.0;
msg.info.origin.orientation.x = 0.0;
msg.info.origin.orientation.y = 0.0;
msg.info.origin.orientation.z = 0.0;
msg.info.origin.orientation.w = 1.0;
//设置栅格地图大小
msg.info.width = int((x_max - x_min) / map_resolution);
msg.info.height = int((y_max - y_min) / map_resolution);
//实际地图中某点坐标为(x,y)，对应栅格地图中坐标为[x*map.info.width+y]
msg.data.resize(msg.info.width * msg.info.height);
msg.data.assign(msg.info.width * msg.info.height, 0);
ROS_INFO("data size = %d\n", msg.data.size());
for (int iter = 0; iter < cloud->points.size(); iter++) {
int i = int((cloud->points[iter].x - x_min) / map_resolution);
if (i < 0 || i >= msg.info.width)
continue;
int j = int((cloud->points[iter].y - y_min) / map_resolution);
if (j < 0 || j >= msg.info.height - 1)
continue;
// 栅格地图的占有概率[0,100]，这里设置为占据
msg.data[i + j * msg.info.width] = 100;
// msg.data[i + j * msg.info.width] = int(255 * (cloud->points[iter].z *
// k_line + b_line)) % 255;
}
}