first commit repaire loop bug

CMakeLists.txt

@@ -0,0 +1,114 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(fast_lio_sam)
+
+SET(CMAKE_BUILD_TYPE "Release")
+
+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(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
+find_package(PythonLibs REQUIRED)
+find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  roscpp
+  rospy
+  std_msgs
+  pcl_ros
+  tf
+  livox_ros_driver
+  message_generation
+  eigen_conversions
+)
+
+find_package(Eigen3 REQUIRED)
+find_package(PCL REQUIRED)
+#find_package (GeographicLib REQUIRED)
+
+find_package( GTSAMCMakeTools )
+find_package(GTSAM REQUIRED QUIET)
+
+message(Eigen: ${EIGEN3_INCLUDE_DIR})
+
+include_directories(
+	${catkin_INCLUDE_DIRS} 
+  ${EIGEN3_INCLUDE_DIR}
+  ${PCL_INCLUDE_DIRS}
+  ${PYTHON_INCLUDE_DIRS}
+  ${GTSAM_INCLUDE_DIR}
+  #${GeographicLib_INCLUDE_DIRS}
+  include)
+
+  # link directories
+# link_directories(
+#   ${GTSAM_LIBRARY_DIRS}
+# )
+
+add_message_files(
+  FILES
+  Pose6D.msg
+)
+
+add_service_files(
+  DIRECTORY srv
+  FILES
+  save_map.srv
+  save_pose.srv
+)
+
+generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
+  DEPENDS EIGEN3 PCL GTSAM
+  INCLUDE_DIRS
+)
+
+# add_executable(fastlio_sam_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
+# target_link_libraries(fastlio_sam_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+# target_include_directories(fastlio_sam_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
+
+add_library(ikd_Tree include/ikd-Tree/ikd_Tree.cpp)
+add_library(preprocess  src/preprocess.cpp)
+add_executable(fastlio_sam_mapping src/laserMapping.cpp src/scancontext/Scancontext.cpp)
+target_link_libraries(fastlio_sam_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}  ikd_Tree preprocess gtsam)  #${GeographicLib_LIBRARIES}
+target_include_directories(fastlio_sam_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})

LICENSE

@@ -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.

Log/fast_lio_time_log_analysis.m

@@ -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')
+

Log/guide.md

@@ -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.

Log/plot.py

@@ -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()

README.md

@@ -0,0 +1,194 @@
+## Related Works
+
+1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search.
+2. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter.
+3. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning.
+4. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end.
+5. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds.
+6. [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.
+7. [FAST-LIO-LOCALIZATION](https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION): The integration of FAST-LIO with **Re-localization** function module.
+
+## FAST-LIO
+**FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues:
+1. Fast iterated Kalman filter for odometry optimization;
+2. Automaticaly initialized at most steady environments;
+3. Parallel KD-Tree Search to decrease the computation;
+
+## FAST-LIO 2.0 (2021-07-05 Update)
+<!-- ![image](doc/real_experiment2.gif) -->
+<!-- [![Watch the video](doc/real_exp_2.png)](https://youtu.be/2OvjGnxszf8) -->
+
+<div align="left">
+<img src="doc/real_experiment2.gif" width=49.6% />
+<img src="doc/ulhkwh_fastlio.gif" width = 49.6% >
+</div>
+
+**Related video:**  [FAST-LIO2](https://youtu.be/2OvjGnxszf8),  [FAST-LIO1](https://youtu.be/iYCY6T79oNU),  [FAST-LIO2 + Scan-context Loop Closure](https://www.youtube.com/watch?v=nu8j4yaBMnw)
+
+**Pipeline:**
+<div align="center">
+<img src="doc/overview_fastlio2.svg" width=99% />
+</div>
+
+**New Features:**
+1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate.
+2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy.
+3. Since no requirements for feature extraction, FAST-LIO2 can support many types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs.
+4. Support external IMU.
+5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM).
+
+**Related papers**: 
+
+[FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf)
+
+[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196)
+
+**Contributors**
+
+[Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan)
+
+<!-- <div align="center">
+    <img src="doc/results/HKU_HW.png" width = 49% >
+    <img src="doc/results/HKU_MB_001.png" width = 49% >
+</div> -->
+
+## 1. Prerequisites
+### 1.1 **Ubuntu** and **ROS**
+**Ubuntu >= 16.04**
+
+For **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally.
+
+ROS    >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
+
+### 1.2. **PCL && Eigen**
+PCL    >= 1.8,   Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).
+
+Eigen  >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page).
+
+### 1.3. **livox_ros_driver**
+Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver).
+
+*Remarks:*
+- Since the FAST-LIO must support Livox serials LiDAR firstly, so the **livox_ros_driver** must be installed and **sourced** before run any FAST-LIO luanch file.
+- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document).
+
+
+## 2. Build
+Clone the repository and catkin_make:
+
+```
+    cd ~/$A_ROS_DIR$/src
+    git clone https://github.com/hku-mars/FAST_LIO.git
+    cd FAST_LIO
+    git submodule update --init
+    cd ../..
+    catkin_make
+    source devel/setup.bash
+```
+- Remember to source the livox_ros_driver before build (follow 1.3 **livox_ros_driver**)
+- If you want to use a custom build of PCL, add the following line to ~/.bashrc
+```export PCL_ROOT={CUSTOM_PCL_PATH}```
+## 3. Directly run
+Noted:
+A. Please make sure the IMU and LiDAR are **Synchronized**, that's important.
+B. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important for the forward propagation and backwark propagation.
+### 3.1 For Avia
+Connect to your PC to Livox Avia LiDAR by following  [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then
+```
+    cd ~/$FAST_LIO_ROS_DIR$
+    source devel/setup.bash
+    roslaunch fast_lio mapping_avia.launch
+    roslaunch livox_ros_driver livox_lidar_msg.launch
+```
+- For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. ``` livox_lidar.launch ``` can not produce it right now.
+- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage.
+
+### 3.2 For Livox serials with external IMU
+
+mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run:
+
+Edit ``` config/avia.yaml ``` to set the below parameters:
+
+1. LiDAR point cloud topic name: ``` lid_topic ```
+2. IMU topic name: ``` imu_topic ```
+3. Translational extrinsic: ``` extrinsic_T ```
+4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
+- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual.
+- FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter ```time_sync_en``` to ture to turn on. But turn on **ONLY IF external time synchronization is really not possible**, since the software time sync cannot make sure accuracy.
+
+### 3.3 For Velodyne or Ouster (Velodyne as an example)
+
+Step A: Setup before run
+
+Edit ``` config/velodyne.yaml ``` to set the below parameters:
+
+1. LiDAR point cloud topic name: ``` lid_topic ```
+2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine)
+3. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ```
+4. Translational extrinsic: ``` extrinsic_T ```
+5. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix)
+- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame).
+
+Step B: Run below
+```
+    cd ~/$FAST_LIO_ROS_DIR$
+    source devel/setup.bash
+    roslaunch fast_lio mapping_velodyne.launch
+```
+
+Step C: Run LiDAR's ros driver or play rosbag.
+
+### 3.4 PCD file save
+
+Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds.
+
+*Tips for pcl_viewer:*
+- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. 
+```
+    1 is all random
+    2 is X values
+    3 is Y values
+    4 is Z values
+    5 is intensity
+```
+
+## 4. Rosbag Example
+### 4.1 Livox Avia Rosbag
+<div align="left">
+<img src="doc/results/HKU_LG_Indoor.png" width=47% />
+<img src="doc/results/HKU_MB_002.png" width = 51% >
+
+Files: Can be downloaded from [google drive](https://drive.google.com/drive/folders/1YL5MQVYgAM8oAWUm7e3OGXZBPKkanmY1?usp=sharing)
+
+Run:
+```
+roslaunch fast_lio mapping_avia.launch
+rosbag play YOUR_DOWNLOADED.bag
+
+```
+
+### 4.2 Velodyne HDL-32E Rosbag
+
+**NCLT Dataset**: Original bin file can be found [here](http://robots.engin.umich.edu/nclt/).
+
+We produce [Rosbag Files](https://drive.google.com/drive/folders/1VBK5idI1oyW0GC_I_Hxh63aqam3nocNK?usp=sharing) and [a python script](https://drive.google.com/file/d/1leh7DxbHx29DyS1NJkvEfeNJoccxH7XM/view) to generate Rosbag files: ```python3 sensordata_to_rosbag_fastlio.py bin_file_dir bag_name.bag```
+    
+Run:
+```
+roslaunch fast_lio mapping_velodyne.launch
+rosbag play YOUR_DOWNLOADED.bag
+```
+
+## 5.Implementation on UAV
+In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below.
+
+The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future.
+
+<div align="center">
+    <img src="doc/uav01.jpg" width=40.5% >
+    <img src="doc/uav_system.png" width=57% >
+</div>
+
+## 6.Acknowledgments
+
+Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox).

config/avia.yaml

@@ -0,0 +1,79 @@
+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: 2
+
+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_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_T: [ 0.04165, 0.02326, -0.0284 ]
+    extrinsic_R: [ 1, 0, 0,
+                   0, 1, 0,
+                   0, 0, 1]
+
+publish:
+    path_en:  true
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: false  # 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.
+                                 # voxel filter paprams
+odometrySurfLeafSize: 0.1                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.1                   # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1                            # meters
+rotation_tollerance: 1                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: true                   # use loopclousre or not 
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+locate_mode: false
+locate_map: "/home/zx/Downloads/LOAM2/filterGlobalMap.pcd"  #"/home/zx/doc/ros_ws/map/open_road/filterGlobalMap.pcd"  #/home/zx/Downloads/LOAM2
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/Downloads/avia/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+
+

config/avia_debug.yaml

@@ -0,0 +1,85 @@
+common:
+    lid_topic:  "/livox/lidar"
+    imu_topic:  "/handsfree/imu"
+    gnss_topic:  /gps/fix         #"/imu/data"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+
+preprocess:
+    lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 6
+    blind: 4
+
+mapping:
+    acc_cov: 0.1
+    gyr_cov: 0.1
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    fov_degree:    70
+    det_range:     450.0
+    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_T: [ 0.008, 0.01, -0.08 ]
+    extrinsic_R: [ 1, 0, 0,
+                   0, 1, 0,
+                   0, 0, 1]
+
+publish:
+    path_en:  true
+    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: true
+    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.
+
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.2                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.1                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.2                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold	选取关键帧的距离阈值
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold	角度阈值
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   no_used
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled) no_used
+
+# Loop closure
+loopClosureEnableFlag: true
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 3.0            # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# GPS Settings
+useImuHeadingInitialization: false           # if using GPS data, set to "true"
+useGpsElevation: false                      # if GPS elevation is bad, set to "false"
+gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
+poseCovThreshold: 0 #25.0                      # m^2, threshold for using GPS data  位姿协方差阈值 from isam2
+
+
+# Visualization
+globalMapVisualizationSearchRadius: 100.0    # meters, global map visualization radius,  iktree submap 的搜索范围
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/fast_lio_sam_ws/src/FAST_LIO_SAM/PCD/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

config/horizon (copy).yaml

@@ -0,0 +1,82 @@
+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: 2                   #4m  point distance<blind   ignored
+
+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_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
+    extrinsic_R: [ 1, 0, 0,
+                   0, 1, 0,
+                   0, 0, 1]
+
+publish:
+    path_en:  true
+    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: true
+    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.
+
+
+
+
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.1                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.1                   # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1                            # meters
+rotation_tollerance: 1                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: true                   # use loopclousre or not 
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/Downloads/LOAM2/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+
+

config/horizon.yaml

@@ -0,0 +1,84 @@
+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: 2                   #4m  point distance<blind   ignored
+
+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_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_T: [ 0.05512, 0.02226, -0.0297 ]
+    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: false       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: false  # 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.
+
+
+
+
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.1                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.1                   # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1                            # meters
+rotation_tollerance: 1                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: true                   # use loopclousre or not 
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+locate_mode: true
+locate_map: "/home/zx/Downloads/LOAM2/filterGlobalMap.pcd"  #"/home/zx/doc/ros_ws/map/open_road/filterGlobalMap.pcd"  #/home/zx/Downloads/LOAM2
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/Downloads/LOAM2/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+
+

config/large_velodyne.yaml

@@ -0,0 +1,94 @@
+common:
+    lid_topic:  "/points_raw"
+    imu_topic:  "/imu_correct"
+
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+    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: 16
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    blind: 1
+
+mapping:
+    acc_cov: 3.9939570888238808e-03
+    gyr_cov: 1.5636343949698187e-03
+    b_acc_cov: 6.4356659353532566e-05
+    b_gyr_cov:  3.5640318696367613e-05
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ 0, 0, 0]
+    extrinsic_R: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, 1]
+
+publish:
+    path_en:  true
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: false  # 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.
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.2                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.2                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: true                   # use loopclousre or not 
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+
+
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+locate_mode: true
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/doc/ros_ws/map/large_vlp"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+

config/ouster64.yaml

@@ -0,0 +1,33 @@
+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_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_T: [ 0.0, 0.0, 0.0 ]
+    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: true
+    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.

config/rs128.yaml

@@ -0,0 +1,43 @@
+common:
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+    lid_topic: "/rslidar"
+    imu_topic: "/imu"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+    
+preprocess:
+    lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 RS
+    scan_line: 128
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    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_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ 1.494363791111, 0.000030061324, 0.903719820280]
+    extrinsic_R: [ -0.508348737805, 0.861029086113, -0.014507709641,
+                   -0.861100925582, -0.508429079519, -0.002251013466,
+                   -0.009314329527, 0.011348302346, 0.999892223842]
+
+publish:
+    path_en:  true
+    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.

config/rs_16.yaml

@@ -0,0 +1,99 @@
+common:
+    lid_topic:  "/rslidar_points"     #"/velodyne_points"
+    imu_topic:  "/imu/data"        #"/imu/data"
+    gnss_topic:  /gps/fix         #"/imu/data"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+
+    
+preprocess:
+    lidar_type: 4               # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 32
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    blind: 3
+
+mapping:
+    acc_cov: 0.1
+    gyr_cov: 0.1
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    init_pos: [1.0, 1.0, 1.0]
+    init_rot: [0, 0, 0, 1]
+    extrinsic_T: [ 0, 0, -0.1]
+    extrinsic_R: [ 1, 0, 0,
+                   0, 1, 0,
+                   0, 0, 1]
+publish:
+    path_en:  true
+    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: true
+    interval: 100                # 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.
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold	选取关键帧的距离阈值
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold	角度阈值
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   no_used
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled) no_used
+
+# Loop closure
+loopClosureEnableFlag: true
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 20.0            # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# GPS Settings
+useImuHeadingInitialization: false           # if using GPS data, set to "true"
+useGpsElevation: false                      # if GPS elevation is bad, set to "false"
+gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
+poseCovThreshold: 0 #25.0                      # m^2, threshold for using GPS data  位姿协方差阈值 from isam2
+
+
+# Visualization
+globalMapVisualizationSearchRadius: 100.0    # meters, global map visualization radius,  iktree submap 的搜索范围
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/fast_lio_sam_ws/src/FAST_LIO_SAM/PCD/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+

config/velodyne.yaml

@@ -0,0 +1,46 @@
+common:
+    lid_topic:  "/velodyne_points"
+    imu_topic:  "/imu/data"
+
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+    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
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    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_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ 0, 0, 0.28]
+    extrinsic_R: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, 1]
+
+publish:
+    path_en:  true
+    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: true
+    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.

config/velodyne16.yaml

@@ -0,0 +1,94 @@
+common:
+    lid_topic:  "/velodyne_points"
+    imu_topic:  "/imu/data"
+
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+    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: 16
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    blind: 1
+
+mapping:
+    acc_cov: 0.1
+    gyr_cov: 0.1
+    b_acc_cov: 0.0001
+    b_gyr_cov: 0.0001
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ 0, 0, 0.28]
+    extrinsic_R: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, 1]
+
+publish:
+    path_en:  true
+    scan_publish_en:  true       # false: close all the point cloud output
+    dense_publish_en: false       # false: low down the points number in a global-frame point clouds scan.
+    scan_bodyframe_pub_en: false  # 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.
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.2                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.2                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: true                   # use loopclousre or not 
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+
+
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+locate_mode: true
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/doc/ros_ws/map/vlp16"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+

config/velodyne16_campus.yaml

@@ -0,0 +1,92 @@
+common:
+    lid_topic:  /points_raw     #"/velodyne_points"
+    imu_topic:  /imu_correct         #"/imu/data"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+
+    
+preprocess:
+    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 16
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+    blind: 1
+
+mapping:
+    acc_cov: 3.9939570888238808e-03
+    gyr_cov: 1.5636343949698187e-03
+    b_acc_cov: 6.4356659353532566e-05
+    b_gyr_cov: 3.5640318696367613e-05
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ 0, 0, 0]
+    extrinsic_R: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, 1]
+
+
+publish:
+    path_en:  true
+    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: true
+    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.
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: true
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/fast_lio_sam_ws/src/FAST_LIO_SAM/PCD/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+

config/velodyne16_lio_sam_dataset.yaml

@@ -0,0 +1,102 @@
+common:
+    lid_topic:  /points_raw     #"/velodyne_points"
+    imu_topic:  /imu_raw         #"/imu/data"
+    gnss_topic:  /gps/fix         #"/imu/data"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+
+    
+preprocess:
+    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 16
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    blind: 1
+
+mapping:
+    acc_cov: 3.9939570888238808e-03
+    gyr_cov: 1.5636343949698187e-03
+    b_acc_cov: 6.4356659353532566e-05
+    b_gyr_cov: 3.5640318696367613e-05
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ 0, 0, 0]
+    extrinsic_R: [ -1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, -1]
+
+    extrinT_Gnss2Lidar: [ 0, 0, 0]
+    extrinR_Gnss2Lidar: [ 1, 0, 0, 
+                   0, 1, 0, 
+                   0, 0, 1]
+publish:
+    path_en:  true
+    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: true
+    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.
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold	选取关键帧的距离阈值
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold	角度阈值
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   no_used
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled) no_used
+
+# Loop closure
+loopClosureEnableFlag: true
+loopClosureFrequency: 4.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 20.0            # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment
+
+# GPS Settings
+useImuHeadingInitialization: false           # if using GPS data, set to "true"
+useGpsElevation: false                      # if GPS elevation is bad, set to "false"
+gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
+poseCovThreshold: 0 #25.0                      # m^2, threshold for using GPS data  位姿协方差阈值 from isam2
+
+
+# Visualization
+globalMapVisualizationSearchRadius: 100.0    # meters, global map visualization radius,  iktree submap 的搜索范围
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/fast_lio_sam_ws/src/FAST_LIO_SAM/PCD/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+

config/velodyne64_kitti_dataset.yaml

@@ -0,0 +1,89 @@
+common:
+    lid_topic:  /points_raw     #"/velodyne_points"
+    imu_topic:  /imu_raw         #"/imu/data"
+    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+#NCLT
+#    lid_topic: "/points_raw"
+#    imu_topic: "/imu_raw"
+
+#KITTI
+#    lid_topic: "/kitti/velo/pointcloud"
+#    imu_topic: "/kitti/oxts/imu"
+
+#RS LiDar
+#    lid_topic: "/rslidar"
+#    imu_topic: "/imu"
+
+    
+preprocess:
+    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
+    scan_line: 64
+    scan_rate: 10                # only need to be set for velodyne, unit: Hz,
+    timestamp_unit: 2            # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
+    blind: 1
+
+mapping:
+    acc_cov: 3.9939570888238808e-03
+    gyr_cov: 1.5636343949698187e-03
+    b_acc_cov: 6.4356659353532566e-05
+    b_gyr_cov: 3.5640318696367613e-05
+    fov_degree:    180
+    det_range:     100.0
+    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic,
+    extrinsic_T: [ -8.086759e-01, 3.195559e-01, -7.997231e-01]
+    extrinsic_R: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]
+
+publish:
+    path_en:  true
+    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: true
+    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.
+
+# voxel filter paprams
+odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
+mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
+mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor
+
+# robot motion constraint (in case you are using a 2D robot)
+z_tollerance: 1000                            # meters
+rotation_tollerance: 1000                     # radians
+
+# CPU Params
+numberOfCores: 4                              # number of cores for mapping optimization
+mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency
+
+# Surrounding map
+surroundingkeyframeAddingDistThreshold: 10  #1.0   # meters, regulate keyframe adding threshold
+surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
+surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
+surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+# Loop closure
+loopClosureEnableFlag: false
+loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
+surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
+historyKeyframeSearchRadius: 5 #1.5             # meters, key frame that is within n meters from current pose will be considerd for loop closure
+historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
+historyKeyframeSearchNum: 20                  # number of hostory key frames will be fused into a submap for loop closure
+historyKeyframeFitnessScore: 0.5              # icp threshold, the smaller the better alignment
+
+# Visualization
+globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
+globalMapVisualizationPoseDensity: 10      # meters, global map visualization keyframe density
+globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
+
+# visual iktree_map  
+visulize_IkdtreeMap: true
+
+# visual iktree_map  
+recontructKdTree: true
+
+# Export settings
+savePCD: true                         # https://github.com/TixiaoShan/LIO-SAM/issues/3
+savePCDDirectory: "/fast_lio_sam_ws/src/FAST_LIO_SAM/PCD/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
+

include/Exp_mat.h

@@ -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

include/IKFoM_toolkit/esekfom/util.hpp

@@ -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__

include/IKFoM_toolkit/mtk/build_manifold.hpp

@@ -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_*/

include/IKFoM_toolkit/mtk/src/SubManifold.hpp

@@ -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_ */

include/IKFoM_toolkit/mtk/src/mtkmath.hpp

@@ -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_ */

include/IKFoM_toolkit/mtk/src/vectview.hpp

@@ -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_ */

include/IKFoM_toolkit/mtk/startIdx.hpp

@@ -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_

include/IKFoM_toolkit/mtk/types/S2.hpp

@@ -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_*/

include/IKFoM_toolkit/mtk/types/SOn.hpp

@@ -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_*/
+

include/IKFoM_toolkit/mtk/types/vect.hpp

@@ -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_*/

include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp

@@ -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_ */

include/common_lib.h

@@ -0,0 +1,260 @@
+#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_sam/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_sam::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);
+
+// 储存一帧lidar数据及imu数据序列
+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; // lidar data begin time in the MeasureGroup
+    double lidar_end_time; // lidar data end time in the MeasureGroup
+    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

include/ikd-Tree/README.md

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

include/ikd-Tree/ikd_Tree.h

@@ -0,0 +1,188 @@
+#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);
+    void reconstruct(PointVector point_cloud);
+    BoxPointType tree_range();
+    PointVector PCL_Storage;     
+    KD_TREE_NODE * Root_Node = nullptr;
+    int max_queue_size = 0;
+};

include/so3_math.h

@@ -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

include/use-ikfom.hpp

@@ -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; //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)) //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

launch/avia_debug.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for mid-70 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/avia_debug.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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

launch/gdb_debug_example.launch

@@ -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>

launch/mapping_avia.launch

@@ -0,0 +1,24 @@
+<launch>
+<!-- Launch file for Livox AVIA LiDAR -->
+
+	<arg name="rviz" default="true" />
+
+	<rosparam command="load" file="$(find fast_lio_sam)/config/avia.yaml" />
+
+<!--	feature-based-->
+	<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_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+	<group if="$(arg rviz)">
+	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+	</group>
+
+</launch>

launch/mapping_horizon.launch

@@ -0,0 +1,21 @@
+<launch>
+<!-- Launch file for Livox Horizon LiDAR -->
+
+	<arg name="rviz" default="true" />
+
+	<rosparam command="load" file="$(find fast_lio_sam)/config/horizon.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_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+	<group if="$(arg rviz)">
+	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+	</group>
+
+</launch>

launch/mapping_large_velodyne.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/large_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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

launch/mapping_ouster64.launch

@@ -0,0 +1,21 @@
+<launch>
+<!-- Launch file for ouster OS2-64 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio)/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" />
+    <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
+
+    <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>

launch/mapping_rs.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/rs_16.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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

launch/mapping_velodyne16.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/velodyne16.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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

launch/mapping_velodyne16_campus.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/large_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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

launch/mapping_velodyne16_lio_sam_dataset.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/velodyne16_lio_sam_dataset.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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" launch-prefix="gdb -q -ex  run --args" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

launch/mapping_velodyne64_kitti_dataset.launch

@@ -0,0 +1,22 @@
+<launch>
+  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
+
+    <arg name="rviz" default="true" />
+
+    <rosparam command="load" file="$(find fast_lio_sam)/config/velodyne64_kitti_dataset.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" />
+    <node pkg="fast_lio_sam" type="fastlio_sam_mapping" name="laserMapping" output="screen" /> 
+
+    <group if="$(arg rviz)">
+    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_sam)/rviz_cfg/loam_livox.rviz" />
+    </group>
+
+</launch>

msg/Pose6D.msg

@@ -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

package.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<package>
+  <name>fast_lio_sam</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_driver</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_driver</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <test_depend>rostest</test_depend>
+  <test_depend>rosbag</test_depend>
+
+  <export>
+  </export>
+</package>

src/GNSS_Processing.hpp

@@ -0,0 +1,130 @@
+#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"
+
+#include <GeographicLib/LocalCartesian.hpp>                 //  调用GeographicLib库
+
+class GnssProcess
+{
+ public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    double time ;
+    double latitude ;
+    double longitude ;
+    double altitude ;
+    double local_E ;
+    double local_N ;
+    double local_U ;
+    int status ;
+    int service ;
+
+    double origin_longitude;
+    double origin_latitude;
+    double origin_altitude;
+
+    V3D pose_cov ;
+
+    GnssProcess();
+    ~GnssProcess();
+
+    void InitOriginPosition(double latitude, double longitude, double altitude);
+    void UpdateXYZ(double latitude, double longitude, double altitude);
+
+    void Reverse(
+      const double &local_E, const double &local_N, const double &local_U,
+      double &lat, double &lon, double &alt
+    );
+
+    void set_extrinsic(const V3D &transl, const M3D &rot);
+    void set_extrinsic(const V3D &transl);
+    void set_extrinsic(const MD(4,4) &T);
+
+  private:
+    GeographicLib::LocalCartesian geo_converter;
+
+    M3D Gnss_R_wrt_Lidar ;
+    V3D Gnss_T_wrt_Lidar;
+};
+
+GnssProcess::GnssProcess()
+{
+    time = 0.0;
+    local_E = 0.0;
+    local_N = 0.0;
+    local_U = 0.0;
+    status = 0;
+    service = 0;
+
+    origin_longitude = 0 ;
+    origin_latitude = 0;
+    origin_altitude = 0;
+    pose_cov = Zero3d;
+    Gnss_T_wrt_Lidar = Zero3d;  
+    Gnss_R_wrt_Lidar = Eye3d;
+}
+
+GnssProcess::~GnssProcess() {}
+
+// 初始化原点， WGS84 -> ENU   ???  调试结果好像是 NED 北东地
+void GnssProcess::InitOriginPosition(double latitude, double longitude, double altitude)
+{
+    geo_converter.Reset(latitude, longitude, altitude);
+    ROS_INFO("Init    Gnss  OriginPosition");   
+    origin_latitude = latitude;
+    origin_longitude = longitude;
+    origin_altitude = altitude;
+}
+
+// 获取更新后的ENU坐标
+void GnssProcess::UpdateXYZ(double latitude, double longitude, double altitude) {
+    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
+}
+
+void GnssProcess::Reverse(
+    const double &local_E, const double &local_N, const double &local_U,
+    double &lat, double &lon, double &alt
+) {
+    geo_converter.Reverse(local_E, local_N, local_U, lat, lon, alt);
+}
+
+void GnssProcess::set_extrinsic(const MD(4,4) &T)
+{
+  Gnss_T_wrt_Lidar = T.block<3,1>(0,3);
+  Gnss_R_wrt_Lidar = T.block<3,3>(0,0);
+}
+
+void GnssProcess::set_extrinsic(const V3D &transl)
+{
+  Gnss_T_wrt_Lidar = transl;
+  Gnss_R_wrt_Lidar.setIdentity();
+}
+
+void GnssProcess::set_extrinsic(const V3D &transl, const M3D &rot)
+{
+  Gnss_T_wrt_Lidar = transl;
+  Gnss_R_wrt_Lidar = rot;
+}
+
+

src/IMU_Processing.hpp

@@ -0,0 +1,407 @@
+#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; //vector
+  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; // 初始化时，imu数据迭代的次数
+  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    初始化 重力(b系下) 、 gyro_bias 、 bgn 、 ban 
+   ** 2. normalize the acceleration measurenments to unit gravity **/   //  归一化重力
+  
+  V3D cur_acc, cur_gyr;
+  
+  if (b_first_frame_)     //   提取第一帧IMU数据
+  {
+    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; //记录lidar起始时间（绝对）
+  }
+
+  //读取所有imu信息
+  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); // - acc * (g / acc)，负值，根据加速度均值，重力模长做归一化，重力记为S2流形
+  
+  //state_inout.rot = Eye3d; // Exp(mean_acc.cross(V3D(0, 0, -1 / scale_gravity)));
+  init_state.bg  = mean_gyr; // 初始化陀螺仪bias（初值？）为平均角速度
+  init_state.offset_T_L_I = Lidar_T_wrt_IMU;        //   t_lidar_imu  translate外参
+  init_state.offset_R_L_I = Lidar_R_wrt_IMU;       //   R_lidar_imu rotation 外参
+  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; //外参r
+  init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001;//外参t
+  init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001;//bias g
+  init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001;//bias a
+  init_P(21,21) = init_P(22,22) = 0.00001; // S2   重力记为S2流形
+  kf_state.change_P(init_P);
+  last_imu_ = meas.imu.back(); //记录最后一个imu
+}
+
+// IMU 后向传播，点云去畸变
+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; // imu数据序列
+  v_imu.push_front(last_imu_); //从头插入上一个imu数据
+  const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); //imu序列起始时间
+  const double &imu_end_time = v_imu.back()->header.stamp.toSec(); //imu序列结束时间
+  const double &pcl_beg_time = meas.lidar_beg_time; //点云起始时间
+  const double &pcl_end_time = meas.lidar_end_time; //点云结束时间
+  
+  /*** sort point clouds by offset time ***/
+  pcl_out = *(meas.lidar);
+  sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); //点按时间从小到大排序(从旧到新）
+  // 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();
+  //设定初始时刻相对状态(相对于imu积分初始状态的时间，上一加速度，上一角速度，速度，位置，旋转矩阵）
+  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;
+
+    //加速度和角速度均值，j与j+1的均值
+    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_) //前一IMU时间小于上一lidar结束时间
+    {
+      dt = tail->header.stamp.toSec() - last_lidar_end_time_; //记录后一IMU时间与上一lidar结束时间之差
+      // dt = tail->header.stamp.toSec() - pcl_beg_time;
+    }
+    else
+    {
+      dt = tail->header.stamp.toSec() - head->header.stamp.toSec();//相邻imu时刻时间差
+    }
+
+    //记录输入量的均值和协方差，记录的是测量值
+    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);//根据输入数据向前传播(两imu数据间隔时间差，协方差矩阵，输入数据） todo
+
+    /* save the poses at each IMU measurements */
+    imu_state = kf_state.get_x();
+    angvel_last = angvel_avr - imu_state.bg; // w = w - bg
+    acc_s_last  = imu_state.rot * (acc_avr - imu_state.ba);// a(world) = r * ( a(body) - ba)
+    for(int i=0; i<3; i++)
+    {
+      acc_s_last[i] += imu_state.grav[i];// a(world) + g(负值)
+    }
+    double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time;//m+1时刻与lidar起始时刻时间差
+    //保存帧内imu数据 m+1 时刻的pose、前一时刻与当前时刻imu数据的均值（w系）、v、p、r、
+    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);// lidar终点与imu终点时间差
+  kf_state.predict(dt, Q, in); // in 此时为最后一个imu数据，传播到lidar终点时刻与imu终点时刻较大者
+  
+  imu_state = kf_state.get_x();//lidar终点时刻与imu终点时刻较大者，imu位姿
+  last_imu_ = meas.imu.back(); //记录最后一个imu数据为上一imu数据，用于下一次imu前向传播的开头
+  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--) //imu pose从后向前遍历
+  {
+    auto head = it_kp - 1;// 前一imu位姿，j-1
+    auto tail = it_kp;// 后一imu位姿，j
+    //j-1时刻imu的p、v、r
+    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);
+
+    //j时刻的加速度和角速度
+    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 --) //head->offset_time为j-1时刻，imu相对于传播起始时刻的时间
+    {
+      dt = it_pcl->curvature / double(1000) - head->offset_time;//相对于前一imu时刻的时间差
+
+      /* 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));//R(global <-- i)
+      // global <-- head <-- i
+      
+      V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);//lidar系下索引i点的坐标
+      //imu_state:lidar终点时刻与imu终点时刻较大者，imu位姿
+      //Ti - Te
+      //T_ei 从imu终点位置指向索引i时刻imu位置的平移向量，w系, 即T_i - T_e
+      V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);//T(i <-- end)
+      // 从imu终点位置指向索引i时刻imu位置的平移向量
+
+      //imu_state.offset_R_L_I:终点时刻imu和lidar的外参
+
+      /***  变换： 终点时刻lidar系 <-- 终点时刻imu（body系） <-- 畸变纠正到global系 <-- 终点时刻imu（body系）<-- 终点时刻lidar系
+       * 在imu（body）系下进行畸变纠正到w系，可能不太准确
+       * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) 将i点，按照imu终点时刻的外参，从lidar系转换到终点时刻的imu系下
+       * (R_i * ( P ) + T_i)  畸变纠正，将i点纠正到global系下
+       * (imu_state.rot.conjugate() * ( P - T_e)  将i点旋转到终点时刻的，imu系下
+       * T_i - T_e = T_ei
+       * imu_state.offset_R_L_I.conjugate() * （P - imu_state.offset_T_L_I) 变换到终点时刻的lidar系下
+       ***/
+      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);
+
+  //imu 初始化，初始化完成后才做畸变纠正
+  if (imu_need_init_)
+  {
+      /** 1. initializing the gravity, gyro bias, acc and gyro covariance
+        * 2. normalize the acceleration measurenments to unit gravity **/
+    /// The very first lidar frame
+    IMU_init(meas, kf_state, init_iter_num);
+
+    imu_need_init_ = true;
+    
+    last_imu_   = meas.imu.back(); // 记录最后一个imu数据
+
+    state_ikfom imu_state = kf_state.get_x(); // 获取状态量
+    if (init_iter_num > MAX_INI_COUNT) //初始化imu数据量大于阈值
+    {
+      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();
+  
+  //std::cout<<"[ IMU Process ]: Time: "<<t3 - t1<<std::endl;
+}

src/preprocess.cpp

@@ -0,0 +1,981 @@
+#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;
+  SCAN_RATE = 10;
+  group_size = 8;
+  disA = 0.01;
+  disA = 0.1; // B? todo
+  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_driver::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;
+  case RS128:
+      rs_handler(msg);
+      break;
+  case LdNormal:
+    cloud_handler(msg);
+      break;
+  default:
+    printf("Error LiDAR Type");
+    break;
+  }
+  *pcl_out = pl_surf;
+}
+
+void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg) {
+  pl_surf.clear();
+
+  double t1 = omp_get_wtime();
+  int plsize = msg->point_num;
+  // cout<<"plsie: "<<plsize<<endl;
+
+  //清空缓存内的点云并预留足够空间
+  for (int i = 0; i < N_SCANS; i++) {
+    pl_buff[i].clear();
+    pl_buff[i].reserve(plsize);
+  }
+  uint valid_num = 0;
+
+  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++;
+      double range=msg->points[i].x*msg->points[i].x+msg->points[i].y*msg->points[i].y+msg->points[i].z*msg->points[i].z;
+      if (range>(blind*blind)/*valid_num % point_filter_num == 0*/) {
+        PointType pt;
+        pt.x = msg->points[i].x;
+        pt.y = msg->points[i].y;
+        pt.z = msg->points[i].z;
+        pt.intensity = msg->points[i].reflectivity;
+        pt.curvature = msg->points[i].offset_time /
+                               float(1000000); // use curvature as time of each laser points, curvature unit: ms
+        pl_surf.push_back(pt);
+        /*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 + pl_full[i].z * pl_full[i].z >
+                   (blind * blind))) {
+
+        }*/
+      }
+    }
+  }
+
+}
+
+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);
+
+  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 * 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;
+    added_pt.curvature = pl_orig.points[i].t / 1e6; // curvature unit: ms
+
+    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 = 0.361 * SCAN_RATE;       // 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)//todo check pl_orig.points[plsize - 1].time
+  {
+    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; // 记录第一个点(index 0)的yaw， to degree
+    double yaw_end = yaw_first;
+    int layer_first = pl_orig.points[0].ring; // 第一个点(index 0)的layer序号
+    for (uint i = plsize - 1; i > 0; i--) // 倒序遍历，找到与第一个点相同layer的最后一个点
+    {
+      if (pl_orig.points[i].ring == layer_first) {
+        yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;// 与第一个点相同layer的最后一个点的yaw
+        break;
+      }
+    }
+  }
+
+  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;  // curvature unit: ms
+
+    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 * 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;
+  //更新head为第一个大于blind范围的点索引
+  while(types[head].range < blind)
+  {
+    head++;
+  }
+
+  // Surf
+  plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; // 本layer除group_size（局部平面点）之外的剩余点数
+
+  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;
+
+    //第一个大于blind范围的点索引开始遍历,判断平面特征
+  for(uint i=head; i<plsize2; i++)
+  {
+    if(types[i].range < blind) // blind范围内直接跳过
+    {
+      continue;
+    }
+
+    i2 = i; //i2记录当前点索引
+
+    //输出
+    // i_nex 局部最后最后一个点的索引
+    //curr_direct 归一化后的，局部范围最后一个点与第一个点的坐标差值，即向量（i_cur --> i_nex)
+    //return 1 正常退出， 0 中途break，curr_direct置零
+    plane_type = plane_judge(pl, types, i, i_nex, curr_direct);
+    
+    if(plane_type == 1) //plane_judge正常退出
+    {
+      for(uint j=i; j<=i_nex; j++)
+      { 
+        if(j!=i && j!=i_nex)
+        {
+          types[j].ftype = Real_Plane; // 局部范围内部点定义为real平面点
+        }
+        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) // 平面夹角30度
+        {
+          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;
+    // 从head+3向后遍历，判断非平面点是不是edge，以及判断edge的类型
+  for(uint i=head+3; i<plsize2; i++)
+  {
+    if(types[i].range<blind || types[i].ftype>=Real_Plane) // 距离太近或这已经判断为edge或者平面
+    {
+      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); // sensor到当前点ray
+    Eigen::Vector3d vecs[2]; //从当前点指向前后相邻点的两个向量
+
+    for(int j=0; j<2; j++) // 计算当前点与前一点、后一点的向量，判断当前点前后两个方向的edge属性
+    {
+      int m = -1;
+      if(j == 1)
+      {
+        m = 1;
+      }
+
+      if(types[i+m].range < blind) //索引i的前一个点（m = -1) 或者后一个点（m = 1) ，距离很小
+      {
+        if(types[i].range > inf_bound) // 根据当前点平面距离，判断edge jump属性
+        {
+          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); // sensor指向 当前点的前一点或后一点的向量
+      vecs[j] = vecs[j] - vec_a; // 当前点指向前一点或后一点的向量
+      
+      types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); // cos(当前点指向前一点或后一点的向量, ray)
+      if(types[i].angle[j] < jump_up_limit) //jump_up_limit 默认cos170度
+      {
+        types[i].edj[j] = Nr_180; //ray 与 当前点指向端点外一点的向量 夹角接近180度
+      }
+      else if(types[i].angle[j] > jump_down_limit) //jump_down_limit 默认8度
+      {
+        types[i].edj[j] = Nr_zero;//ray 与 当前点指向端点外一点的向量 夹角接近0度
+      }
+    }
+
+    types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); // 当前点与相邻两点的夹角cos值
+    //根据 前端点edge jump类型，后端点edge jump类型，与后一点间距，与前一点间距的4倍，判断edge的类型
+    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;
+      }
+
+      //smallp_intersect：默认172.5度余弦值
+      //smallp_intersect：默认1.2
+      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]);//记录edge特征
+      }
+      if(last_surface != -1)
+      {
+          //edge特征和上一个surface特征之间取所有点均值，作为面特征
+          //todo 检查超限
+        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;
+}
+
+// （line点云，点属性， 当前点索引，当前点索引，当前方向）
+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; //disB?? 用于限制特征点计算时，局部范围的阈值
+  group_dis = group_dis * group_dis;
+  // i_nex = i_cur;
+
+  double two_dis;
+  vector<double> disarr; //与后一个点的距离，（line内相邻有效点间距）
+  disarr.reserve(20);
+
+  //group_size(计算特征需要的点数，默认8）， 索引范围内遍历
+  for(i_nex=i_cur; i_nex<i_cur+group_size; i_nex++)
+  {
+    if(types[i_nex].range < blind) // group_size存在点的平面距离小于blind， 方向置零，直接返回2
+    {
+      curr_direct.setZero();
+      return 2;
+    }
+    disarr.push_back(types[i_nex].dista); // 保存与后一个点的距离
+  }
+  
+  for(;;) // 继续向后遍历，保存group_dis范围以内，与后一个点的间距,记录最后一个局部点索引为i_nex
+  {
+    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++) //局部范围内，从当前点向后遍历，i_nex为局部范围内索引最大值
+  {
+    if((j >= pl.size()) || (i_cur >= pl.size())) break;
+    //计算向量（i_cur --> j）
+    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;
+    // vx,vy,vz为局部范围最后一个点与第一个点的坐标差值，即向量（i_cur --> i_nex)，以下为叉乘
+    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) //?? 最大距离平方和的乘积与最大平行四边形面积比，判断比例，返回0
+  {
+    curr_direct.setZero();
+    return 0;
+  }
+
+  uint disarrsize = disarr.size();
+  for(uint j=0; j<disarrsize-1; j++) // 排序，disarr按从大到小，leng_wid为最小的相邻点间隔距离
+  {
+    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; //j、k互换
+      }
+    }
+  }
+
+  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;// vx,vy,vz为局部范围最后一个点与第一个点的坐标差值，即向量（i_cur --> i_nex)
+  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;
+    }
+  }
+  // d1,d2为向前或向后的相邻点间距，通过nor_dir控制方向
+  double d1 = types[i+nor_dir-1].dista;
+  double d2 = types[i+3*nor_dir-2].dista;
+  double d;
+
+  //d1为较大间距
+  if(d1<d2)
+  {
+    d = d1;
+    d1 = d2;
+    d2 = d;
+  }
+
+  d1 = sqrt(d1);
+  d2 = sqrt(d2);
+
+ //间距大于edgea（2）倍 或 间距差大于edgeb（0.1）
+  if(d1>edgea*d2 || (d1-d2)>edgeb)
+  {
+    return false;
+  }
+  
+  return true;
+}
+
+void Preprocess::rs_handler(const sensor_msgs::PointCloud2_<allocator<void>>::ConstPtr &msg) {
+  pl_surf.clear();
+
+  pcl::PointCloud<rslidar_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 = 0.361 * SCAN_RATE;       // 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)//todo check pl_orig.points[plsize - 1].time
+  {
+    given_offset_time = true;
+  } else {
+    given_offset_time = false;
+  }
+
+
+  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;  // curvature unit: ms
+
+    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 * blind)) {
+        pl_surf.points.push_back(added_pt);
+      }
+    }
+  }
+
+}
+void Preprocess::cloud_handler(const sensor_msgs::PointCloud2_<allocator<void>>::ConstPtr &msg) {
+  pl_surf.clear();
+  pl_corn.clear();
+  pl_full.clear();
+
+  pcl::fromROSMsg(*msg, pl_surf);
+
+  return ;
+
+  pcl::PointCloud<rslidar_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 = 0.361 * SCAN_RATE;       // 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)//todo check pl_orig.points[plsize - 1].time
+  {
+    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; // 记录第一个点(index 0)的yaw， to degree
+    double yaw_end  = yaw_first;
+    int layer_first = pl_orig.points[0].ring; // 第一个点(index 0)的layer序号
+    for (uint i = plsize - 1; i > 0; i--) // 倒序遍历，找到与第一个点相同layer的最后一个点
+    {
+      if (pl_orig.points[i].ring == layer_first)
+      {
+        yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;// 与第一个点相同layer的最后一个点的yaw
+        break;
+      }
+    }
+  }
+
+  if(feature_enabled)
+  {
+    for (int i = 0; i < N_SCANS; i++)
+    {
+      pl_buff[i].clear();
+      pl_buff[i].reserve(plsize);
+    }
+
+    //计算时间、转换点云格式为PointType，正序遍历
+    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; // 但前点yaw, to degree
+        if (is_first[layer]) // 如果当前点是其对应layer的第一个点
+        {
+          // printf("layer: %d; is first: %d", layer, is_first[layer]);
+          yaw_fp[layer]=yaw_angle; // 记录为当前点对应layer的起始yaw
+          is_first[layer]=false;
+          added_pt.curvature = 0.0; //当前点curvature（时间）置零
+          yaw_last[layer]=yaw_angle; // 暂时记录为当前点对应layer的结束yaw
+          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; // 记录当前layer最后一个点的yaw
+        time_last[layer]=added_pt.curvature; //  记录当前layer最后一个点的时间
+      }
+
+      pl_buff[layer].points.push_back(added_pt);
+    }
+
+    for (int j = 0; j < N_SCANS; j++)
+    {
+      PointCloudXYZI &pl = pl_buff[j]; // points_line
+      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;  // curvature unit: ms
+
+      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 * blind))
+        {
+          pl_surf.points.push_back(added_pt);
+        }
+      }
+    }
+  }
+}
+

src/preprocess.h

@@ -0,0 +1,166 @@
+#include <ros/ros.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <livox_ros_driver/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,RS128,LdNormal}; //{1, 2, 3, 4}
+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}; // 未判断，接近0度，接近180度，接近远端，接近近端
+
+//用于记录每个点的距离、角度、特征种类等属性
+struct orgtype
+{
+  double range; //平面距离
+  double dista; //与后一个点的间距平方
+  double angle[2]; // cos(当前点指向前一点或后一点的向量, ray)
+  double intersect;// // 当前点与相邻两点的夹角cos值
+  E_jump edj[2]; // 点前后两个方向的edge_jump类型
+  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 rslidar_ros {
+    struct EIGEN_ALIGN16 Point {
+        PCL_ADD_POINT4D;
+        float intensity;
+        float time;
+        uint16_t ring;
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
+}  // namespace rslidar_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(rslidar_ros::Point,
+                                  (float, x, x)
+                                  (float, y, y)
+                                  (float, z, z)
+                                  (float, intensity, curvature)
+                                  (float, time, normal_x)
+                                  (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)
+)
+
+/**
+ * 6D位姿点云结构定义
+*/
+struct PointXYZIRPYT
+{
+    PCL_ADD_POINT4D     
+    PCL_ADD_INTENSITY;  
+    float roll;         
+    float pitch;
+    float yaw;
+    double time;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
+} EIGEN_ALIGN16;                    
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
+                                   (float, x, x) (float, y, y)
+                                   (float, z, z) (float, intensity, intensity)
+                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
+                                   (double, time, time))
+
+typedef PointXYZIRPYT  PointTypePose;
+
+
+class Preprocess
+{
+  public:
+//   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Preprocess();
+  ~Preprocess();
+  
+  void process(const livox_ros_driver::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, SCAN_RATE;
+  double blind; //xy平面距离，小于此阈值不计算特征
+  bool feature_enabled, given_offset_time;
+  ros::Publisher pub_full, pub_surf, pub_corn;
+    
+
+  private:
+  void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg);
+  void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void rs_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+    void cloud_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;
+};

src/scancontext/KDTreeVectorOfVectorsAdaptor.h

@@ -0,0 +1,117 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
+ *   All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *************************************************************************/
+
+#pragma once
+
+#include "nanoflann.hpp"
+
+#include <vector>
+
+// ===== This example shows how to use nanoflann with these types of containers: =======
+//typedef std::vector<std::vector<double> > my_vector_of_vectors_t;
+//typedef std::vector<Eigen::VectorXd> my_vector_of_vectors_t;   // This requires #include <Eigen/Dense>
+// =====================================================================================
+
+
+/** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
+  *  The i'th vector represents a point in the state space.
+  *
+  *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
+  *  \tparam num_t The type of the point coordinates (typically, double or float).
+  *  \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
+  *  \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
+  */
+template <class VectorOfVectorsType, typename num_t = double, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
+struct KDTreeVectorOfVectorsAdaptor
+{
+	typedef KDTreeVectorOfVectorsAdaptor<VectorOfVectorsType,num_t,DIM,Distance> self_t;
+	typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
+	typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType>  index_t;
+
+	index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
+
+	/// Constructor: takes a const ref to the vector of vectors object with the data points
+	KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)
+	{
+		assert(mat.size() != 0 && mat[0].size() != 0);
+		const size_t dims = mat[0].size();
+		if (DIM>0 && static_cast<int>(dims) != DIM)
+			throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
+		index = new index_t( static_cast<int>(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
+		index->buildIndex();
+	}
+
+	~KDTreeVectorOfVectorsAdaptor() {
+		delete index;
+	}
+
+	const VectorOfVectorsType &m_data;
+
+	/** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
+	  *  Note that this is a short-cut method for index->findNeighbors().
+	  *  The user can also call index->... methods as desired.
+	  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
+	  */
+	inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
+	{
+		nanoflann::KNNResultSet<num_t,IndexType> resultSet(num_closest);
+		resultSet.init(out_indices, out_distances_sq);
+		index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
+	}
+
+	/** @name Interface expected by KDTreeSingleIndexAdaptor
+	  * @{ */
+
+	const self_t & derived() const {
+		return *this;
+	}
+	self_t & derived()       {
+		return *this;
+	}
+
+	// Must return the number of data points
+	inline size_t kdtree_get_point_count() const {
+		return m_data.size();
+	}
+
+	// Returns the dim'th component of the idx'th point in the class:
+	inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
+		return m_data[idx][dim];
+	}
+
+	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
+	//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
+	//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
+	template <class BBOX>
+	bool kdtree_get_bbox(BBOX & /*bb*/) const {
+		return false;
+	}
+
+	/** @} */
+
+}; // end of KDTreeVectorOfVectorsAdaptor

src/scancontext/Scancontext.cpp

@@ -0,0 +1,422 @@
+#include "Scancontext.h"
+// namespace SC2
+// {
+
+void coreImportTest (void)
+{
+    std::cout << "scancontext lib is successfully imported." << std::endl;
+} // coreImportTest
+
+
+float rad2deg(float radians)
+{
+    return radians * 180.0 / M_PI;
+}
+
+float deg2rad(float degrees)
+{
+    return degrees * M_PI / 180.0;
+}
+
+// return [0,360)
+float xy2theta( const float & _x, const float & _y )
+{
+    if ( (_x >= 0) & (_y >= 0)) 
+        return (180/M_PI) * atan(_y / _x);
+
+    if ( (_x < 0) & (_y >= 0)) 
+        return 180 - ( (180/M_PI) * atan(_y / (-_x)) );
+
+    if ( (_x < 0) & (_y < 0)) 
+        return 180 + ( (180/M_PI) * atan(_y / _x) );
+
+    if ( (_x >= 0) & (_y < 0))
+        return 360 - ( (180/M_PI) * atan((-_y) / _x) );
+} // xy2theta
+
+
+Eigen::MatrixXd circshift( Eigen::MatrixXd &_mat, int _num_shift )
+{
+    // shift columns to right direction 
+    assert(_num_shift >= 0);
+
+    if( _num_shift == 0 )
+    {
+        Eigen::MatrixXd shifted_mat( _mat );
+        return shifted_mat; // Early return 
+    }
+
+    Eigen::MatrixXd shifted_mat = Eigen::MatrixXd::Zero( _mat.rows(), _mat.cols() );
+    for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )
+    {
+        int new_location = (col_idx + _num_shift) % _mat.cols();
+        shifted_mat.col(new_location) = _mat.col(col_idx);
+    }
+
+    return shifted_mat;
+
+} // circshift
+
+
+std::vector<float> eig2stdvec( Eigen::MatrixXd _eigmat )
+{
+    std::vector<float> vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );
+    return vec;
+} // eig2stdvec
+
+
+double SCManager::distDirectSC ( Eigen::MatrixXd &_sc1, Eigen::MatrixXd &_sc2 )
+{
+    int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
+    double sum_sector_similarity = 0;
+    for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )
+    {
+      Eigen::VectorXd col_sc1 = _sc1.col(col_idx);
+      Eigen::VectorXd col_sc2 = _sc2.col(col_idx);
+        
+        if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) )
+            continue; // don't count this sector pair. 
+
+        double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());
+
+        sum_sector_similarity = sum_sector_similarity + sector_similarity;
+        num_eff_cols = num_eff_cols + 1;
+    }
+    
+    double sc_sim = sum_sector_similarity / num_eff_cols;
+    return 1.0 - sc_sim;
+
+} // distDirectSC
+
+
+int SCManager::fastAlignUsingVkey( Eigen::MatrixXd & _vkey1, Eigen::MatrixXd & _vkey2)
+{
+    int argmin_vkey_shift = 0;
+    double min_veky_diff_norm = 10000000;
+    for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
+    {
+        Eigen::MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
+
+        Eigen::MatrixXd vkey_diff = _vkey1 - vkey2_shifted;
+
+        double cur_diff_norm = vkey_diff.norm();
+        if( cur_diff_norm < min_veky_diff_norm )
+        {
+            argmin_vkey_shift = shift_idx;
+            min_veky_diff_norm = cur_diff_norm;
+        }
+    }
+
+    return argmin_vkey_shift;
+
+} // fastAlignUsingVkey
+
+
+std::pair<double, int> SCManager::distanceBtnScanContext( Eigen::MatrixXd &_sc1, Eigen::MatrixXd &_sc2 )
+{
+    // 1. fast align using variant key (not in original IROS18)
+    Eigen::MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
+    Eigen::MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
+    int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );
+
+    const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 
+    std::vector<int> shift_idx_search_space { argmin_vkey_shift };
+    for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
+    {
+        shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
+        shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
+    }
+    std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());
+
+    // 2. fast columnwise diff 
+    int argmin_shift = 0;
+    double min_sc_dist = 10000000;
+    for ( int num_shift: shift_idx_search_space )
+    {
+        Eigen::MatrixXd sc2_shifted = circshift(_sc2, num_shift);
+        double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
+        if( cur_sc_dist < min_sc_dist )
+        {
+            argmin_shift = num_shift;
+            min_sc_dist = cur_sc_dist;
+        }
+    }
+
+    return make_pair(min_sc_dist, argmin_shift);
+
+} // distanceBtnScanContext
+
+
+Eigen::MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
+{
+
+
+    int num_pts_scan_down = _scan_down.points.size();
+
+    // main
+    const int NO_POINT = -1000;
+    Eigen::MatrixXd desc = NO_POINT * Eigen::MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
+
+    SCPointType pt;
+    float azim_angle, azim_range; // wihtin 2d plane
+    int ring_idx, sctor_idx;
+    for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
+    {
+        pt.x = _scan_down.points[pt_idx].x; 
+        pt.y = _scan_down.points[pt_idx].y;
+        pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).
+
+        // xyz to ring, sector
+        azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
+        azim_angle = xy2theta(pt.x, pt.y);
+
+        // if range is out of roi, pass
+        if( azim_range > PC_MAX_RADIUS )
+            continue;
+
+        ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
+        sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );
+
+        // taking maximum z 
+        if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
+            desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
+    }
+
+    // reset no points to zero (for cosine dist later)
+    for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
+        for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
+            if( desc(row_idx, col_idx) == NO_POINT )
+                desc(row_idx, col_idx) = 0;
+
+
+    return desc;
+} // SCManager::makeScancontext
+
+
+Eigen::MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
+{
+    /* 
+     * summary: rowwise mean vector
+    */
+    Eigen::MatrixXd invariant_key(_desc.rows(), 1);
+    for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
+    {
+        Eigen::MatrixXd curr_row = _desc.row(row_idx);
+        invariant_key(row_idx, 0) = curr_row.mean();
+    }
+
+    return invariant_key;
+} // SCManager::makeRingkeyFromScancontext
+
+
+Eigen::MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
+{
+    /* 
+     * summary: columnwise mean vector
+    */
+    Eigen::MatrixXd variant_key(1, _desc.cols());
+    for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )
+    {
+        Eigen::MatrixXd curr_col = _desc.col(col_idx);
+        variant_key(0, col_idx) = curr_col.mean();
+    }
+
+    return variant_key;
+} // SCManager::makeSectorkeyFromScancontext
+
+
+const Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void)
+{
+    return polarcontexts_.back();
+}
+
+
+void SCManager::saveScancontextAndKeys( Eigen::MatrixXd _scd )
+{
+    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( _scd );
+    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( _scd );
+    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
+
+    polarcontexts_.push_back( _scd ); 
+    polarcontext_invkeys_.push_back( ringkey );
+    polarcontext_vkeys_.push_back( sectorkey );
+    polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
+} // SCManager::makeAndSaveScancontextAndKeys
+
+
+void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
+{
+    Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
+    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
+    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
+    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
+
+    polarcontexts_.push_back( sc ); 
+    polarcontext_invkeys_.push_back( ringkey );
+    polarcontext_vkeys_.push_back( sectorkey );
+    polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
+} // SCManager::makeAndSaveScancontextAndKeys
+
+void SCManager::setSCdistThres(double _new_thres)
+{
+    SC_DIST_THRES = _new_thres;
+} // SCManager::setThres
+
+void SCManager::setMaximumRadius(double _max_r)
+{
+    PC_MAX_RADIUS = _max_r;
+} // SCManager::setMaximumRadius
+
+std::pair<int, float> SCManager::detectLoopClosureIDBetweenSession (std::vector<float>& _curr_key, Eigen::MatrixXd& _curr_desc)
+{
+    int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
+
+    auto& curr_key = _curr_key;
+    auto& curr_desc = _curr_desc; // current observation (query)
+
+    // step 0: if first, construct the tree in batch
+    if( ! is_tree_batch_made ) // run only once
+    {
+        polarcontext_invkeys_to_search_.clear();
+        polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() ) ;
+
+        polarcontext_tree_batch_.reset(); 
+        polarcontext_tree_batch_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
+
+        is_tree_batch_made = true; // for running this block only once
+    }
+        
+    double min_dist = 10000000; // init with somthing large
+    int nn_align = 0;
+    int nn_idx = 0;
+
+    // step 1: knn search
+    std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 
+    std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
+
+    nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
+    knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
+    polarcontext_tree_batch_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); // error here
+
+    // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
+
+    for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
+    {
+      Eigen::MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
+        std::pair<double, int> sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 
+        
+        double candidate_dist = sc_dist_result.first;
+        int candidate_align = sc_dist_result.second;
+
+        if( candidate_dist < min_dist )
+        {
+            min_dist = candidate_dist;
+            nn_align = candidate_align;
+
+            nn_idx = candidate_indexes[candidate_iter_idx];
+        }
+    }
+
+    // step 3: similarity threshold
+    if( min_dist < SC_DIST_THRES )
+        loop_id = nn_idx; 
+
+    // To do: return also nn_align (i.e., yaw diff)
+    float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
+    std::pair<int, float> result {loop_id, yaw_diff_rad};
+
+    return result;
+
+} // SCManager::detectLoopClosureIDBetweenSession
+
+
+std::pair<int, float> SCManager::detectLoopClosureID ( void )
+{
+    int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
+
+    auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
+    auto curr_desc = polarcontexts_.back(); // current observation (query)
+
+    /* 
+     * step 1: candidates from ringkey tree_
+     */
+    if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)
+    {
+        std::pair<int, float> result {loop_id, 0.0};
+        return result; // Early return 
+    }
+
+    // tree_ reconstruction (not mandatory to make everytime)
+    if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost
+    {
+
+
+        polarcontext_invkeys_to_search_.clear();
+        polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;
+
+        polarcontext_tree_.reset(); 
+        polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
+        // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
+    }
+    tree_making_period_conter = tree_making_period_conter + 1;
+        
+    double min_dist = 10000000; // init with somthing large
+    int nn_align = 0;
+    int nn_idx = 0;
+
+    // knn search
+    std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 
+    std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
+
+    nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
+    knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
+    polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 
+
+
+    /* 
+     *  step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
+     */
+    for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
+    {
+        Eigen::MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
+        std::pair<double, int> sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 
+        
+        double candidate_dist = sc_dist_result.first;
+        int candidate_align = sc_dist_result.second;
+
+        if( candidate_dist < min_dist )
+        {
+            min_dist = candidate_dist;
+            nn_align = candidate_align;
+
+            nn_idx = candidate_indexes[candidate_iter_idx];
+        }
+    }
+
+    /* 
+     * loop threshold check
+     */
+    if( min_dist < SC_DIST_THRES )
+    {
+        loop_id = nn_idx; 
+    
+        // std::cout.precision(3); 
+        std::cout << "[Loop found] Nearest distance: " << min_dist << ", between " << polarcontexts_.size()-1 << " and " << nn_idx  << std::endl;
+        // cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
+    }
+    /*else
+    {
+        std::cout.precision(3); 
+        std::cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << std::endl;
+
+    }*/
+
+    // To do: return also nn_align (i.e., yaw diff)
+    float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
+    std::pair<int, float> result {loop_id, yaw_diff_rad};
+
+    return result;
+
+} // SCManager::detectLoopClosureID
+
+// } // namespace SC2

src/scancontext/Scancontext.h

@@ -0,0 +1,116 @@
+#pragma once
+
+#include <ctime>
+#include <cassert>
+#include <cmath>
+#include <utility>
+#include <vector>
+#include <algorithm> 
+#include <cstdlib>
+#include <memory>
+#include <iostream>
+
+#include <Eigen/Dense>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "nanoflann.hpp"
+#include "KDTreeVectorOfVectorsAdaptor.h"
+
+using namespace nanoflann;
+
+using std::make_pair;
+
+using std::atan2;
+using std::cos;
+using std::sin;
+
+using SCPointType = pcl::PointXYZINormal; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context)
+using KeyMat = std::vector<std::vector<float> >;
+using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >;
+
+
+// namespace SC2
+// {
+
+void coreImportTest ( void );
+
+
+// sc param-independent helper functions 
+float xy2theta( const float & _x, const float & _y );
+Eigen::MatrixXd circshift( Eigen::MatrixXd &_mat, int _num_shift );
+std::vector<float> eig2stdvec( Eigen::MatrixXd _eigmat );
+
+
+class SCManager
+{
+public: 
+    SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care.
+
+    Eigen::MatrixXd makeScancontext( pcl::PointCloud<SCPointType> & _scan_down );
+    Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc );
+    Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc );
+
+    int fastAlignUsingVkey ( Eigen::MatrixXd & _vkey1, Eigen::MatrixXd & _vkey2 );
+    double distDirectSC ( Eigen::MatrixXd &_sc1, Eigen::MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18)
+    std::pair<double, int> distanceBtnScanContext ( Eigen::MatrixXd &_sc1, Eigen::MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18)
+
+    // User-side API
+    void makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down );
+    std::pair<int, float> detectLoopClosureID( void ); // int: nearest node index, float: relative yaw  
+
+    // for ltslam 
+    // User-side API for multi-session
+    void saveScancontextAndKeys( Eigen::MatrixXd _scd );
+    std::pair<int, float> detectLoopClosureIDBetweenSession ( std::vector<float>& curr_key,  Eigen::MatrixXd& curr_desc);
+
+    const Eigen::MatrixXd& getConstRefRecentSCD(void);
+
+public:
+    // hyper parameters ()
+    const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.
+
+    const int    PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
+    const int    PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
+    double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)
+    const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
+    const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);
+
+    // tree
+    const int    NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. 
+    const int    NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper)
+
+    // loop thres
+    const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
+    // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
+
+    double SC_DIST_THRES = 0.2; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
+    // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
+
+    // config 
+    const int    TREE_MAKING_PERIOD_ = 30; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N.
+    int          tree_making_period_conter = 0;
+
+    // setter
+    void setSCdistThres(double _new_thres);
+    void setMaximumRadius(double _max_r);
+
+    // data 
+    std::vector<double> polarcontexts_timestamp_; // optional.
+    std::vector<Eigen::MatrixXd> polarcontexts_;
+    std::vector<Eigen::MatrixXd> polarcontext_invkeys_;
+    std::vector<Eigen::MatrixXd> polarcontext_vkeys_;
+
+    KeyMat polarcontext_invkeys_mat_;
+    KeyMat polarcontext_invkeys_to_search_;
+    std::unique_ptr<InvKeyTree> polarcontext_tree_;
+
+    bool is_tree_batch_made = false;
+    std::unique_ptr<InvKeyTree> polarcontext_tree_batch_;
+
+}; // SCManager
+
+// } // namespace SC2

srv/save_map.srv

@@ -0,0 +1,4 @@
+float32 resolution
+string destination
+---
+bool success

srv/save_pose.srv

@@ -0,0 +1,4 @@
+float32 resolution
+string destination
+---
+bool success