Showing preview only (1,376K chars total). Download the full file or copy to clipboard to get everything.
Repository: YWL0720/I2EKF-LO
Branch: master
Commit: 8d2158cda30e
Files: 164
Total size: 1.3 MB
Directory structure:
gitextract_qkvnzuum/
├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── Thirdparty/
│ ├── Sophus/
│ │ ├── .clang-format
│ │ ├── CMakeLists.txt
│ │ ├── LICENSE.txt
│ │ ├── README.rst
│ │ ├── Sophus.code-workspace
│ │ ├── SophusConfig.cmake.in
│ │ ├── appveyor.yml
│ │ ├── cmake_modules/
│ │ │ └── FindEigen3.cmake
│ │ ├── doxyfile
│ │ ├── doxyrest-config.lua
│ │ ├── examples/
│ │ │ ├── CMakeLists.txt
│ │ │ └── HelloSO3.cpp
│ │ ├── generate_stubs.py
│ │ ├── make_docs.sh
│ │ ├── package.xml
│ │ ├── rst-dir/
│ │ │ ├── conf.py
│ │ │ ├── page_index.rst
│ │ │ └── pysophus.rst
│ │ ├── run_format.sh
│ │ ├── scripts/
│ │ │ ├── install_docs_deps.sh
│ │ │ ├── install_linux_deps.sh
│ │ │ ├── install_linux_fmt_deps.sh
│ │ │ ├── install_osx_deps.sh
│ │ │ └── run_cpp_tests.sh
│ │ ├── setup.py
│ │ ├── sophus/
│ │ │ ├── average.hpp
│ │ │ ├── cartesian.hpp
│ │ │ ├── ceres_local_parameterization.hpp
│ │ │ ├── ceres_manifold.hpp
│ │ │ ├── ceres_typetraits.hpp
│ │ │ ├── common.hpp
│ │ │ ├── example_ensure_handler.cpp
│ │ │ ├── geometry.hpp
│ │ │ ├── interpolate.hpp
│ │ │ ├── interpolate_details.hpp
│ │ │ ├── num_diff.hpp
│ │ │ ├── rotation_matrix.hpp
│ │ │ ├── rxso2.hpp
│ │ │ ├── rxso3.hpp
│ │ │ ├── se2.hpp
│ │ │ ├── se3.hpp
│ │ │ ├── sim2.hpp
│ │ │ ├── sim3.hpp
│ │ │ ├── sim_details.hpp
│ │ │ ├── so2.hpp
│ │ │ ├── so3.hpp
│ │ │ ├── spline.hpp
│ │ │ ├── test_macros.hpp
│ │ │ ├── types.hpp
│ │ │ └── velocities.hpp
│ │ ├── sophus_pybind/
│ │ │ ├── README
│ │ │ ├── SE3PyBind.h
│ │ │ ├── SO3PyBind.h
│ │ │ ├── SophusPyBind.h
│ │ │ ├── bindings.cpp
│ │ │ ├── examples/
│ │ │ │ └── sophus_quickstart_tutorial.ipynb
│ │ │ └── tests/
│ │ │ └── sophusPybindTests.py
│ │ ├── sophus_pybind-stubs/
│ │ │ ├── py.typed
│ │ │ └── sophus_pybind.pyi
│ │ ├── sympy/
│ │ │ ├── cpp_gencode/
│ │ │ │ ├── Se2_Dx_exp_x.cpp
│ │ │ │ ├── Se2_Dx_log_this.cpp
│ │ │ │ ├── Se2_Dx_this_mul_exp_x_at_0.cpp
│ │ │ │ ├── Se3_Dx_exp_x.cpp
│ │ │ │ ├── Se3_Dx_log_this.cpp
│ │ │ │ ├── Se3_Dx_this_mul_exp_x_at_0.cpp
│ │ │ │ ├── So2_Dx_exp_x.cpp
│ │ │ │ ├── So2_Dx_log_exp_x_times_this_at_0.cpp
│ │ │ │ ├── So2_Dx_log_this.cpp
│ │ │ │ ├── So2_Dx_this_mul_exp_x_at_0.cpp
│ │ │ │ ├── So3_Dx_exp_x.cpp
│ │ │ │ ├── So3_Dx_log_exp_x_times_this_at_0.cpp
│ │ │ │ ├── So3_Dx_log_this.cpp
│ │ │ │ └── So3_Dx_this_mul_exp_x_at_0.cpp
│ │ │ ├── run_tests.sh
│ │ │ └── sophus/
│ │ │ ├── __init__.py
│ │ │ ├── complex.py
│ │ │ ├── cse_codegen.py
│ │ │ ├── dual_quaternion.py
│ │ │ ├── matrix.py
│ │ │ ├── quaternion.py
│ │ │ ├── se2.py
│ │ │ ├── se3.py
│ │ │ ├── so2.py
│ │ │ └── so3.py
│ │ └── test/
│ │ ├── CMakeLists.txt
│ │ ├── ceres/
│ │ │ ├── CMakeLists.txt
│ │ │ ├── test_ceres_rxso2.cpp
│ │ │ ├── test_ceres_rxso3.cpp
│ │ │ ├── test_ceres_se2.cpp
│ │ │ ├── test_ceres_se3.cpp
│ │ │ ├── test_ceres_sim2.cpp
│ │ │ ├── test_ceres_sim3.cpp
│ │ │ ├── test_ceres_so2.cpp
│ │ │ ├── test_ceres_so3.cpp
│ │ │ └── tests.hpp
│ │ └── core/
│ │ ├── CMakeLists.txt
│ │ ├── test_cartesian2.cpp
│ │ ├── test_cartesian3.cpp
│ │ ├── test_common.cpp
│ │ ├── test_geometry.cpp
│ │ ├── test_rxso2.cpp
│ │ ├── test_rxso3.cpp
│ │ ├── test_se2.cpp
│ │ ├── test_se3.cpp
│ │ ├── test_sim2.cpp
│ │ ├── test_sim3.cpp
│ │ ├── test_so2.cpp
│ │ ├── test_so3.cpp
│ │ ├── test_velocities.cpp
│ │ └── tests.hpp
│ └── tessil-src/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── README.md
│ ├── appveyor.yml
│ ├── cmake/
│ │ └── tsl-robin-mapConfig.cmake.in
│ ├── doxygen.conf
│ ├── include/
│ │ └── tsl/
│ │ ├── robin_growth_policy.h
│ │ ├── robin_hash.h
│ │ ├── robin_map.h
│ │ └── robin_set.h
│ ├── tests/
│ │ ├── CMakeLists.txt
│ │ ├── custom_allocator_tests.cpp
│ │ ├── main.cpp
│ │ ├── policy_tests.cpp
│ │ ├── robin_map_tests.cpp
│ │ ├── robin_set_tests.cpp
│ │ └── utils.h
│ └── tsl-robin-map.natvis
├── config/
│ ├── avia.yaml
│ ├── horizon.yaml
│ ├── mid360.yaml
│ ├── ouster.yaml
│ ├── pandar.yaml
│ ├── robosense.yaml
│ └── velodyne.yaml
├── include/
│ ├── color.h
│ ├── common_lib.h
│ ├── ikd-Tree/
│ │ ├── README.md
│ │ ├── ikd_Tree.cpp
│ │ └── ikd_Tree.h
│ ├── matplotlibcpp.h
│ ├── scope_timer.hpp
│ └── so3_math.h
├── launch/
│ ├── hesai_pandarXT.launch
│ ├── livox_avia.launch
│ ├── livox_horizon.launch
│ ├── livox_mid360.launch
│ ├── ouster.launch
│ ├── robosense.launch
│ └── velodyne.launch
├── msg/
│ ├── Pose6D.msg
│ └── States.msg
├── package.xml
├── rviz_cfg/
│ ├── .gitignore
│ ├── fast_lo.rviz
│ └── spinning.rviz
└── src/
├── IMU_Processing.hpp
├── laserMapping.cpp
├── preprocess.cpp
└── preprocess.h
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(i2ekf_lo)
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 architecture: ${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()
add_subdirectory(Thirdparty/Sophus)
add_subdirectory(Thirdparty/tessil-src)
find_package(OpenMP QUIET)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
find_package(PythonLibs REQUIRED)
find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h")
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
std_msgs
pcl_ros
tf
livox_ros_driver
message_generation
eigen_conversions
)
find_package(Eigen3 REQUIRED)
find_package(PCL 1.8 REQUIRED)
find_package(Ceres REQUIRED)
message(Eigen: ${EIGEN3_INCLUDE_DIR})
include_directories(
../../devel/include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${PYTHON_INCLUDE_DIRS}
include)
add_message_files(
FILES
Pose6D.msg
States.msg
)
generate_messages(
DEPENDENCIES
geometry_msgs
)
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime
DEPENDS EIGEN3 PCL
INCLUDE_DIRS
)
add_executable(i2ekf_lo_mapping
src/laserMapping.cpp
include/ikd-Tree/ikd_Tree.cpp
src/preprocess.cpp)
add_dependencies(i2ekf_lo_mapping ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(i2ekf_lo_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${CERES_LIBRARIES} Sophus::Sophus tsl::robin_map)
target_include_directories(i2ekf_lo_mapping PRIVATE ${PYTHON_INCLUDE_DIRS})
================================================
FILE: LICENSE
================================================
GNU GENERAL PUBLIC LICENSE
Version 2, June 1991
Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
License is intended to guarantee your freedom to share and change free
software--to make sure the software is free for all its users. This
General Public License applies to most of the Free Software
Foundation's software and to any other program whose authors commit to
using it. (Some other Free Software Foundation software is covered by
the GNU Lesser General Public License instead.) You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
this service if you wish), that you receive source code or can get it
if you want it, that you can change the software or use pieces of it
in new free programs; and that you know you can do these things.
To protect your rights, we need to make restrictions that forbid
anyone to deny you these rights or to ask you to surrender the rights.
These restrictions translate to certain responsibilities for you if you
distribute copies of the software, or if you modify it.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must give the recipients all the rights that
you have. You must make sure that they, too, receive or can get the
source code. And you must show them these terms so they know their
rights.
We protect your rights with two steps: (1) copyright the software, and
(2) offer you this license which gives you legal permission to copy,
distribute and/or modify the software.
Also, for each author's protection and ours, we want to make certain
that everyone understands that there is no warranty for this free
software. If the software is modified by someone else and passed on, we
want its recipients to know that what they have is not the original, so
that any problems introduced by others will not reflect on the original
authors' reputations.
Finally, any free program is threatened constantly by software
patents. We wish to avoid the danger that redistributors of a free
program will individually obtain patent licenses, in effect making the
program proprietary. To prevent this, we have made it clear that any
patent must be licensed for everyone's free use or not licensed at all.
The precise terms and conditions for copying, distribution and
modification follow.
GNU GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License applies to any program or other work which contains
a notice placed by the copyright holder saying it may be distributed
under the terms of this General Public License. The "Program", below,
refers to any such program or work, and a "work based on the Program"
means either the Program or any derivative work under copyright law:
that is to say, a work containing the Program or a portion of it,
either verbatim or with modifications and/or translated into another
language. (Hereinafter, translation is included without limitation in
the term "modification".) Each licensee is addressed as "you".
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running the Program is not restricted, and the output from the Program
is covered only if its contents constitute a work based on the
Program (independent of having been made by running the Program).
Whether that is true depends on what the Program does.
1. You may copy and distribute verbatim copies of the Program's
source code as you receive it, in any medium, provided that you
conspicuously and appropriately publish on each copy an appropriate
copyright notice and disclaimer of warranty; keep intact all the
notices that refer to this License and to the absence of any warranty;
and give any other recipients of the Program a copy of this License
along with the Program.
You may charge a fee for the physical act of transferring a copy, and
you may at your option offer warranty protection in exchange for a fee.
2. You may modify your copy or copies of the Program or any portion
of it, thus forming a work based on the Program, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) You must cause the modified files to carry prominent notices
stating that you changed the files and the date of any change.
b) You must cause any work that you distribute or publish, that in
whole or in part contains or is derived from the Program or any
part thereof, to be licensed as a whole at no charge to all third
parties under the terms of this License.
c) If the modified program normally reads commands interactively
when run, you must cause it, when started running for such
interactive use in the most ordinary way, to print or display an
announcement including an appropriate copyright notice and a
notice that there is no warranty (or else, saying that you provide
a warranty) and that users may redistribute the program under
these conditions, and telling the user how to view a copy of this
License. (Exception: if the Program itself is interactive but
does not normally print such an announcement, your work based on
the Program is not required to print an announcement.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Program,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Program, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Program.
In addition, mere aggregation of another work not based on the Program
with the Program (or with a work based on the Program) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may copy and distribute the Program (or a work based on it,
under Section 2) in object code or executable form under the terms of
Sections 1 and 2 above provided that you also do one of the following:
a) Accompany it with the complete corresponding machine-readable
source code, which must be distributed under the terms of Sections
1 and 2 above on a medium customarily used for software interchange; or,
b) Accompany it with a written offer, valid for at least three
years, to give any third party, for a charge no more than your
cost of physically performing source distribution, a complete
machine-readable copy of the corresponding source code, to be
distributed under the terms of Sections 1 and 2 above on a medium
customarily used for software interchange; or,
c) Accompany it with the information you received as to the offer
to distribute corresponding source code. (This alternative is
allowed only for noncommercial distribution and only if you
received the program in object code or executable form with such
an offer, in accord with Subsection b above.)
The source code for a work means the preferred form of the work for
making modifications to it. For an executable work, complete source
code means all the source code for all modules it contains, plus any
associated interface definition files, plus the scripts used to
control compilation and installation of the executable. However, as a
special exception, the source code distributed need not include
anything that is normally distributed (in either source or binary
form) with the major components (compiler, kernel, and so on) of the
operating system on which the executable runs, unless that component
itself accompanies the executable.
If distribution of executable or object code is made by offering
access to copy from a designated place, then offering equivalent
access to copy the source code from the same place counts as
distribution of the source code, even though third parties are not
compelled to copy the source along with the object code.
4. You may not copy, modify, sublicense, or distribute the Program
except as expressly provided under this License. Any attempt
otherwise to copy, modify, sublicense or distribute the Program is
void, and will automatically terminate your rights under this License.
However, parties who have received copies, or rights, from you under
this License will not have their licenses terminated so long as such
parties remain in full compliance.
5. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Program or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Program (or any work based on the
Program), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Program or works based on it.
6. Each time you redistribute the Program (or any work based on the
Program), the recipient automatically receives a license from the
original licensor to copy, distribute or modify the Program subject to
these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties to
this License.
7. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Program at all. For example, if a patent
license would not permit royalty-free redistribution of the Program by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Program.
If any portion of this section is held invalid or unenforceable under
any particular circumstance, the balance of the section is intended to
apply and the section as a whole is intended to apply in other
circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system, which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
8. If the distribution and/or use of the Program is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Program under this License
may add an explicit geographical distribution limitation excluding
those countries, so that distribution is permitted only in or among
countries not thus excluded. In such case, this License incorporates
the limitation as if written in the body of this License.
9. The Free Software Foundation may publish revised and/or new versions
of the General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the Program
specifies a version number of this License which applies to it and "any
later version", you have the option of following the terms and conditions
either of that version or of any later version published by the Free
Software Foundation. If the Program does not specify a version number of
this License, you may choose any version ever published by the Free Software
Foundation.
10. If you wish to incorporate parts of the Program into other free
programs whose distribution conditions are different, write to the author
to ask for permission. For software which is copyrighted by the Free
Software Foundation, write to the Free Software Foundation; we sometimes
make exceptions for this. Our decision will be guided by the two goals
of preserving the free status of all derivatives of our free software and
of promoting the sharing and reuse of software generally.
NO WARRANTY
11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
REPAIR OR CORRECTION.
12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
Also add information on how to contact you by electronic and paper mail.
If the program is interactive, make it output a short notice like this
when it starts in an interactive mode:
Gnomovision version 69, Copyright (C) year name of author
Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, the commands you use may
be called something other than `show w' and `show c'; they could even be
mouse-clicks or menu items--whatever suits your program.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the program, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the program
`Gnomovision' (which makes passes at compilers) written by James Hacker.
<signature of Ty Coon>, 1 April 1989
Ty Coon, President of Vice
This General Public License does not permit incorporating your program into
proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License.
================================================
FILE: README.md
================================================
<div align="center">
<h1>I2EKF-LO</h1>
<i>A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry</i>
<br>
<strong>IROS 2024 Oral</strong>
<br>
<img src="image/overview.png" width="80%" height="auto" alt="Pipeline Image">
<br>
</div>
### Introduction
LiDAR odometry is a pivotal technology in the fields of autonomous driving and autonomous mobile robotics. However, most of the current works focuse on nonlinear optimization methods, and there are still many challenges in using the traditional Iterative Extended Kalman Filter (IEKF) framework to tackle the problem: IEKF only iterates over the observation equation, relying on a rough estimate of the initial state, which is insufficient to fully eliminate motion distortion in the input point cloud; the system process noise is difficult to be determined during state estimation of the complex motions; and the varying motion models across different sensor carriers. To address these issues, we propose the Dual-Iteration Extended Kalman Filter (I2EKF) and the LiDAR odometry based on I2EKF (I2EKF-LO). This approach not only iterates over the observation equation but also leverages state updates to iteratively mitigate motion distortion in LiDAR point clouds. Moreover, it dynamically adjusts process noise based on the confidence level of prior predictions during state estimation and establishes motion models for different sensor carriers to achieve accurate and efficient state estimation. Comprehensive experiments demonstrate that I2EKF-LO achieves outstanding levels of accuracy and computational efficiency in the realm of LiDAR odometry.
**Developers**: The codes of this repo are contributed by [Wenlu Yu (于文录)](https://github.com/YWL0720), [Jie Xu (徐杰)](https://github.com/jiejie567), [Chengwei Zhao (赵成伟)](https://github.com/chengwei0427)
### News
* **[30/06/2024]**: I2EKF-LO is accepted to IROS 2024.
* **[01/07/2024]**: We are currently working on organizing and refining the complete code. The full version will be released soon.
* **[02/07/2024]**: Updated the video link and submitted the paper to arxiv.
### Related Paper
Related paper available on arxiv: [I2EKF-LO: A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry](https://arxiv.org/abs/2407.02190)
### Related Video:
<div align="center">
<a href="https://youtu.be/jaNavKRnpiE" target="_blank"/>
<img src="image/video_cover.png" width=70% />
</div>
## 1. Prerequisites
### 1.1 **Ubuntu** and **ROS**
Ubuntu >= 18.04.
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).
## 2. Build
Clone the repository and catkin_make:
```
cd ~/catkin_ws/src
git clone https://github.com/YWL0720/I2EKF-LO
cd ..
catkin_make -j
source devel/setup.bash
```
## 3. Directly run
```bash
cd ~/$I2EKF_LO_ROS_DIR$
source devel/setup.bash
roslaunch i2ekf_lo xxx.launch
```
## 4. Rosbag Example
Download our test bags here: [HIT-TIB Datasets](https://drive.google.com/drive/folders/1L5cX9uyAiZei17oq7ELyd8WyIRReHq8u?usp=drive_link).
## 5. Acknowledgments
Thanks for [Fast-LIO2](https://github.com/hku-mars/FAST_LIO) (Fast Direct LiDAR-inertial Odometry) and [LI-INIT](https://github.com/hku-mars/LiDAR_IMU_Init)(Robust Real-time LiDAR-inertial Initialization).
## 6. License
The source code is released under [GPLv2](http://www.gnu.org/licenses/) license.
## 7. TODO(s)
- [ ] Updated video link
- [ ] Upload a preprint of our paper
================================================
FILE: Thirdparty/Sophus/.clang-format
================================================
Language: Cpp
# BasedOnStyle: Google
AccessModifierOffset: -1
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlinesLeft: true
AlignOperands: true
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: false
AfterControlStatement: false
AfterEnum: false
AfterFunction: false
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: false
AfterUnion: false
BeforeCatch: false
BeforeElse: false
IndentBraces: false
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Attach
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
ColumnLimit: 80
CommentPragmas: '^ IWYU pragma:'
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ]
IncludeCategories:
- Regex: '^<.*\.h>'
Priority: 1
- Regex: '^<.*'
Priority: 2
- Regex: '.*'
Priority: 3
IndentCaseLabels: true
IndentWidth: 2
IndentWrappedFunctionNames: false
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: false
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
ReflowComments: true
SortIncludes: true
SpaceAfterCStyleCast: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Auto
TabWidth: 8
UseTab: Never
================================================
FILE: Thirdparty/Sophus/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.4)
project(Sophus VERSION 1.22.10)
include(CMakePackageConfigHelpers)
include(GNUInstallDirs)
# Determine if sophus is built as a subproject (using add_subdirectory)
# or if it is the master project.
if (NOT DEFINED SOPHUS_MASTER_PROJECT)
set(SOPHUS_MASTER_PROJECT OFF)
if (CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR)
set(SOPHUS_MASTER_PROJECT ON)
message(STATUS "CMake version: ${CMAKE_VERSION}")
endif ()
endif ()
option(SOPHUS_INSTALL "Generate the install target." ${SOPHUS_MASTER_PROJECT})
option(SOPHUS_USE_BASIC_LOGGING "Use basic logging (in ensure and test macros)" OFF)
if(SOPHUS_MASTER_PROJECT)
# Release by default
# Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug"
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
set(CMAKE_CXX_STANDARD 14)
# Set compiler specific settings (FixMe: Should not cmake do this for us automatically?)
IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics")
ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3")
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -Wextra -std=c++14 -Wno-deprecated-declarations -ftemplate-backtrace-limit=0")
SET(CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_DEBUG} --coverage -fno-inline -fno-inline-small-functions -fno-default-inline")
SET(CMAKE_EXE_LINKER_FLAGS_COVERAGE "${CMAKE_EXE_LINKER_FLAGS_DEBUG} --coverage")
SET(CMAKE_SHARED_LINKER_FLAGS_COVERAGE "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} --coverage")
ELSEIF(CMAKE_CXX_COMPILER_ID MATCHES "^MSVC$")
ADD_DEFINITIONS("-D _USE_MATH_DEFINES /bigobj /wd4305 /wd4244 /MP")
ENDIF()
# Add local path for finding packages, set the local version first
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules")
endif()
if(SOPHUS_USE_BASIC_LOGGING)
set (CMAKE_DISABLE_FIND_PACKAGE_fmt ON)
endif()
# Find public dependencies if targets are not yet defined. (Targets might be for example
# defined by a parent project including Sophus via `add_subdirectory`.)
if(NOT TARGET Eigen3::Eigen)
find_package(Eigen3 3.3.0 REQUIRED)
endif()
if(NOT TARGET fmt::fmt)
find_package(fmt)
endif()
# Define interface library target
add_library(sophus INTERFACE)
add_library (Sophus::Sophus ALIAS sophus)
set(SOPHUS_HEADER_FILES
sophus/average.hpp
sophus/cartesian.hpp
sophus/ceres_local_parameterization.hpp
sophus/ceres_manifold.hpp
sophus/ceres_typetraits.hpp
sophus/common.hpp
sophus/geometry.hpp
sophus/interpolate.hpp
sophus/interpolate_details.hpp
sophus/num_diff.hpp
sophus/rotation_matrix.hpp
sophus/rxso2.hpp
sophus/rxso3.hpp
sophus/se2.hpp
sophus/se3.hpp
sophus/sim2.hpp
sophus/sim3.hpp
sophus/sim_details.hpp
sophus/so2.hpp
sophus/so3.hpp
sophus/spline.hpp
sophus/types.hpp
sophus/velocities.hpp
)
set(SOPHUS_OTHER_FILES
sophus/test_macros.hpp
sophus/example_ensure_handler.cpp
)
if(MSVC)
# Define common math constants if we compile with MSVC
target_compile_definitions (sophus INTERFACE _USE_MATH_DEFINES)
endif (MSVC)
# Add Eigen interface dependency, depending on available cmake info
if(TARGET Eigen3::Eigen)
target_link_libraries(sophus INTERFACE Eigen3::Eigen)
set(Eigen3_DEPENDENCY "find_dependency (Eigen3 ${Eigen3_VERSION})")
else()
target_include_directories (sophus SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR})
endif()
if(SOPHUS_USE_BASIC_LOGGING OR NOT TARGET fmt::fmt)
# NOTE fmt_FOUND does not seem to be defined even though the package config
# was found.
target_compile_definitions(sophus INTERFACE SOPHUS_USE_BASIC_LOGGING=1)
message(STATUS "Turning basic logging ON")
else()
target_link_libraries(sophus INTERFACE fmt::fmt)
set(fmt_DEPENDENCY "find_dependency (fmt ${fmt_VERSION})")
message(STATUS "Turning basic logging OFF")
endif()
# Associate target with include directory
target_include_directories(sophus INTERFACE
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
"$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>"
)
# Declare all used C++14 features
target_compile_features (sophus INTERFACE
cxx_auto_type
cxx_decltype
cxx_nullptr
cxx_right_angle_brackets
cxx_variadic_macros
cxx_variadic_templates
)
# Add sources as custom target so that they are shown in IDE's
add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES} ${SOPHUS_HEADER_FILES})
# Create 'test' make target using ctest
option(BUILD_SOPHUS_TESTS "Build tests." ON)
if(BUILD_SOPHUS_TESTS)
enable_testing()
add_subdirectory(test)
endif()
# Create examples make targets using ctest
option(BUILD_SOPHUS_EXAMPLES "Build examples." ON)
if(BUILD_SOPHUS_EXAMPLES)
add_subdirectory(examples)
endif()
# Build python sophus bindings
option(BUILD_PYTHON_BINDINGS "Build python sophus bindings." OFF)
if(BUILD_PYTHON_BINDINGS)
include(FetchContent)
FetchContent_Declare(
pybind11
GIT_REPOSITORY https://github.com/pybind/pybind11.git
GIT_TAG master
)
FetchContent_MakeAvailable(pybind11)
add_subdirectory(${pybind11_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR}/pybind)
pybind11_add_module(sophus_pybind ${CMAKE_CURRENT_SOURCE_DIR}/sophus_pybind/bindings.cpp)
target_link_libraries(sophus_pybind PUBLIC sophus)
endif(BUILD_PYTHON_BINDINGS)
if(SOPHUS_INSTALL)
# Export package for use from the build tree
set(SOPHUS_CMAKE_EXPORT_DIR ${CMAKE_INSTALL_DATADIR}/sophus/cmake)
set_target_properties(sophus PROPERTIES EXPORT_NAME Sophus)
install(TARGETS sophus EXPORT SophusTargets)
install(EXPORT SophusTargets
NAMESPACE Sophus::
DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}
)
export(TARGETS sophus NAMESPACE Sophus:: FILE SophusTargets.cmake)
export(PACKAGE Sophus)
configure_package_config_file(
SophusConfig.cmake.in
${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake
INSTALL_DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}
NO_CHECK_REQUIRED_COMPONENTS_MACRO
)
# Remove architecture dependence. Sophus is a header-only library.
set(TEMP_SIZEOF_VOID_P ${CMAKE_SIZEOF_VOID_P})
unset(CMAKE_SIZEOF_VOID_P)
# Write version to file
write_basic_package_version_file (
SophusConfigVersion.cmake
VERSION ${PROJECT_VERSION}
COMPATIBILITY SameMajorVersion
)
# Recover architecture dependence
set(CMAKE_SIZEOF_VOID_P ${TEMP_SIZEOF_VOID_P})
# Install cmake targets
install(
FILES ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake
${CMAKE_CURRENT_BINARY_DIR}/SophusConfigVersion.cmake
DESTINATION ${SOPHUS_CMAKE_EXPORT_DIR}
)
# Install header files
install(
FILES ${SOPHUS_HEADER_FILES}
DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sophus
)
endif()
================================================
FILE: Thirdparty/Sophus/LICENSE.txt
================================================
Copyright 2011-2017 Hauke Strasdat
2012-2017 Steven Lovegrove
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to
deal in the Software without restriction, including without limitation the
rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
sell copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
IN THE SOFTWARE.
================================================
FILE: Thirdparty/Sophus/README.rst
================================================
|GithubCICpp|_ windows: |AppVeyor|_ |GithubCISympy|_ |ci_cov|_
Sophus
======
Overview
--------
This is a c++ implementation of Lie groups commonly used for 2d and 3d
geometric problems (i.e. for Computer Vision or Robotics applications).
Among others, this package includes the special orthogonal groups SO(2) and
SO(3) to present rotations in 2d and 3d as well as the special Euclidean group
SE(2) and SE(3) to represent rigid body transformations (i.e. rotations and
translations) in 2d and 3d.
API documentation: https://strasdat.github.io/Sophus/
Cross platform support
----------------------
Sophus compiles with clang and gcc on Linux and OS X as well as msvc on Windows.
The specific compiler and operating system versions which are supported are
the ones which are used in the Continuous Integration (CI): See GitHubCI_ and
AppVeyor_ for details.
However, it should work (with no to minor modification) on many other
modern configurations as long they support c++14, CMake, Eigen 3.3.X and
(optionally) fmt. The fmt dependency can be eliminated by passing
"-DUSE_BASIC_LOGGING=ON" to cmake when configuring Sophus.
.. _GitHubCI: https://github.com/strasdat/Sophus/actions
.. |AppVeyor| image:: https://ci.appveyor.com/api/projects/status/um4285lwhs8ci7pt/branch/master?svg=true
.. _AppVeyor: https://ci.appveyor.com/project/strasdat/sophus/branch/master
.. |ci_cov| image:: https://coveralls.io/repos/github/strasdat/Sophus/badge.svg?branch=master
.. _ci_cov: https://coveralls.io/github/strasdat/Sophus?branch=master
.. |GithubCICpp| image:: https://github.com/strasdat/Sophus/actions/workflows/main.yml/badge.svg?branch=master
.. _GithubCICpp: https://github.com/strasdat/Sophus/actions/workflows/main.yml?query=branch%3Amaster
.. |GithubCISympy| image:: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml/badge.svg?branch=master
.. _GithubCISympy: https://github.com/strasdat/Sophus/actions/workflows/sympy.yml?query=branch%3Amaster
================================================
FILE: Thirdparty/Sophus/Sophus.code-workspace
================================================
{
"folders": [
{
"path": "."
}
],
"settings": {}
}
================================================
FILE: Thirdparty/Sophus/SophusConfig.cmake.in
================================================
@PACKAGE_INIT@
include (CMakeFindDependencyMacro)
@Eigen3_DEPENDENCY@
@fmt_DEPENDENCY@
include ("${CMAKE_CURRENT_LIST_DIR}/SophusTargets.cmake")
================================================
FILE: Thirdparty/Sophus/appveyor.yml
================================================
branches:
only:
- master
os: Visual Studio 2015
clone_folder: c:\projects\sophus
platform: x64
configuration: Debug
build:
project: c:\projects\sophus\build\Sophus.sln
install:
- ps: wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.zip -outfile eigen3.zip
- cmd: 7z x eigen3.zip -o"C:\projects" -y > nul
- git clone https://github.com/fmtlib/fmt.git
- cd fmt
- git checkout 5.3.0
- mkdir build
- cd build
- cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug ..
- cmake --build .
- cmake --build . --target install
- cd ../..
before_build:
- cd c:\projects\sophus
- mkdir build
- cd build
- cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=Debug -D EIGEN3_INCLUDE_DIR=C:\projects\eigen-3.3.4 ..
after_build:
- ctest --output-on-failure
================================================
FILE: Thirdparty/Sophus/cmake_modules/FindEigen3.cmake
================================================
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
#
# and the following imported target:
#
# Eigen3::Eigen - The header-only Eigen library
#
# This module reads hints about search locations from
# the following environment variables:
#
# EIGEN3_ROOT
# EIGEN3_ROOT_DIR
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif()
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif()
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif()
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif()
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else()
set(EIGEN3_VERSION_OK TRUE)
endif()
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif()
endmacro()
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
set(Eigen3_FOUND ${EIGEN3_VERSION_OK})
else ()
# search first if an Eigen3Config.cmake is available in the system,
# if successful this would set EIGEN3_INCLUDE_DIR and the rest of
# the script will work as usual
find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET)
if(NOT EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
HINTS
ENV EIGEN3_ROOT
ENV EIGEN3_ROOT_DIR
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
endif()
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif()
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif()
if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen)
add_library(Eigen3::Eigen INTERFACE IMPORTED)
set_target_properties(Eigen3::Eigen PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}")
endif()
================================================
FILE: Thirdparty/Sophus/doxyfile
================================================
DOXYFILE_ENCODING = UTF-8
PROJECT_NAME = "Sophus"
INPUT = sophus
EXTRACT_ALL = YES
ENABLE_PREPROCESSING = YES
MACRO_EXPANSION = YES
WARN_AS_ERROR = YES
EXPAND_ONLY_PREDEF = NO
SKIP_FUNCTION_MACROS = NO
AUTOLINK_SUPPORT = YES
MULTILINE_CPP_IS_BRIEF = YES
MARKDOWN_SUPPORT = YES
INLINE_INHERITED_MEMB = NO
EXCLUDE_SYMBOLS = Eigen::internal Sophus::details Sophus::interp_details Sophus::experimental
GENERATE_LATEX = NO
STRIP_CODE_COMMENTS = NO
GENERATE_XML = YES
GENERATE_HTML = NO
XML_OUTPUT = xml-dir
XML_PROGRAMLISTING = NO
CASE_SENSE_NAMES = NO
HIDE_UNDOC_RELATIONS = YES
EXTRACT_ALL = YES
================================================
FILE: Thirdparty/Sophus/doxyrest-config.lua
================================================
FRAME_DIR_LIST = { "doxyrest_b/doxyrest/frame/cfamily", "doxyrest_b/doxyrest/frame/common" }
FRAME_FILE = "index.rst.in"
INPUT_FILE = "xml-dir/index.xml"
OUTPUT_FILE = "rst-dir/index.rst"
INTRO_FILE = "page_index.rst"
SORT_GROUPS_BY = "title"
GLOBAL_AUX_COMPOUND_ID = "group_global"
LANGUAGE = cpp
VERBATIM_TO_CODE_BLOCK = "cpp"
ESCAPE_ASTERISKS = true
ESCAPE_PIPES = true
ESCAPE_TRAILING_UNDERSCORES = true
PROTECTION_FILTER = "protected"
EXCLUDE_EMPTY_DEFINES = true
EXCLUDE_DEFAULT_CONSTRUCTORS = false
EXCLUDE_DESTRUCTORS = false
EXCLUDE_PRIMITIVE_TYPEDEFS = true
SHOW_DIRECT_DESCENDANTS = true
TYPEDEF_TO_USING = true
ML_PARAM_LIST_LENGTH_THRESHOLD = 80
================================================
FILE: Thirdparty/Sophus/examples/CMakeLists.txt
================================================
# Tests to run
SET( EXAMPLE_SOURCES HelloSO3)
FOREACH(example_src ${EXAMPLE_SOURCES})
ADD_EXECUTABLE( ${example_src} ${example_src}.cpp)
TARGET_LINK_LIBRARIES( ${example_src} sophus)
ENDFOREACH(example_src)
================================================
FILE: Thirdparty/Sophus/examples/HelloSO3.cpp
================================================
#include <iostream>
#include "sophus/geometry.hpp"
int main() {
// The following demonstrates the group multiplication of rotation matrices
// Create rotation matrices from rotations around the x and y and z axes:
const double kPi = Sophus::Constants<double>::pi();
Sophus::SO3d R1 = Sophus::SO3d::rotX(kPi / 4);
Sophus::SO3d R2 = Sophus::SO3d::rotY(kPi / 6);
Sophus::SO3d R3 = Sophus::SO3d::rotZ(-kPi / 3);
std::cout << "The rotation matrices are" << std::endl;
std::cout << "R1:\n" << R1.matrix() << std::endl;
std::cout << "R2:\n" << R2.matrix() << std::endl;
std::cout << "R3:\n" << R3.matrix() << std::endl;
std::cout << "Their product R1*R2*R3:\n"
<< (R1 * R2 * R3).matrix() << std::endl;
std::cout << std::endl;
// Rotation matrices can act on vectors
Eigen::Vector3d x;
x << 0.0, 0.0, 1.0;
std::cout << "Rotation matrices can act on vectors" << std::endl;
std::cout << "x\n" << x << std::endl;
std::cout << "R2*x\n" << R2 * x << std::endl;
std::cout << "R1*(R2*x)\n" << R1 * (R2 * x) << std::endl;
std::cout << "(R1*R2)*x\n" << (R1 * R2) * x << std::endl;
std::cout << std::endl;
// SO(3) are internally represented as unit quaternions.
std::cout << "R1 in matrix form:\n" << R1.matrix() << std::endl;
std::cout << "R1 in unit quaternion form:\n"
<< R1.unit_quaternion().coeffs() << std::endl;
// Note that the order of coefficients of Eigen's quaternion class is
// (imag0, imag1, imag2, real)
std::cout << std::endl;
}
================================================
FILE: Thirdparty/Sophus/generate_stubs.py
================================================
import subprocess
subprocess.run(
"pybind11-stubgen sophus_pybind -o sophus_pybind-stubs/",
shell=True,
check=True,
)
subprocess.run("touch sophus_pybind-stubs/py.typed", shell=True, check=True)
================================================
FILE: Thirdparty/Sophus/make_docs.sh
================================================
doxygen doxyfile
doxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua
sphinx-build -b html rst-dir html-dir
================================================
FILE: Thirdparty/Sophus/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>sophus</name>
<version>1.22.10</version>
<description>
C++ implementation of Lie Groups using Eigen.
</description>
<url type="repository">https://github.com/strasdat/sophus</url>
<url type="bugtracker">https://github.com/strasdat/sophus/issues</url>
<maintainer email="d.stonier@gmail.com">Daniel Stonier</maintainer>
<author>Hauke Strasdat</author>
<license>MIT</license>
<buildtool_depend>cmake</buildtool_depend>
<build_depend>eigen</build_depend>
<build_export_depend>eigen</build_export_depend>
<export>
<build_type>cmake</build_type>
</export>
</package>
================================================
FILE: Thirdparty/Sophus/rst-dir/conf.py
================================================
# Configuration file for the Sphinx documentation builder.
#
# This file only contains a selection of the most common options. For a full
# list see the documentation:
# http://www.sphinx-doc.org/en/master/config
# -- Path setup --------------------------------------------------------------
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
#
import os
import sys
sys.path.insert(0, os.path.abspath('../sympy'))
sys.path.insert(1, os.path.abspath('../doxyrest_b/doxyrest/sphinx'))
extensions = ['doxyrest', 'cpplexer', 'sphinx.ext.autodoc']
# -- Project information -----------------------------------------------------
project = 'Sophus'
copyright = '2019, Hauke Strasdat'
author = 'Hauke Strasdat'
# Tell sphinx what the primary language being documented is.
primary_domain = 'cpp'
# Tell sphinx what the pygments highlight language should be.
highlight_language = 'cpp'
# Add any paths that contain templates here, relative to this directory.
templates_path = ['_templates']
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
# This pattern also affects html_static_path and html_extra_path.
exclude_patterns = []
# -- Options for HTML output -------------------------------------------------
# The theme to use for HTML and HTML Help pages. See the documentation for
# a list of builtin themes.
#
html_theme = "sphinx_rtd_theme"
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ['_static']
================================================
FILE: Thirdparty/Sophus/rst-dir/page_index.rst
================================================
Sophus - Lie groups for 2d/3d Geometry
=======================================
.. toctree::
:maxdepth: 2
:caption: Contents:
GitHub Page <https://github.com/strasdat/Sophus>
pysophus
================================================
FILE: Thirdparty/Sophus/rst-dir/pysophus.rst
================================================
Python API
==========
.. automodule:: sophus.matrix
:members:
.. automodule:: sophus.complex
:members:
.. automodule:: sophus.quaternion
:members:
.. automodule:: sophus.so2
:members:
.. automodule:: sophus.so3
:members:
.. automodule:: sophus.se2
:members:
.. automodule:: sophus.se3
:members:
================================================
FILE: Thirdparty/Sophus/run_format.sh
================================================
find . -type d \( -path ./sympy -o -path ./doxyrest_b -o -path "./*/CMakeFiles/*" \) -prune -o \( -iname "*.hpp" -o -iname "*.cpp" \) -print | xargs clang-format -i
================================================
FILE: Thirdparty/Sophus/scripts/install_docs_deps.sh
================================================
#!/bin/bash
set -x # echo on
set -e # exit on error
sudo apt-get -qq update
sudo apt-get install doxygen liblua5.3-dev ragel
pip3 install 'sphinx==2.0.1'
pip3 install sphinx_rtd_theme
pip3 install sympy
git clone https://github.com/vovkos/doxyrest_b
cd doxyrest_b
git reset --hard ad45c064d1199e71b8cae5aa66d4251c4228b958
git submodule update --init
mkdir build
cd build
cmake ..
cmake --build .
cd ../..
================================================
FILE: Thirdparty/Sophus/scripts/install_linux_deps.sh
================================================
#!/bin/bash
set -x # echo on
set -e # exit on error
cmake --version
sudo apt-get -qq update
sudo apt-get install gfortran libc++-dev libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev libceres-dev ccache
wget https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2
tar xvf eigen-3.3.4.tar.bz2
mkdir build-eigen
cd build-eigen
cmake ../eigen-3.3.4 -DEIGEN_DEFAULT_TO_ROW_MAJOR=$ROW_MAJOR_DEFAULT
sudo make install
cd ..
git clone https://ceres-solver.googlesource.com/ceres-solver ceres-solver
cd ceres-solver
git reset --hard b0aef211db734379319c19c030e734d6e23436b0
mkdir build
cd build
ccache -s
cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ..
make -j8
sudo make install
cd ../..
================================================
FILE: Thirdparty/Sophus/scripts/install_linux_fmt_deps.sh
================================================
#!/bin/bash
set -x # echo on
set -e # exit on error
git clone https://github.com/fmtlib/fmt.git
cd fmt
git checkout 5.3.0
mkdir build
cd build
cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache ..
make -j8
sudo make install
cd ../..
================================================
FILE: Thirdparty/Sophus/scripts/install_osx_deps.sh
================================================
#!/bin/bash
set -x # echo on
set -e # exit on error
brew update
brew install fmt
brew install ccache
# Build a specific version of ceres-solver instead of one shipped over brew
curl https://raw.githubusercontent.com/Homebrew/homebrew-core/b0792ccba6e71cd028263ca7621db894afc602d2/Formula/ceres-solver.rb -o ceres-solver.rb
patch <<EOF
--- ceres-solver.rb
+++ ceres-solver.rb
@@ -1,8 +1,8 @@
class CeresSolver < Formula
desc "C++ library for large-scale optimization"
homepage "http://ceres-solver.org/"
- url "http://ceres-solver.org/ceres-solver-2.0.0.tar.gz"
- sha256 "10298a1d75ca884aa0507d1abb0e0f04800a92871cd400d4c361b56a777a7603"
+ url "https://github.com/ceres-solver/ceres-solver/archive/refs/tags/2.1.0rc1.tar.gz"
+ sha256 "9138a7d80a3142fe3a98519d58a489da9e204b815cd8c0571b3e643f04eca574"
license "BSD-3-Clause"
revision 4
head "https://ceres-solver.googlesource.com/ceres-solver.git", branch: "master"
@@ -31,23 +31,16 @@
depends_on "suite-sparse"
depends_on "tbb"
- # Fix compatibility with TBB 2021.1
- # See https://github.com/ceres-solver/ceres-solver/issues/669
- # Remove in the next release
- patch do
- url "https://github.com/ceres-solver/ceres-solver/commit/941ea13475913ef8322584f7401633de9967ccc8.patch?full_index=1"
- sha256 "c61ca2ff1e92cc2134ba8e154bd9052717ba3fcae085e8f44957b9c22e6aa4ff"
- end
def install
system "cmake", ".", *std_cmake_args,
"-DBUILD_SHARED_LIBS=ON",
"-DBUILD_EXAMPLES=OFF",
- "-DLIB_SUFFIX=''"
+ "-DLIB_SUFFIX=''",
+ "-DCMAKE_CXX_COMPILER_LAUNCHER=/usr/local/bin/ccache"
system "make"
system "make", "install"
pkgshare.install "examples", "data"
- doc.install "docs/html" unless build.head?
end
EOF
HOMEBREW_NO_INSTALLED_DEPENDENTS_CHECK=1 brew install --build-from-source ./ceres-solver.rb
================================================
FILE: Thirdparty/Sophus/scripts/run_cpp_tests.sh
================================================
#!/bin/bash
set -x # echo on
set -e # exit on error
mkdir build
cd build
pwd
cmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DUSE_BASIC_LOGGING=$USE_BASIC_LOGGING -DCMAKE_BUILD_TYPE=$BUILD_TYPE ..
# Ubuntu builds via Github actions run on 2-core virtual machines
make -j2
make CTEST_OUTPUT_ON_FAILURE=1 test
================================================
FILE: Thirdparty/Sophus/setup.py
================================================
import os
import re
import shutil
import subprocess
import sys
from setuptools import Extension, find_packages, setup
from setuptools.command.build_ext import build_ext
ROOT_DIR = os.path.dirname(os.path.abspath(__file__))
# Convert distutils Windows platform specifiers to CMake -A arguments
PLAT_TO_CMAKE = {
"win32": "Win32",
"win-amd64": "x64",
"win-arm32": "ARM",
"win-arm64": "ARM64",
}
# A CMakeExtension needs a sourcedir instead of a file list.
# The name must be the _single_ output extension from the CMake build.
# If you need multiple extensions, see scikit-build.
class CMakeExtension(Extension):
def __init__(self, name, sourcedir=""):
Extension.__init__(self, name, sources=[])
self.sourcedir = os.path.abspath(sourcedir)
class CMakeBuild(build_ext):
def build_extension(self, ext):
extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name)))
# required for auto-detection & inclusion of auxiliary "native" libs
if not extdir.endswith(os.path.sep):
extdir += os.path.sep
debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug
cfg = "Debug" if debug else "Release"
# CMake lets you override the generator - we need to check this.
# Can be set with Conda-Build, for example.
cmake_generator = os.environ.get("CMAKE_GENERATOR", "")
cmake_args = [
"-DBUILD_PYTHON_BINDINGS=ON",
"-DBUILD_SOPHUS_EXAMPLES=OFF",
"-DBUILD_SOPHUS_TESTS=OFF",
]
build_args = []
# Adding CMake arguments set as environment variable
# (needed e.g. to build for ARM OSx on conda-forge)
if "CMAKE_ARGS" in os.environ:
cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item]
if "PYTHONPATH" in os.environ and "pip-build-env" in os.environ["PYTHONPATH"]:
del os.environ["PYTHONPATH"]
if self.compiler.compiler_type != "msvc":
# Using Ninja-build since it a) is available as a wheel and b)
# multithreaded automatically. MSVC would require all variables be
# exported for Ninja to pick it up, which is a little tricky to do.
# Users can override the generator with CMAKE_GENERATOR in CMake
# 3.15+.
if not cmake_generator:
try:
import ninja # noqa: F401
cmake_args += ["-GNinja"]
except ImportError:
pass
else:
# Single config generators are handled "normally"
single_config = any(x in cmake_generator for x in {"NMake", "Ninja"})
# CMake allows an arch-in-generator style for backward compatibility
contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"})
# Specify the arch if using MSVC generator, but only if it doesn't
# contain a backward-compatibility arch spec already in the
# generator name.
if not single_config and not contains_arch:
cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]]
# Multi-config generators have a different way to specify configs
if not single_config:
cmake_args += [
f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}"
]
build_args += ["--config", cfg]
if sys.platform.startswith("darwin"):
# Cross-compile support for macOS - respect ARCHFLAGS if set
archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", ""))
if archs:
cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))]
# Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level
# across all generators.
if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ:
# self.parallel is a Python 3 only way to set parallel jobs by hand
# using -j in the build_ext call, not supported by pip or PyPA-build.
if hasattr(self, "parallel") and self.parallel:
# CMake 3.12+ only.
build_args += [f"-j{self.parallel}"]
if not os.path.exists(self.build_temp):
os.makedirs(self.build_temp)
if not os.path.exists(self.build_lib):
os.makedirs(self.build_lib)
subprocess.check_call(["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp)
subprocess.check_call(
["cmake", "--build", "."] + build_args, cwd=self.build_temp
)
# copy stubs files from sophus_pybind-stubs to lib folder to be installed
subprocess.run(
f"cp sophus_pybind-stubs/*.pyi {self.build_lib}", shell=True, check=True,
)
subprocess.run(
f"cp sophus_pybind-stubs/*.typed {self.build_lib}", shell=True, check=True,
)
# copy .so file to lib
subprocess.run(f"cp {self.build_temp}/*.so {self.build_lib}/", shell=True, check=True,)
def main():
# The information here can also be placed in setup.cfg - better separation of
# logic and declaration, and simpler if you include description/version in a file.
setup(
name="sophus_pybind",
version="1.22.10",
description="Sophus python API",
long_description="Python API for sophus library",
url="https://github.com/strasdat/sophus",
ext_modules=[CMakeExtension("sophus_pybind", sourcedir=ROOT_DIR)],
author="Cheng Peng, David Caruso",
cmdclass={"build_ext": CMakeBuild},
zip_safe=False,
python_requires=">=3.6",
install_requires=["numpy"],
packages=find_packages(),
license="Apache-2.0",
)
if __name__ == "__main__":
main()
================================================
FILE: Thirdparty/Sophus/sophus/average.hpp
================================================
/// @file
/// Calculation of biinvariant means.
#pragma once
#include <complex>
#include "cartesian.hpp"
#include "common.hpp"
#include "rxso2.hpp"
#include "rxso3.hpp"
#include "se2.hpp"
#include "se3.hpp"
#include "sim2.hpp"
#include "sim3.hpp"
#include "so2.hpp"
#include "so3.hpp"
namespace Sophus {
/// Calculates mean iteratively.
///
/// Returns ``nullopt`` if it does not converge.
///
template <class SequenceContainer>
optional<typename SequenceContainer::value_type> iterativeMean(
SequenceContainer const& foo_Ts_bar, int max_num_iterations) {
size_t N = foo_Ts_bar.size();
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
using Group = typename SequenceContainer::value_type;
using Scalar = typename Group::Scalar;
using Tangent = typename Group::Tangent;
// This implements the algorithm in the beginning of Sec. 4.2 in
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
Group foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
for (int i = 0; i < max_num_iterations; ++i) {
Tangent average;
setToZero<Tangent>(average);
for (Group const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
Group foo_T_newaverage = foo_T_average * Group::exp(average);
if (squaredNorm<Tangent>(
(foo_T_newaverage.inverse() * foo_T_average).log()) <
Constants<Scalar>::epsilon()) {
return foo_T_newaverage;
}
foo_T_average = foo_T_newaverage;
}
// LCOV_EXCL_START
return nullopt;
// LCOV_EXCL_STOP
}
#ifdef DOXYGEN_SHOULD_SKIP_THIS
/// Mean implementation for any Lie group.
template <class SequenceContainer, class Scalar>
optional<typename SequenceContainer::value_type> average(
SequenceContainer const& foo_Ts_bar);
#else
// Mean implementation for Cartesian.
template <class SequenceContainer, int Dim = SequenceContainer::value_type::DoF,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<std::is_same<typename SequenceContainer::value_type,
Cartesian<Scalar, Dim> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
Sophus::Vector<Scalar, Dim> average;
average.setZero();
for (Cartesian<Scalar, Dim> const& foo_T_bar : foo_Ts_bar) {
average += foo_T_bar.params();
}
return Cartesian<Scalar, Dim>(average / Scalar(N));
}
// Mean implementation for SO(2).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SO2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
// This implements rotational part of Proposition 12 from Sec. 6.2 of
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
SO2<Scalar> foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
Scalar average(0);
for (SO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
return foo_T_average * SO2<Scalar>::exp(average);
}
// Mean implementation for RxSO(2).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, RxSO2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
RxSO2<Scalar> foo_T_average = foo_Ts_bar.front();
Scalar w = Scalar(1. / N);
Vector2<Scalar> average(Scalar(0), Scalar(0));
for (RxSO2<Scalar> const& foo_T_bar : foo_Ts_bar) {
average += w * (foo_T_average.inverse() * foo_T_bar).log();
}
return foo_T_average * RxSO2<Scalar>::exp(average);
}
namespace details {
template <class T>
void getQuaternion(T const&);
template <class Scalar>
Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
return R.unit_quaternion();
}
template <class Scalar>
Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
return sR.so3().unit_quaternion();
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
Eigen::Quaternion<Scalar> averageUnitQuaternion(
SequenceContainer const& foo_Ts_bar) {
// This: http://stackoverflow.com/a/27410865/1221742
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
Eigen::Matrix<Scalar, 4, Eigen::Dynamic> Q(4, N);
int i = 0;
Scalar w = Scalar(1. / N);
for (auto const& foo_T_bar : foo_Ts_bar) {
Q.col(i) = w * details::getUnitQuaternion(foo_T_bar).coeffs();
++i;
}
Eigen::Matrix<Scalar, 4, 4> QQt = Q * Q.transpose();
// TODO: Figure out why we can't use SelfAdjointEigenSolver here.
Eigen::EigenSolver<Eigen::Matrix<Scalar, 4, 4> > es(QQt);
std::complex<Scalar> max_eigenvalue = es.eigenvalues()[0];
Eigen::Matrix<std::complex<Scalar>, 4, 1> max_eigenvector =
es.eigenvectors().col(0);
for (int i = 1; i < 4; i++) {
if (std::norm(es.eigenvalues()[i]) > std::norm(max_eigenvalue)) {
max_eigenvalue = es.eigenvalues()[i];
max_eigenvector = es.eigenvectors().col(i);
}
}
Eigen::Quaternion<Scalar> quat;
quat.coeffs() << //
max_eigenvector[0].real(), //
max_eigenvector[1].real(), //
max_eigenvector[2].real(), //
max_eigenvector[3].real();
return quat;
}
} // namespace details
// Mean implementation for SO(3).
//
// TODO: Detect degenerated cases and return nullopt.
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SO3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
return SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar));
}
// Mean implementation for R x SO(3).
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, RxSO3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar) {
size_t N = std::distance(std::begin(foo_Ts_bar), std::end(foo_Ts_bar));
SOPHUS_ENSURE(N >= 1, "N must be >= 1.");
Scalar scale_sum = Scalar(0);
using std::exp;
using std::log;
for (RxSO3<Scalar> const& foo_T_bar : foo_Ts_bar) {
scale_sum += log(foo_T_bar.scale());
}
return RxSO3<Scalar>(exp(scale_sum / Scalar(N)),
SO3<Scalar>(details::averageUnitQuaternion(foo_Ts_bar)));
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SE2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
// TODO: Implement Proposition 12 from Sec. 6.2 of
// ftp://ftp-sop.inria.fr/epidaure/Publications/Arsigny/arsigny_rr_biinvariant_average.pdf.
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, Sim2<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, SE3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
template <class SequenceContainer,
class Scalar = typename SequenceContainer::value_type::Scalar>
enable_if_t<
std::is_same<typename SequenceContainer::value_type, Sim3<Scalar> >::value,
optional<typename SequenceContainer::value_type> >
average(SequenceContainer const& foo_Ts_bar, int max_num_iterations = 20) {
return iterativeMean(foo_Ts_bar, max_num_iterations);
}
#endif // DOXYGEN_SHOULD_SKIP_THIS
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/cartesian.hpp
================================================
/// @file
/// Cartesian - Euclidean vector space as Lie group
#pragma once
#include <sophus/types.hpp>
namespace Sophus {
template <class Scalar_, int M, int Options = 0>
class Cartesian;
template <class Scalar_>
using Cartesian2 = Cartesian<Scalar_, 2>;
template <class Scalar_>
using Cartesian3 = Cartesian<Scalar_, 3>;
using Cartesian2d = Cartesian2<double>;
using Cartesian3d = Cartesian3<double>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int M, int Options>
struct traits<Sophus::Cartesian<Scalar_, M, Options>> {
using Scalar = Scalar_;
using ParamsType = Sophus::Vector<Scalar, M, Options>;
};
template <class Scalar_, int M, int Options>
struct traits<Map<Sophus::Cartesian<Scalar_, M>, Options>>
: traits<Sophus::Cartesian<Scalar_, M, Options>> {
using Scalar = Scalar_;
using ParamsType = Map<Sophus::Vector<Scalar, M>, Options>;
};
template <class Scalar_, int M, int Options>
struct traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>>
: traits<Sophus::Cartesian<Scalar_, M, Options> const> {
using Scalar = Scalar_;
using ParamsType = Map<Sophus::Vector<Scalar, M> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// Cartesian base type - implements Cartesian class but is storage agnostic.
///
/// Euclidean vector space as Lie group.
///
/// Lie groups can be seen as a generalization over the Euclidean vector
/// space R^M. Here a N-dimensional vector ``p`` is represented as a
// (M+1) x (M+1) homogeneous matrix:
///
/// | I p |
/// | o 1 |
///
/// On the other hand, Cartesian(M) can be seen as a special case of SE(M)
/// with identity rotation, and hence represents pure translation.
///
/// The purpose of this class is two-fold:
/// - for educational purpose, to highlight how Lie groups generalize over
/// Euclidean vector spaces.
/// - to be used in templated/generic algorithms (such as Sophus::Spline)
/// which are implemented against the Lie group interface.
///
/// Obviously, Cartesian(M) can just be represented as a M-tuple.
///
/// Cartesian is not compact, but a commutative group. For vector additions it
/// holds `a+b = b+a`.
///
/// See Cartesian class for more details below.
///
template <class Derived, int M>
class CartesianBase {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using ParamsType = typename Eigen::internal::traits<Derived>::ParamsType;
/// Degrees of freedom of manifold, equals to number of Cartesian coordinates.
static int constexpr DoF = M;
/// Number of internal parameters used, also M.
static int constexpr num_parameters = M;
/// Group transformations are (M+1)x(M+1) matrices.
static int constexpr N = M + 1;
static int constexpr Dim = M;
using Transformation = Sophus::Matrix<Scalar, N, N>;
using Point = Sophus::Vector<Scalar, M>;
using HomogeneousPoint = Sophus::Vector<Scalar, N>;
using Line = ParametrizedLine<Scalar, M>;
using Hyperplane = Eigen::Hyperplane<Scalar, M>;
using Tangent = Sophus::Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with Cartesian operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using CartesianSum = Cartesian<ReturnScalar<OtherDerived>, M>;
template <typename PointDerived>
using PointProduct = Sophus::Vector<ReturnScalar<PointDerived>, M>;
template <typename HPointDerived>
using HomogeneousPointProduct =
Sophus::Vector<ReturnScalar<HPointDerived>, N>;
/// Adjoint transformation
///
/// Always identity of commutative groups.
SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC Cartesian<NewScalarType, M> cast() const {
return Cartesian<NewScalarType, M>(params().template cast<NewScalarType>());
}
/// Returns derivative of this * exp(x) wrt x at x=0.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Sophus::Matrix<Scalar, num_parameters, DoF> m;
m.setIdentity();
return m;
}
/// Returns derivative of log(this^{-1} * x) by x at x=this.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_log_this_inv_by_x_at_this()
const {
Matrix<Scalar, DoF, num_parameters> m;
m.setIdentity();
return m;
}
/// Returns group inverse.
///
/// The additive inverse.
///
SOPHUS_FUNC Cartesian<Scalar, M> inverse() const {
return Cartesian<Scalar, M>(-params());
}
/// Logarithmic map
///
/// For Euclidean vector space, just the identity. Or to be more precise
/// it just extracts the significant M-vector from the NxN matrix.
///
SOPHUS_FUNC Tangent log() const { return params(); }
/// Returns 4x4 matrix representation of the instance.
///
/// It has the following form:
///
/// | I p |
/// | o 1 |
///
SOPHUS_FUNC Transformation matrix() const {
Sophus::Matrix<Scalar, N, N> matrix;
matrix.setIdentity();
matrix.col(M).template head<M>() = params();
return matrix;
}
/// Group multiplication, are vector additions.
///
template <class OtherDerived>
SOPHUS_FUNC CartesianBase<Derived, M>& operator=(
CartesianBase<OtherDerived, M> const& other) {
params() = other.params();
return *this;
}
/// Group multiplication, are vector additions.
///
template <typename OtherDerived>
SOPHUS_FUNC CartesianSum<OtherDerived> operator*(
CartesianBase<OtherDerived, M> const& other) const {
return CartesianSum<OtherDerived>(params() + other.params());
}
/// Group action on points, again just vector addition.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, M>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return PointProduct<PointDerived>(params() + p);
}
/// Group action on homogeneous points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, N>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rp = *this * p.template head<M>();
HomogeneousPointProduct<HPointDerived> r;
r << rp, p(M);
return r;
}
/// Group action on lines.
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(), l.direction());
}
/// Group action on planes.
///
SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
return Hyperplane(p.normal(), p.offset() - params().dot(p.normal()));
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this Cartesian's Scalar
/// type.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC CartesianBase<Derived, M>& operator*=(
CartesianBase<OtherDerived, M> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Mutator of params vector.
///
SOPHUS_FUNC ParamsType& params() {
return static_cast<Derived*>(this)->params();
}
/// Accessor of params vector
///
SOPHUS_FUNC ParamsType const& params() const {
return static_cast<Derived const*>(this)->params();
}
};
/// Cartesian using default storage; derived from CartesianBase.
template <class Scalar_, int M, int Options>
class Cartesian : public CartesianBase<Cartesian<Scalar_, M, Options>, M> {
using Base = CartesianBase<Cartesian<Scalar_, M, Options>, M>;
public:
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
static int constexpr N = Base::N;
static int constexpr Dim = Base::Dim;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using ParamsMember = Sophus::Vector<Scalar, M, Options>;
using Base::operator=;
/// Define copy-assignment operator explicitly. The definition of
/// implicit copy assignment operator is deprecated in presence of a
/// user-declared copy constructor (-Wdeprecated-copy in clang >= 13).
SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes to zero vector.
///
SOPHUS_FUNC Cartesian() { params_.setZero(); }
/// Copy constructor
///
SOPHUS_FUNC Cartesian(Cartesian const& other) = default;
/// Copy-like constructor from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC Cartesian(CartesianBase<OtherDerived, M> const& other)
: params_(other.params()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Accepts either M-vector or (M+1)x(M+1) matrices.
///
template <class D>
explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase<D> const& m) {
static_assert(
std::is_same<typename Eigen::MatrixBase<D>::Scalar, Scalar>::value, "");
if (m.rows() == DoF && m.cols() == 1) {
// trick so this compiles
params_ = m.template block<M, 1>(0, 0);
} else if (m.rows() == N && m.cols() == N) {
params_ = m.template block<M, 1>(0, M);
} else {
SOPHUS_ENSURE(false, "{} {}", m.rows(), m.cols());
}
}
/// This provides unsafe read/write access to internal data.
///
SOPHUS_FUNC Scalar* data() { return params_.data(); }
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const { return params_.data(); }
/// Returns derivative of exp(x) wrt. x.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
Sophus::Matrix<Scalar, num_parameters, DoF> m;
m.setIdentity();
return m;
}
/// Returns derivative of exp(x) wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const&) {
return Dx_exp_x_at_0();
}
/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, Dim, DoF> Dx_exp_x_times_point_at_0(
Point const&) {
Sophus::Matrix<Scalar, Dim, DoF> J;
J.setIdentity();
return J;
}
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Mutator of params vector
///
SOPHUS_FUNC ParamsMember& params() { return params_; }
/// Accessor of params vector
///
SOPHUS_FUNC ParamsMember const& params() const { return params_; }
/// Returns the ith infinitesimal generators of Cartesian(M).
///
/// The infinitesimal generators for e.g. the 3-dimensional case:
///
/// ```
/// | 0 0 0 1 |
/// G_0 = | 0 0 0 0 |
/// | 0 0 0 0 |
/// | 0 0 0 0 |
///
/// | 0 0 0 0 |
/// G_1 = | 0 0 0 1 |
/// | 0 0 0 0 |
/// | 0 0 0 0 |
///
/// | 0 0 0 0 |
/// G_2 = | 0 0 0 0 |
/// | 0 0 0 1 |
/// | 0 0 0 0 |
/// ```
///
/// Precondition: ``i`` must be in [0, M-1].
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 && i <= M, "i should be in range [0,M-1].");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// Group exponential
///
/// For Euclidean vector space, just the identity. Or to be more precise
/// it just constructs the (M+1xM+1) homogeneous matrix representation
// from the M-vector.
///
SOPHUS_FUNC static Cartesian<Scalar, M> exp(Tangent const& a) {
return Cartesian<Scalar, M>(a);
}
/// hat-operator
///
/// Formally, the hat()-operator of Cartesian(M) is defined as
///
/// ``hat(.): R^M -> R^{M+1xM+1}, hat(a) = sum_i a_i * G_i``
/// (for i=0,...,M-1)
///
/// with ``G_i`` being the ith infinitesimal generator of Cartesian(M).
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation Omega;
Omega.setZero();
Omega.col(M).template head<M>() = a.template head<M>();
return Omega;
}
/// Lie bracket
///
/// Always 0 for commutative groups.
SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
return Tangent::Zero();
}
/// Draws uniform samples in the range [-1, 1] per coordinates.
///
template <class UniformRandomBitGenerator>
static Cartesian sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
Vector<Scalar, M> v;
for (int i = 0; i < M; ++i) {
v[i] = uniform(generator);
}
return Cartesian(v);
}
/// vee-operator
///
/// This is the inverse of the hat()-operator, see above.
///
SOPHUS_FUNC static Tangent vee(Transformation const& m) {
return m.col(M).template head<M>();
}
protected:
ParamsMember params_;
};
} // namespace Sophus
namespace Eigen {
/// Specialization of Eigen::Map for ``Cartesian``; derived from
/// CartesianBase.
///
/// Allows us to wrap Cartesian objects around POD array.
template <class Scalar_, int M, int Options>
class Map<Sophus::Cartesian<Scalar_, M>, Options>
: public Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>,
M> {
public:
using Base =
Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>, M>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Base::operator=;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {}
/// Mutator of params vector
///
SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>>& params() {
return params_;
}
/// Accessor of params vector
///
SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>> const& params() const {
return params_;
}
protected:
Map<Sophus::Vector<Scalar, M>, Options> params_;
};
/// Specialization of Eigen::Map for ``Cartesian const``; derived from
/// CartesianBase.
///
/// Allows us to wrap Cartesian objects around POD array.
template <class Scalar_, int M, int Options>
class Map<Sophus::Cartesian<Scalar_, M> const, Options>
: public Sophus::CartesianBase<
Map<Sophus::Cartesian<Scalar_, M> const, Options>, M> {
public:
using Base =
Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M> const, Options>,
M>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Base::operator*;
SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {}
/// Accessor of params vector
///
SOPHUS_FUNC Map<Sophus::Vector<Scalar, M> const, Options> const& params()
const {
return params_;
}
protected:
Map<Sophus::Vector<Scalar, M> const, Options> const params_;
};
} // namespace Eigen
================================================
FILE: Thirdparty/Sophus/sophus/ceres_local_parameterization.hpp
================================================
#pragma once
#include <ceres/local_parameterization.h>
#include <sophus/ceres_typetraits.hpp>
namespace Sophus {
/// Templated local parameterization for LieGroup [with implemented
/// LieGroup::Dx_this_mul_exp_x_at_0() ]
template <template <typename, int = 0> class LieGroup>
class LocalParameterization : public ceres::LocalParameterization {
public:
using LieGroupd = LieGroup<double>;
using Tangent = typename LieGroupd::Tangent;
using TangentMap = typename Sophus::Mapper<Tangent>::ConstMap;
static int constexpr DoF = LieGroupd::DoF;
static int constexpr num_parameters = LieGroupd::num_parameters;
/// LieGroup plus operation for Ceres
///
/// T * exp(x)
///
bool Plus(double const* T_raw, double const* delta_raw,
double* T_plus_delta_raw) const override {
Eigen::Map<LieGroupd const> const T(T_raw);
TangentMap delta = Sophus::Mapper<Tangent>::map(delta_raw);
Eigen::Map<LieGroupd> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * LieGroupd::exp(delta);
return true;
}
/// Jacobian of LieGroup plus operation for Ceres
///
/// Dx T * exp(x) with x=0
///
bool ComputeJacobian(double const* T_raw,
double* jacobian_raw) const override {
Eigen::Map<LieGroupd const> T(T_raw);
Eigen::Map<Eigen::Matrix<double, num_parameters, DoF,
DoF == 1 ? Eigen::ColMajor : Eigen::RowMajor>>
jacobian(jacobian_raw);
jacobian = T.Dx_this_mul_exp_x_at_0();
return true;
}
int GlobalSize() const override { return LieGroupd::num_parameters; }
int LocalSize() const override { return LieGroupd::DoF; }
};
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/ceres_manifold.hpp
================================================
#pragma once
#include <ceres/manifold.h>
#include <sophus/ceres_typetraits.hpp>
namespace Sophus {
/// Templated local parameterization for LieGroup [with implemented
/// LieGroup::Dx_this_mul_exp_x_at_0() ]
template <template <typename, int = 0> class LieGroup>
class Manifold : public ceres::Manifold {
public:
using LieGroupd = LieGroup<double>;
using Tangent = typename LieGroupd::Tangent;
using TangentMap = typename Sophus::Mapper<Tangent>::Map;
using TangentConstMap = typename Sophus::Mapper<Tangent>::ConstMap;
static int constexpr DoF = LieGroupd::DoF;
static int constexpr num_parameters = LieGroupd::num_parameters;
/// LieGroup plus operation for Ceres
///
/// T * exp(x)
///
bool Plus(double const* T_raw, double const* delta_raw,
double* T_plus_delta_raw) const override {
Eigen::Map<LieGroupd const> const T(T_raw);
TangentConstMap delta = Sophus::Mapper<Tangent>::map(delta_raw);
Eigen::Map<LieGroupd> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * LieGroupd::exp(delta);
return true;
}
/// Jacobian of LieGroup plus operation for Ceres
///
/// Dx T * exp(x) with x=0
///
bool PlusJacobian(double const* T_raw, double* jacobian_raw) const override {
Eigen::Map<LieGroupd const> T(T_raw);
Eigen::Map<Eigen::Matrix<double, num_parameters, DoF,
DoF == 1 ? Eigen::ColMajor : Eigen::RowMajor>>
jacobian(jacobian_raw);
jacobian = T.Dx_this_mul_exp_x_at_0();
return true;
}
bool Minus(double const* y_raw, double const* x_raw,
double* y_minus_x_raw) const override {
Eigen::Map<LieGroupd const> y(y_raw), x(x_raw);
TangentMap y_minus_x = Sophus::Mapper<Tangent>::map(y_minus_x_raw);
y_minus_x = (x.inverse() * y).log();
return true;
}
bool MinusJacobian(double const* x_raw, double* jacobian_raw) const override {
Eigen::Map<LieGroupd const> x(x_raw);
Eigen::Map<Eigen::Matrix<double, DoF, num_parameters, Eigen::RowMajor>>
jacobian(jacobian_raw);
jacobian = x.Dx_log_this_inv_by_x_at_this();
return true;
}
int AmbientSize() const override { return LieGroupd::num_parameters; }
int TangentSize() const override { return LieGroupd::DoF; }
};
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/ceres_typetraits.hpp
================================================
#ifndef SOPHUS_CERES_TYPETRAITS_HPP
#define SOPHUS_CERES_TYPETRAITS_HPP
namespace Sophus {
template <class T, std::size_t = sizeof(T)>
constexpr std::true_type complete(T*);
constexpr std::false_type complete(...);
template <class T>
using IsSpecialized = decltype(complete(std::declval<T*>()));
/// Type trait used to distinguish mappable vector types from scalars
///
/// We use this class to distinguish Sophus::Vector<Scalar, N> from Scalar types
/// in LieGroup<T>::Tangent
///
/// Primary use is mapping LieGroup::Tangent over raw data, with 2 options:
/// - LieGroup::Tangent is "scalar" (for SO2), then we just dereference pointer
/// - LieGroup::Tangent is Sophus::Vector<...>, then we need to use Eigen::Map
///
/// Specialization of Eigen::internal::traits<T> for T is crucial for
/// for constructing Eigen::Map<T>, thus we use that property for distinguishing
/// between those two options.
/// At this moment there seem to be no option to check this using only
/// "external" API of Eigen
template <class T>
using IsMappable = IsSpecialized<Eigen::internal::traits<std::decay_t<T>>>;
template <class T>
constexpr bool IsMappableV = IsMappable<T>::value;
/// Helper for mapping tangent vectors (scalars) over pointers to data
template <typename T, typename E = void>
struct Mapper {
using Scalar = T;
using Map = Scalar&;
using ConstMap = const Scalar&;
static Map map(Scalar* ptr) noexcept { return *ptr; }
static ConstMap map(const Scalar* ptr) noexcept { return *ptr; }
};
template <typename T>
struct Mapper<T, typename std::enable_if<IsMappableV<T>>::type> {
using Scalar = typename T::Scalar;
using Map = Eigen::Map<T>;
using ConstMap = Eigen::Map<const T>;
static Map map(Scalar* ptr) noexcept { return Map(ptr); }
static ConstMap map(const Scalar* ptr) noexcept { return ConstMap(ptr); }
};
} // namespace Sophus
#endif
================================================
FILE: Thirdparty/Sophus/sophus/common.hpp
================================================
/// @file
/// Common functionality.
#pragma once
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <random>
#include <type_traits>
#include <Eigen/Core>
#undef SOPHUS_COMPILE_TIME_FMT
#ifdef SOPHUS_USE_BASIC_LOGGING
#define SOPHUS_FMT_CSTR(description, ...) description
#define SOPHUS_FMT_STR(description, ...) std::string(description)
#define SOPHUS_FMT_PRINT(description, ...) std::printf("%s\n", description)
#define SOPHUS_FMT_ARG(arg)
#else // !SOPHUS_USE_BASIC_LOGGING
#ifdef __linux__
#define SOPHUS_COMPILE_TIME_FMT
#endif
#ifdef __APPLE__
#include "TargetConditionals.h"
#ifdef TARGET_OS_MAC
#define SOPHUS_COMPILE_TIME_FMT
#endif
#endif
#include <fmt/format.h>
#include <fmt/ostream.h>
#if FMT_VERSION >= 90000
#define SOPHUS_FMT_ARG(arg) fmt::streamed(arg)
#else
#define SOPHUS_FMT_ARG(arg) arg
#endif
#ifdef SOPHUS_COMPILE_TIME_FMT
// To keep compatibility with older libfmt versions,
// disable the compile time check if FMT_STRING is not available.
#ifdef FMT_STRING
// compile-time format check on x
#define SOPHUS_FMT_STRING(x) FMT_STRING(x)
#else
// identity, hence no compile-time check on x
#define SOPHUS_FMT_STRING(x) x
#endif
#else // ! SOPHUS_COMPILE_TIME_FMT
// identity, hence no compile-time check on x
#define SOPHUS_FMT_STRING(x) x
#endif // ! SOPHUS_COMPILE_TIME_FMT
#define SOPHUS_FMT_CSTR(description, ...) \
fmt::format(SOPHUS_FMT_STRING(description), ##__VA_ARGS__).c_str()
#define SOPHUS_FMT_STR(description, ...) \
fmt::format(SOPHUS_FMT_STRING(description), ##__VA_ARGS__)
#define SOPHUS_FMT_PRINT(description, ...) \
fmt::print(SOPHUS_FMT_STRING(description), ##__VA_ARGS__); \
fmt::print("\n")
#endif // !SOPHUS_USE_BASIC_LOGGING
// following boost's assert.hpp
#undef SOPHUS_ENSURE
// ENSURES are similar to ASSERTS, but they are always checked for (including in
// release builds). At the moment there are no ASSERTS in Sophus which should
// only be used for checks which are performance critical.
#ifdef __GNUC__
#define SOPHUS_FUNCTION __PRETTY_FUNCTION__
#elif (_MSC_VER >= 1310)
#define SOPHUS_FUNCTION __FUNCTION__
#else
#define SOPHUS_FUNCTION "unknown"
#endif
// Make sure this compiles with older versions of Eigen which do not have
// EIGEN_DEVICE_FUNC defined.
#ifndef EIGEN_DEVICE_FUNC
#define EIGEN_DEVICE_FUNC
#endif
// NVCC on windows has issues with defaulting the Map specialization
// constructors, so special case that specific platform case.
#if defined(_MSC_VER) && defined(__CUDACC__)
#define SOPHUS_WINDOW_NVCC_FALLBACK
#endif
#define SOPHUS_FUNC EIGEN_DEVICE_FUNC
#if defined(SOPHUS_DISABLE_ENSURES)
#define SOPHUS_ENSURE(expr, ...) ((void)0)
#elif defined(SOPHUS_ENABLE_ENSURE_HANDLER)
namespace Sophus {
void ensureFailed(char const* function, char const* file, int line,
char const* description);
}
#define SOPHUS_ENSURE(expr, description, ...) \
((expr) \
? ((void)0) \
: ::Sophus::ensureFailed(SOPHUS_FUNCTION, __FILE__, __LINE__, \
SOPHUS_FMT_CSTR(description, ##__VA_ARGS__)))
#else
#define SOPHUS_DEDAULT_ENSURE_FAILURE_IMPL(function, file, line, description, \
...) \
do { \
std::printf( \
"Sophus ensure failed in function '%s', " \
"file '%s', line %d.\n", \
function, file, line); \
SOPHUS_FMT_PRINT(description, ##__VA_ARGS__); \
std::abort(); \
} while (false)
#ifdef __CUDACC__
#define SOPHUS_ENSURE(expr, description, ...) \
do { \
if (!(expr)) { \
std::printf( \
"Sophus ensure failed in function '%s', file '%s', line %d.\n", \
SOPHUS_FUNCTION, __FILE__, __LINE__); \
std::printf("%s", description); \
/* there is no std::abort in cuda kernels, hence we just print the error \
* message here*/ \
} \
} while (false)
#else
#define SOPHUS_ENSURE(expr, ...) \
do { \
if (!(expr)) { \
SOPHUS_DEDAULT_ENSURE_FAILURE_IMPL(SOPHUS_FUNCTION, __FILE__, __LINE__, \
##__VA_ARGS__); \
} \
} while (false)
#endif
#endif
namespace Sophus {
template <class Scalar>
struct Constants {
SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
SOPHUS_FUNC static Scalar epsilonPlus() {
return epsilon() * (Scalar(1.) + epsilon());
}
SOPHUS_FUNC static Scalar epsilonSqrt() {
using std::sqrt;
return sqrt(epsilon());
}
SOPHUS_FUNC static Scalar pi() {
return Scalar(3.141592653589793238462643383279502884);
}
};
template <>
struct Constants<float> {
SOPHUS_FUNC static float constexpr epsilon() {
return static_cast<float>(1e-5);
}
SOPHUS_FUNC static float epsilonPlus() {
return epsilon() * (1.f + epsilon());
}
SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
SOPHUS_FUNC static float constexpr pi() {
return 3.141592653589793238462643383279502884f;
}
};
/// Nullopt type of lightweight optional class.
struct nullopt_t {
explicit constexpr nullopt_t() {}
};
constexpr nullopt_t nullopt{};
/// Lightweight optional implementation which requires ``T`` to have a
/// default constructor.
///
/// TODO: Replace with std::optional once Sophus moves to c++17.
///
template <class T>
class optional {
public:
optional() : is_valid_(false) {}
optional(nullopt_t) : is_valid_(false) {}
optional(T const& type) : type_(type), is_valid_(true) {}
explicit operator bool() const { return is_valid_; }
T const* operator->() const {
SOPHUS_ENSURE(is_valid_, "must be valid");
return &type_;
}
T* operator->() {
SOPHUS_ENSURE(is_valid_, "must be valid");
return &type_;
}
T const& operator*() const {
SOPHUS_ENSURE(is_valid_, "must be valid");
return type_;
}
T& operator*() {
SOPHUS_ENSURE(is_valid_, "must be valid");
return type_;
}
private:
T type_;
bool is_valid_;
};
template <bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;
template <class G>
struct IsUniformRandomBitGenerator {
static const bool value = std::is_unsigned<typename G::result_type>::value &&
std::is_unsigned<decltype(G::min())>::value &&
std::is_unsigned<decltype(G::max())>::value;
};
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/example_ensure_handler.cpp
================================================
#include "common.hpp"
#include <cstdio>
#include <cstdlib>
namespace Sophus {
void ensureFailed(char const* function, char const* file, int line,
char const* description) {
std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n",
file, function, line);
std::printf("Description: %s\n", description);
std::abort();
}
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/geometry.hpp
================================================
/// @file
/// Transformations between poses and hyperplanes.
#pragma once
#include "se2.hpp"
#include "se3.hpp"
#include "so2.hpp"
#include "so3.hpp"
#include "types.hpp"
namespace Sophus {
/// Takes in a rotation ``R_foo_plane`` and returns the corresponding line
/// normal along the y-axis (in reference frame ``foo``).
///
template <class T>
Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
return R_foo_line.matrix().col(1);
}
/// Takes in line normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_line``.
///
/// Precondition: ``normal_foo`` must not be close to zero.
///
template <class T>
SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "{}",
SOPHUS_FMT_ARG(normal_foo.transpose()));
normal_foo.normalize();
return SO2<T>(normal_foo.y(), -normal_foo.x());
}
/// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane
/// normal along the z-axis
/// (in reference frame ``foo``).
///
template <class T>
Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
return R_foo_plane.matrix().col(2);
}
/// Takes in plane normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_plane``.
///
/// Note: The ``plane`` frame is defined as such that the normal points along
/// the positive z-axis. One can specify hints for the x-axis and y-axis
/// of the ``plane`` frame.
///
/// Preconditions:
/// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to
/// zero.
/// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular.
///
template <class T>
Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0),
T(0)),
Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1),
T(0))) {
SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::epsilon(),
"xDirHint ({}) and yDirHint ({}) must be perpendicular.",
SOPHUS_FMT_ARG(xDirHint_foo.transpose()),
SOPHUS_FMT_ARG(yDirHint_foo.transpose()));
using std::abs;
using std::sqrt;
T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm();
T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm();
T const normal_foo_sqr_length = normal_foo.squaredNorm();
SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants<T>::epsilon(), "{}",
SOPHUS_FMT_ARG(xDirHint_foo.transpose()));
SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "{}",
SOPHUS_FMT_ARG(yDirHint_foo.transpose()));
SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "{}",
SOPHUS_FMT_ARG(normal_foo.transpose()));
Matrix3<T> basis_foo;
basis_foo.col(2) = normal_foo;
if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
xDirHint_foo.normalize();
}
if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
yDirHint_foo.normalize();
}
if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
basis_foo.col(2).normalize();
}
T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo));
T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo));
if (abs_x_dot_z < abs_y_dot_z) {
// basis_foo.z and xDirHint are far from parallel.
basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized();
basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2));
} else {
// basis_foo.z and yDirHint are far from parallel.
basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized();
basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0));
}
T det = basis_foo.determinant();
// sanity check
SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::epsilon(),
"Determinant of basis is not 1, but {}. Basis is \n{}\n",
SOPHUS_FMT_ARG(det), SOPHUS_FMT_ARG(basis_foo));
return basis_foo;
}
/// Takes in plane normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_plane``.
///
/// See ``rotationFromNormal`` for details.
///
template <class T>
SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
return SO3<T>(rotationFromNormal(normal_foo));
}
/// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in
/// reference frame ``foo``.
///
/// Note: The plane is defined by X-axis of the ``line`` frame.
///
template <class T>
Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation());
}
/// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``.
///
/// Note: The line is defined by X-axis of the frame ``line``.
///
template <class T>
SE2<T> SE2FromLine(Line2<T> const& line_foo) {
T const d = line_foo.offset();
Vector2<T> const n = line_foo.normal();
SO2<T> const R_foo_plane = SO2FromNormal(n);
return SE2<T>(R_foo_plane, -d * n);
}
/// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in
/// reference frame ``foo``.
///
/// Note: The plane is defined by XY-plane of the frame ``plane``.
///
template <class T>
Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation());
}
/// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``.
///
/// Note: The plane is defined by XY-plane of the frame ``plane``.
///
template <class T>
SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
T const d = plane_foo.offset();
Vector3<T> const n = plane_foo.normal();
SO3<T> const R_foo_plane = SO3FromNormal(n);
return SE3<T>(R_foo_plane, -d * n);
}
/// Takes in a hyperplane and returns unique representation by ensuring that the
/// ``offset`` is not negative.
///
template <class T, int N>
Eigen::Hyperplane<T, N> makeHyperplaneUnique(
Eigen::Hyperplane<T, N> const& plane) {
if (plane.offset() >= 0) {
return plane;
}
return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
}
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/interpolate.hpp
================================================
/// @file
/// Interpolation for Lie groups.
#pragma once
#include <Eigen/Eigenvalues>
#include "interpolate_details.hpp"
namespace Sophus {
/// This function interpolates between two Lie group elements ``foo_T_bar``
/// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
///
/// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
/// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
/// ``foo_T_baz``.
///
/// (Since interpolation on Lie groups is inverse-invariant, we can equivalently
/// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
/// return value being ``quiz_T_foo``.)
///
/// Precondition: ``p`` must be in [0, 1].
///
template <class G, class Scalar2 = typename G::Scalar>
enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {
using Scalar = typename G::Scalar;
Scalar inter_p(p);
SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
"p ({}) must in [0, 1].", SOPHUS_FMT_ARG(inter_p));
return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/interpolate_details.hpp
================================================
#pragma once
#include "cartesian.hpp"
#include "rxso2.hpp"
#include "rxso3.hpp"
#include "se2.hpp"
#include "se3.hpp"
#include "sim2.hpp"
#include "sim3.hpp"
#include "so2.hpp"
#include "so3.hpp"
namespace Sophus {
namespace interp_details {
template <class Group>
struct Traits;
template <class Scalar, int Dim>
struct Traits<Cartesian<Scalar, Dim>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Cartesian<Scalar, Dim> const&) {
return false;
}
};
template <class Scalar>
struct Traits<SO2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
using std::abs;
Scalar angle = abs(foo_T_bar.log());
Scalar const kPi = Constants<Scalar>::pi();
return abs(angle - kPi) / (angle + kPi) < Constants<Scalar>::epsilon();
}
};
template <class Scalar>
struct Traits<RxSO2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
}
};
template <class Scalar>
struct Traits<SO3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
using std::abs;
Scalar angle = abs(foo_T_bar.logAndTheta().theta);
Scalar const kPi = Constants<Scalar>::pi();
return abs(angle - kPi) / (angle + kPi) < Constants<Scalar>::epsilon();
}
};
template <class Scalar>
struct Traits<RxSO3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
}
};
template <class Scalar>
struct Traits<SE2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());
}
};
template <class Scalar>
struct Traits<SE3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());
}
};
template <class Scalar>
struct Traits<Sim2<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(
foo_T_bar.rxso2().so2());
;
}
};
template <class Scalar>
struct Traits<Sim3<Scalar>> {
static bool constexpr supported = true;
static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(
foo_T_bar.rxso3().so3());
;
}
};
} // namespace interp_details
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/num_diff.hpp
================================================
/// @file
/// Numerical differentiation using finite differences
#pragma once
#include <functional>
#include <type_traits>
#include <utility>
#include "types.hpp"
namespace Sophus {
namespace details {
template <class Scalar>
class Curve {
public:
template <class Fn>
static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) {
using ReturnType = decltype(curve(t));
static_assert(std::is_floating_point<Scalar>::value,
"Scalar must be a floating point type.");
static_assert(IsFloatingPoint<ReturnType>::value,
"ReturnType must be either a floating point scalar, "
"vector or matrix.");
return (curve(t + h) - curve(t - h)) / (Scalar(2) * h);
}
};
template <class Scalar, int N, int M>
class VectorField {
public:
static Eigen::Matrix<Scalar, N, M> num_diff(
std::function<Sophus::Vector<Scalar, N>(Sophus::Vector<Scalar, M>)>
vector_field,
Sophus::Vector<Scalar, M> const& a, Scalar eps) {
static_assert(std::is_floating_point<Scalar>::value,
"Scalar must be a floating point type.");
Eigen::Matrix<Scalar, N, M> J;
Sophus::Vector<Scalar, M> h;
h.setZero();
for (int i = 0; i < M; ++i) {
h[i] = eps;
J.col(i) =
(vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps);
h[i] = Scalar(0);
}
return J;
}
};
template <class Scalar, int N>
class VectorField<Scalar, N, 1> {
public:
static Eigen::Matrix<Scalar, N, 1> num_diff(
std::function<Sophus::Vector<Scalar, N>(Scalar)> vector_field,
Scalar const& a, Scalar eps) {
return details::Curve<Scalar>::num_diff(std::move(vector_field), a, eps);
}
};
} // namespace details
/// Calculates the derivative of a curve at a point ``t``.
///
/// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it
/// returns either a Scalar, a vector or a matrix.
///
template <class Scalar, class Fn>
auto curveNumDiff(Fn curve, Scalar t,
Scalar h = Constants<Scalar>::epsilonSqrt())
-> decltype(details::Curve<Scalar>::num_diff(std::move(curve), t, h)) {
return details::Curve<Scalar>::num_diff(std::move(curve), t, h);
}
/// Calculates the derivative of a vector field at a point ``a``.
///
/// Here, a vector field is a function from a vector space to another vector
/// space.
///
template <class Scalar, int N, int M, class ScalarOrVector, class Fn>
Eigen::Matrix<Scalar, N, M> vectorFieldNumDiff(
Fn vector_field, ScalarOrVector const& a,
Scalar eps = Constants<Scalar>::epsilonSqrt()) {
return details::VectorField<Scalar, N, M>::num_diff(std::move(vector_field),
a, eps);
}
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/rotation_matrix.hpp
================================================
/// @file
/// Rotation matrix helper functions.
#pragma once
#include <Eigen/Dense>
#include <Eigen/SVD>
#include "types.hpp"
namespace Sophus {
/// Takes in arbitrary square matrix and returns true if it is
/// orthogonal.
template <class D>
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
using Scalar = typename D::Scalar;
static int const N = D::RowsAtCompileTime;
static int const M = D::ColsAtCompileTime;
static_assert(N == M, "must be a square matrix");
static_assert(N >= 2, "must have compile time dimension >= 2");
return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
Constants<Scalar>::epsilon();
}
/// Takes in arbitrary square matrix and returns true if it is
/// "scaled-orthogonal" with positive determinant.
///
template <class D>
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
using Scalar = typename D::Scalar;
static int const N = D::RowsAtCompileTime;
static int const M = D::ColsAtCompileTime;
using std::pow;
using std::sqrt;
Scalar det = sR.determinant();
if (det <= Scalar(0)) {
return false;
}
Scalar scale_sqr = pow(det, Scalar(2. / N));
static_assert(N == M, "must be a square matrix");
static_assert(N >= 2, "must have compile time dimension >= 2");
return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
.template lpNorm<Eigen::Infinity>() <
sqrt(Constants<Scalar>::epsilon());
}
/// Takes in arbitrary square matrix (2x2 or larger) and returns closest
/// orthogonal matrix with positive determinant.
template <class D>
SOPHUS_FUNC enable_if_t<
std::is_floating_point<typename D::Scalar>::value,
Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
using Scalar = typename D::Scalar;
static int const N = D::RowsAtCompileTime;
static int const M = D::ColsAtCompileTime;
static_assert(N == M, "must be a square matrix");
static_assert(N >= 2, "must have compile time dimension >= 2");
Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(
R, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Determine determinant of orthogonal matrix U*V'.
Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
// Starting from the identity matrix D, set the last entry to d (+1 or
// -1), so that det(U*D*V') = 1.
Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity();
Diag(N - 1, N - 1) = d;
return svd.matrixU() * Diag * svd.matrixV().transpose();
}
} // namespace Sophus
================================================
FILE: Thirdparty/Sophus/sophus/rxso2.hpp
================================================
/// @file
/// Direct product R X SO(2) - rotation and scaling in 2d.
#pragma once
#include "so2.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class RxSO2;
using RxSO2d = RxSO2<double>;
using RxSO2f = RxSO2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::RxSO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Sophus::Vector2<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
: traits<Sophus::RxSO2<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
: traits<Sophus::RxSO2<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// RxSO2 base type - implements RxSO2 class but is storage agnostic
///
/// This class implements the group ``R+ x SO(2)``, the direct product of the
/// group of positive scalar 2x2 matrices (= isomorph to the positive
/// real numbers) and the two-dimensional special orthogonal group SO(2).
/// Geometrically, it is the group of rotation and scaling in two dimensions.
/// As a matrix groups, R+ x SO(2) consists of matrices of the form ``s * R``
/// where ``R`` is an orthogonal matrix with ``det(R) = 1`` and ``s > 0``
/// being a positive real number. In particular, it has the following form:
///
/// | s * cos(theta) s * -sin(theta) |
/// | s * sin(theta) s * cos(theta) |
///
/// where ``theta`` being the rotation angle. Internally, it is represented by
/// the first column of the rotation matrix, or in other words by a non-zero
/// complex number.
///
/// R+ x SO(2) is not compact, but a commutative group. First it is not compact
/// since the scale factor is not bound. Second it is commutative since
/// ``sR(alpha, s1) * sR(beta, s2) = sR(beta, s2) * sR(alpha, s1)``, simply
/// because ``alpha + beta = beta + alpha`` and ``s1 * s2 = s2 * s1`` with
/// ``alpha`` and ``beta`` being rotation angles and ``s1``, ``s2`` being scale
/// factors.
///
/// This class has the explicit class invariant that the scale ``s`` is not
/// too close to either zero or infinity. Strictly speaking, it must hold that:
///
/// ``complex().norm() >= Constants::epsilon()`` and
/// ``1. / complex().norm() >= Constants::epsilon()``.
///
/// In order to obey this condition, group multiplication is implemented with
/// saturation such that a product always has a scale which is equal or greater
/// this threshold.
template <class Derived>
class RxSO2Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using ComplexType = typename Eigen::internal::traits<Derived>::ComplexType;
using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (one for rotation and one for scaling).
static int constexpr DoF = 2;
/// Number of internal parameters used (complex number is a tuple).
static int constexpr num_parameters = 2;
/// Group transformations are 2x2 matrices.
static int constexpr N = 2;
/// Points are 2-dimensional
static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Hyperplane = Hyperplane2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with RxSO2 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using RxSO2Product = RxSO2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
/// For RxSO(2), it simply returns the identity matrix.
///
SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
/// Returns rotation angle.
///
SOPHUS_FUNC Scalar angle() const { return SO2<Scalar>(complex()).log(); }
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC RxSO2<NewScalarType> cast() const {
typename RxSO2<NewScalarType>::ComplexType c =
complex().template cast<NewScalarType>();
return RxSO2<NewScalarType>(c);
}
/// This provides unsafe read/write access to internal data. RxSO(2) is
/// represented by a complex number (two parameters). When using direct
/// write access, the user needs to take care of that the complex number is
/// not set close to zero.
///
/// Note: The first parameter represents the real part, while the
/// second parameter represent the imaginary part.
///
SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); }
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const { return complex().data(); }
/// Returns group inverse.
///
SOPHUS_FUNC RxSO2<Scalar> inverse() const {
Scalar squared_scale = complex().squaredNorm();
Vector2<Scalar> xy = complex() / squared_scale;
return RxSO2<Scalar>(xy.x(), -xy.y());
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (scaled rotation matrices) to elements of the tangent
/// space (rotation-vector plus logarithm of scale factor).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of RxSO2.
///
SOPHUS_FUNC Tangent log() const {
using std::log;
Tangent theta_sigma;
theta_sigma[1] = log(scale());
theta_sigma[0] = SO2<Scalar>(complex()).log();
return theta_sigma;
}
/// Returns 2x2 matrix representation of the instance.
///
/// For RxSO2, the matrix representation is an scaled orthogonal matrix ``sR``
/// with ``det(R)=s^2``, thus a scaled rotation matrix ``R`` with scale
/// ``s``.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation sR;
// clang-format off
sR << complex()[0], -complex()[1],
complex()[1], complex()[0];
// clang-format on
return sR;
}
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC RxSO2Base<Derived>& operator=(
RxSO2Base<OtherDerived> const& other) {
complex_nonconst() = other.complex();
return *this;
}
/// Group multiplication, which is rotation concatenation and scale
/// multiplication.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived>
SOPHUS_FUNC RxSO2Product<OtherDerived> operator*(
RxSO2Base<OtherDerived> const& other) const {
using ResultT = ReturnScalar<OtherDerived>;
Scalar lhs_real = complex().x();
Scalar lhs_imag = complex().y();
typename OtherDerived::Scalar const& rhs_real = other.complex().x();
typename OtherDerived::Scalar const& rhs_imag = other.complex().y();
/// complex multiplication
typename RxSO2Product<OtherDerived>::ComplexType result_complex(
lhs_real * rhs_real - lhs_imag * rhs_imag,
lhs_real * rhs_imag + lhs_imag * rhs_real);
const ResultT squared_scale = result_complex.squaredNorm();
if (squared_scale <
Constants<ResultT>::epsilon() * Constants<ResultT>::epsilon()) {
/// Saturation to ensure class invariant.
result_complex.normalize();
result_complex *= Constants<ResultT>::epsilonPlus();
}
if (squared_scale > Scalar(1.) / (Constants<ResultT>::epsilon() *
Constants<ResultT>::epsilon())) {
/// Saturation to ensure class invariant.
result_complex.normalize();
result_complex /= Constants<ResultT>::epsilonPlus();
}
return RxSO2Product<OtherDerived>(result_complex);
}
/// Group action on 2-points.
///
/// This function rotates a 2 dimensional point ``p`` by the SO2 element
/// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor ``s``:
///
/// ``p_bar = s * (bar_R_foo * p_foo)``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return matrix() * p;
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rsp = *this * p.template head<2>();
return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), p(2));
}
/// Group action on lines.
///
/// This function rotates a parameterized line ``l(t) = o + t * d`` by the SO2
/// element and scales it by the scale factor
///
/// Origin ``o`` is rotated and scaled
/// Direction ``d`` is rotated (preserving it's norm)
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(), (*this) * l.direction() / scale());
}
/// Group action on hyper-planes.
///
/// This function rotates a hyper-plane ``n.x + d = 0`` by the SO2
/// element and scales offset by the scale factor
///
/// Normal vector ``n`` is rotated
/// Offset ``d`` is scaled
///
/// Note that in 2d-case hyper-planes are just another parametrization of
/// lines
///
SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
const auto this_scale = scale();
return Hyperplane((*this) * p.normal() / this_scale,
this_scale * p.offset());
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SO2's Scalar type.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC RxSO2Base<Derived>& operator*=(
RxSO2Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns derivative of this * RxSO2::exp(x) wrt. x at x=0
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
J << -complex().y(), complex().x(), complex().x(), complex().y();
return J;
}
/// Returns derivative of log(this^{-1} * x) by x at x=this.
///
SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
const {
Matrix<Scalar, DoF, num_parameters> J;
const Scalar norm_sq_inv = Scalar(1.) / complex().squaredNorm();
J << -complex().y(), complex().x(), complex().x(), complex().y();
return J * norm_sq_inv;
}
/// Returns internal parameters of RxSO(2).
///
/// It returns (c[0], c[1]), with c being the complex number.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
return complex();
}
/// Sets non-zero complex
///
/// Precondition: ``z`` must not be close to either zero or infinity.
SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
SOPHUS_ENSURE(z.squaredNorm() > Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
SOPHUS_ENSURE(z.squaredNorm() < Scalar(1.) / (Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()),
"Inverse scale factor must be greater-equal epsilon.");
static_cast<Derived*>(this)->complex_nonconst() = z;
}
/// Accessor of complex.
///
SOPHUS_FUNC ComplexType const& complex() const {
return static_cast<Derived const*>(this)->complex();
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Transformation rotationMatrix() const {
ComplexTemporaryType norm_quad = complex();
norm_quad.normalize();
return SO2<Scalar>(norm_quad).matrix();
}
/// Returns scale.
///
SOPHUS_FUNC Scalar scale() const {
using std::hypot;
return hypot(complex().x(), complex().y());
}
/// Setter of rotation angle, leaves scale as is.
///
SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(theta)); }
/// Setter of complex using rotation matrix ``R``, leaves scale as is.
///
/// Precondition: ``R`` must be orthogonal with determinant of one.
///
SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
setSO2(SO2<Scalar>(R));
}
/// Sets scale and leaves rotation as is.
///
SOPHUS_FUNC void setScale(Scalar const& scale) {
using std::sqrt;
complex_nonconst().normalize();
complex_nonconst() *= scale;
}
/// Setter of complex number using scaled rotation matrix ``sR``.
///
/// Precondition: The 2x2 matrix must be "scaled orthogonal"
/// and have a positive determinant.
///
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR),
"sR must be scaled orthogonal:\n {}", SOPHUS_FMT_ARG(sR));
complex_nonconst() = sR.col(0);
}
/// Setter of SO(2) rotations, leaves scale as is.
///
SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
using std::sqrt;
Scalar saved_scale = scale();
complex_nonconst() = so2.unit_complex();
complex_nonconst() *= saved_scale;
}
SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
private:
/// Mutator of complex is private to ensure class invariant.
///
SOPHUS_FUNC ComplexType& complex_nonconst() {
return static_cast<Derived*>(this)->complex_nonconst();
}
};
/// RxSO2 using storage; derived from RxSO2Base.
template <class Scalar_, int Options>
class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
public:
using Base = RxSO2Base<RxSO2<Scalar_, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using ComplexMember = Eigen::Matrix<Scalar, 2, 1, Options>;
/// ``Base`` is friend so complex_nonconst can be accessed from ``Base``.
friend class RxSO2Base<RxSO2<Scalar_, Options>>;
using Base::operator=;
/// Define copy-assignment operator explicitly. The definition of
/// implicit copy assignment operator is deprecated in presence of a
/// user-declared copy constructor (-Wdeprecated-copy in clang >= 13).
SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes complex number to identity rotation and
/// scale to 1.
///
SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {}
/// Copy constructor
///
SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
/// Copy-like constructor from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
: complex_(other.complex()) {}
/// Constructor from scaled rotation matrix
///
/// Precondition: rotation matrix need to be scaled orthogonal with
/// determinant of ``s^2``.
///
SOPHUS_FUNC explicit RxSO2(Transformation const& sR) {
this->setScaledRotationMatrix(sR);
}
/// Constructor from scale factor and rotation matrix ``R``.
///
/// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant
/// of 1 and ``scale`` must not to be close to either zero or
/// infinity.
///
SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
: RxSO2((scale * SO2<Scalar>(R).unit_complex()).eval()) {}
/// Constructor from scale factor and SO2
///
/// Precondition: ``scale`` must not be close to either zero or infinity.
///
SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
: RxSO2((scale * so2.unit_complex()).eval()) {}
/// Constructor from complex number.
///
/// Precondition: complex number must not be close to either zero or infinity
///
SOPHUS_FUNC explicit RxSO2(Vector2<Scalar> const& z) : complex_(z) {
SOPHUS_ENSURE(complex_.squaredNorm() >= Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon: {} vs {}",
SOPHUS_FMT_ARG(complex_.squaredNorm()),
SOPHUS_FMT_ARG(Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()));
SOPHUS_ENSURE(
complex_.squaredNorm() <= Scalar(1.) / (Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()),
"Inverse scale factor must be greater-equal epsilon: {} vs {}",
SOPHUS_FMT_ARG(Scalar(1.) / complex_.squaredNorm()),
SOPHUS_FMT_ARG(Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()));
}
/// Constructor from complex number.
///
/// Precondition: complex number must not be close to either zero or infinity.
///
SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag)
: RxSO2(Vector2<Scalar>(real, imag)) {}
/// Accessor of complex.
///
SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
/// Returns derivative of exp(x) wrt. ``x``
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const& a) {
using std::cos;
using std::exp;
using std::sin;
Scalar const theta = a[0];
Scalar const sigma = a[1];
Sophus::Matrix<Scalar, num_parameters, DoF> J;
J << -sin(theta), cos(theta), cos(theta), sin(theta);
return J * exp(sigma);
}
/// Returns derivative of exp(x) wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
Sophus::Matrix<Scalar, num_parameters, DoF> J;
static Scalar const i(1.);
static Scalar const o(0.);
J << o, i, i, o;
return J;
}
/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 2, DoF> j;
j << Sophus::SO2<Scalar>::Dx_exp_x_times_point_at_0(point), point;
return j;
}
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Group exponential
///
/// This functions takes in an element of tangent space (= rotation angle
/// plus logarithm of scale) and returns the corresponding element of the
/// group RxSO2.
///
/// To be more specific, this function computes ``expmat(hat(theta))``
/// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
/// hat()-operator of RSO2.
///
SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) {
using std::exp;
using std::max;
using std::min;
Scalar const theta = a[0];
Scalar const sigma = a[1];
Scalar s = exp(sigma);
// Ensuring proper scale
s = max(s, Constants<Scalar>::epsilonPlus());
s = min(s, Scalar(1.) / Constants<Scalar>::epsilonPlus());
Vector2<Scalar> z = SO2<Scalar>::exp(theta).unit_complex();
z *= s;
return RxSO2<Scalar>(z);
}
/// Returns the ith infinitesimal generators of ``R+ x SO(2)``.
///
/// The infinitesimal generators of RxSO2 are:
///
/// ```
/// | 0 -1 |
/// G_0 = | 1 0 |
///
/// | 1 0 |
/// G_1 = | 0 1 |
/// ```
///
/// Precondition: ``i`` must be 0, or 1.
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 && i <= 1, "i should be 0 or 1.");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// hat-operator
///
/// It takes in the 2-vector representation ``a`` (= rotation angle plus
/// logarithm of scale) and returns the corresponding matrix representation
/// of Lie algebra element.
///
/// Formally, the hat()-operator of RxSO2 is defined as
///
/// ``hat(.): R^2 -> R^{2x2}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2)
///
/// with ``G_i`` being the ith infinitesimal generator of RxSO2.
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation A;
// clang-format off
A << a(1), -a(0),
a(0), a(1);
// clang-format on
return A;
}
/// Lie bracket
///
/// It computes the Lie bracket of RxSO(2). To be more specific, it computes
///
/// ``[omega_1, omega_2]_rxso2 := vee([hat(omega_1), hat(omega_2)])``
///
/// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
/// hat()-operator and ``vee(.)`` the vee()-operator of RxSO2.
///
SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
Vector2<Scalar> res;
res.setZero();
return res;
}
/// Draw uniform sample from RxSO(2) manifold.
///
/// The scale factor is drawn uniformly in log2-space from [-1, 1],
/// hence the scale is in [0.5, 2)].
///
template <class UniformRandomBitGenerator>
static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
using std::exp2;
return RxSO2(exp2(uniform(generator)),
SO2<Scalar>::sampleUniform(generator));
}
/// vee-operator
///
/// It takes the 2x2-matrix representation ``Omega`` and maps it to the
/// corresponding vector representation of Lie algebra.
///
/// This is the inverse of the hat()-operator, see above.
///
/// Precondition: ``Omega`` must have the following structure:
///
/// | d -x |
/// | x d |
///
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
using std::abs;
return Tangent(Omega(1, 0), Omega(0, 0));
}
protected:
SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
ComplexMember complex_;
};
} // namespace Sophus
namespace Eigen {
/// Specialization of Eigen::Map for ``RxSO2``; derived from RxSO2Base.
///
/// Allows us to wrap RxSO2 objects around POD array (e.g. external z style
/// complex).
template <class Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_>, Options>
: public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>> {
using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>>;
public:
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
/// ``Base`` is friend so complex_nonconst can be accessed from ``Base``.
friend class Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>>;
using Base::operator=;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC explicit Map(Scalar* coeffs) : complex_(coeffs) {}
/// Accessor of complex.
///
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar>, Options> const& complex() const {
return complex_;
}
protected:
SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& complex_nonconst() {
return complex_;
}
Map<Sophus::Vector2<Scalar>, Options> complex_;
};
/// Specialization of Eigen::Map for ``RxSO2 const``; derived from RxSO2Base.
///
/// Allows us to wrap RxSO2 objects around POD array (e.g. external z style
/// complex).
template <class Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_> const, Options>
: public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>> {
public:
using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
explicit Map(Scalar const* coeffs) : complex_(coeffs) {}
/// Accessor of complex.
///
SOPHUS_FUNC
Map<Sophus::Vector2<Scalar> const, Options> const& complex() const {
return complex_;
}
protected:
Map<Sophus::Vector2<Scalar> const, Options> const complex_;
};
} // namespace Eigen
================================================
FILE: Thirdparty/Sophus/sophus/rxso3.hpp
================================================
/// @file
/// Direct product R X SO(3) - rotation and scaling in 3d.
#pragma once
#include "so3.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class RxSO3;
using RxSO3d = RxSO3<double>;
using RxSO3f = RxSO3<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options_>
struct traits<Sophus::RxSO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Eigen::Quaternion<Scalar, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
: traits<Sophus::RxSO3<Scalar_, Options_>> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
};
template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
: traits<Sophus::RxSO3<Scalar_, Options_> const> {
static constexpr int Options = Options_;
using Scalar = Scalar_;
using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// RxSO3 base type - implements RxSO3 class but is storage agnostic
///
/// This class implements the group ``R+ x SO(3)``, the direct product of the
/// group of positive scalar 3x3 matrices (= isomorph to the positive
/// real numbers) and the three-dimensional special orthogonal group SO(3).
/// Geometrically, it is the group of rotation and scaling in three dimensions.
/// As a matrix groups, RxSO3 consists of matrices of the form ``s * R``
/// where ``R`` is an orthogonal matrix with ``det(R)=1`` and ``s > 0``
/// being a positive real number.
///
/// Internally, RxSO3 is represented by the group of non-zero quaternions.
/// In particular, the scale equals the squared(!) norm of the quaternion ``q``,
/// ``s = |q|^2``. This is a most compact representation since the degrees of
/// freedom (DoF) of RxSO3 (=4) equals the number of internal parameters (=4).
///
/// This class has the explicit class invariant that the scale ``s`` is not
/// too close to either zero or infinity. Strictly speaking, it must hold that:
///
/// ``quaternion().squaredNorm() >= Constants::epsilon()`` and
/// ``1. / quaternion().squaredNorm() >= Constants::epsilon()``.
///
/// In order to obey this condition, group multiplication is implemented with
/// saturation such that a product always has a scale which is equal or greater
/// this threshold.
template <class Derived>
class RxSO3Base {
public:
static constexpr int Options = Eigen::internal::traits<Derived>::Options;
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using QuaternionType =
typename Eigen::internal::traits<Derived>::QuaternionType;
using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (three for rotation and one for scaling).
static int constexpr DoF = 4;
/// Number of internal parameters used (quaternion is a 4-tuple).
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
/// Points are 3-dimensional
static int constexpr Dim = 3;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector3<Scalar>;
using HomogeneousPoint = Vector4<Scalar>;
using Line = ParametrizedLine3<Scalar>;
using Hyperplane = Hyperplane3<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
struct TangentAndTheta {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Tangent tangent;
Scalar theta;
};
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with RxSO3 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using RxSO3Product = RxSO3<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector3<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
/// For RxSO(3), it simply returns the rotation matrix corresponding to ``A``.
///
SOPHUS_FUNC Adjoint Adj() const {
Adjoint res;
res.setIdentity();
res.template topLeftCorner<3, 3>() = rotationMatrix();
return res;
}
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC RxSO3<NewScalarType> cast() const {
return RxSO3<NewScalarType>(quaternion().template cast<NewScalarType>());
}
/// This provides unsafe read/write access to internal data. RxSO(3) is
/// represented by an Eigen::Quaternion (four parameters). When using direct
/// write access, the user needs to take care of that the quaternion is not
/// set close to zero.
///
/// Note: The first three Scalars represent the imaginary parts, while the
/// forth Scalar represent the real part.
///
SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); }
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const {
return quaternion().coeffs().data();
}
/// Returns group inverse.
///
SOPHUS_FUNC RxSO3<Scalar> inverse() const {
return RxSO3<Scalar>(quaternion().inverse());
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (scaled rotation matrices) to elements of the tangent
/// space (rotation-vector plus logarithm of scale factor).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of RxSO3.
///
SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
/// As above, but also returns ``theta = |omega|``.
///
SOPHUS_FUNC TangentAndTheta logAndTheta() const {
using std::log;
Scalar scale = quaternion().squaredNorm();
TangentAndTheta result;
result.tangent[3] = log(scale);
auto omega_and_theta = SO3<Scalar>(quaternion()).logAndTheta();
result.tangent.template head<3>() = omega_and_theta.tangent;
result.theta = omega_and_theta.theta;
return result;
}
/// Returns 3x3 matrix representation of the instance.
///
/// For RxSO3, the matrix representation is an scaled orthogonal matrix ``sR``
/// with ``det(R)=s^3``, thus a scaled rotation matrix ``R`` with scale
/// ``s``.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation sR;
Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x();
Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y();
Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z();
Scalar const w_sq = quaternion().w() * quaternion().w();
Scalar const two_vx = Scalar(2) * quaternion().vec().x();
Scalar const two_vy = Scalar(2) * quaternion().vec().y();
Scalar const two_vz = Scalar(2) * quaternion().vec().z();
Scalar const two_vx_vy = two_vx * quaternion().vec().y();
Scalar const two_vx_vz = two_vx * quaternion().vec().z();
Scalar const two_vx_w = two_vx * quaternion().w();
Scalar const two_vy_vz = two_vy * quaternion().vec().z();
Scalar const two_vy_w = two_vy * quaternion().w();
Scalar const two_vz_w = two_vz * quaternion().w();
sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq;
sR(1, 0) = two_vx_vy + two_vz_w;
sR(2, 0) = two_vx_vz - two_vy_w;
sR(0, 1) = two_vx_vy - two_vz_w;
sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq;
sR(2, 1) = two_vx_w + two_vy_vz;
sR(0, 2) = two_vx_vz + two_vy_w;
sR(1, 2) = -two_vx_w + two_vy_vz;
sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq;
return sR;
}
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC RxSO3Base<Derived>& operator=(
RxSO3Base<OtherDerived> const& other) {
quaternion_nonconst() = other.quaternion();
return *this;
}
/// Group multiplication, which is rotation concatenation and scale
/// multiplication.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived>
SOPHUS_FUNC RxSO3Product<OtherDerived> operator*(
RxSO3Base<OtherDerived> const& other) const {
using std::sqrt;
using ResultT = ReturnScalar<OtherDerived>;
using QuaternionProductType =
typename RxSO3Product<OtherDerived>::QuaternionType;
QuaternionProductType result_quaternion(
Sophus::SO3<double>::QuaternionProduct<QuaternionProductType>(
quaternion(), other.quaternion()));
ResultT scale = result_quaternion.squaredNorm();
if (scale < Constants<ResultT>::epsilon()) {
SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero.");
/// Saturation to ensure class invariant.
result_quaternion.normalize();
result_quaternion.coeffs() *= sqrt(Constants<ResultT>::epsilonPlus());
}
if (scale > ResultT(1.) / Constants<ResultT>::epsilon()) {
result_quaternion.normalize();
result_quaternion.coeffs() /= sqrt(Constants<ResultT>::epsilonPlus());
}
return RxSO3Product<OtherDerived>(result_quaternion);
}
/// Group action on 3-points.
///
/// This function rotates a 3 dimensional point ``p`` by the SO3 element
/// ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor
/// ``s``:
///
/// ``p_bar = s * (bar_R_foo * p_foo)``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 3>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
// Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459
Scalar scale = quaternion().squaredNorm();
PointProduct<PointDerived> two_vec_cross_p = quaternion().vec().cross(p);
two_vec_cross_p += two_vec_cross_p;
return scale * p + (quaternion().w() * two_vec_cross_p +
quaternion().vec().cross(two_vec_cross_p));
}
/// Group action on homogeneous 3-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 4>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const auto rsp = *this * p.template head<3>();
return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), rsp(2), p(3));
}
/// Group action on lines.
///
/// This function rotates a parametrized line ``l(t) = o + t * d`` by the SO3
/// element and scales it by the scale factor:
///
/// Origin ``o`` is rotated and scaled
/// Direction ``d`` is rotated (preserving it's norm)
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(),
(*this) * l.direction() / quaternion().squaredNorm());
}
/// Group action on planes.
///
/// This function rotates parametrized plane
/// ``n.x + d = 0`` by the SO3 element and scales it by the scale factor:
///
/// Normal vector ``n`` is rotated
/// Offset ``d`` is scaled
///
SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
const auto this_scale = scale();
return Hyperplane((*this) * p.normal() / this_scale,
this_scale * p.offset());
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SO3's Scalar type.
///
/// Note: This function performs saturation for products close to zero in
/// order to ensure the class invariant.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC RxSO3Base<Derived>& operator*=(
RxSO3Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns internal parameters of RxSO(3).
///
/// It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the
/// quaternion.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
return quaternion().coeffs();
}
/// Sets non-zero quaternion
///
/// Precondition: ``quat`` must not be close to either zero or infinity
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
SOPHUS_ENSURE(
quat.squaredNorm() < Scalar(1.) / (Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()),
"Inverse scale factor must be greater-equal epsilon.");
static_cast<Derived*>(this)->quaternion_nonconst() = quat;
}
/// Accessor of quaternion.
///
SOPHUS_FUNC QuaternionType const& quaternion() const {
return static_cast<Derived const*>(this)->quaternion();
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Transformation rotationMatrix() const {
QuaternionTemporaryType norm_quad = quaternion();
norm_quad.normalize();
return norm_quad.toRotationMatrix();
}
/// Returns scale.
///
SOPHUS_FUNC
Scalar scale() const { return quaternion().squaredNorm(); }
/// Setter of quaternion using rotation matrix ``R``, leaves scale as is.
///
SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
using std::sqrt;
Scalar saved_scale = scale();
quaternion_nonconst() = R;
quaternion_nonconst().coeffs() *= sqrt(saved_scale);
}
/// Sets scale and leaves rotation as is.
///
/// Note: This function as a significant computational cost, since it has to
/// call the square root twice.
///
SOPHUS_FUNC
void setScale(Scalar const& scale) {
using std::sqrt;
quaternion_nonconst().normalize();
quaternion_nonconst().coeffs() *= sqrt(scale);
}
/// Setter of quaternion using scaled rotation matrix ``sR``.
///
/// Precondition: The 3x3 matrix must be "scaled orthogonal"
/// and have a positive determinant.
///
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
Transformation squared_sR = sR * sR.transpose();
Scalar squared_scale =
Scalar(1. / 3.) *
(squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2));
SOPHUS_ENSURE(squared_scale >= Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
SOPHUS_ENSURE(squared_scale < Scalar(1.) / (Constants<Scalar>::epsilon() *
Constants<Scalar>::epsilon()),
"Inverse scale factor must be greater-equal epsilon.");
Scalar scale = sqrt(squared_scale);
quaternion_nonconst() = sR / scale;
quaternion_nonconst().coeffs() *= sqrt(scale);
}
/// Setter of SO(3) rotations, leaves scale as is.
///
SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
using std::sqrt;
Scalar saved_scale = scale();
quaternion_nonconst() = so3.unit_quaternion();
quaternion_nonconst().coeffs() *= sqrt(saved_scale);
}
SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
/// Returns derivative of this * RxSO3::exp(x) wrt. x at x=0
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
Eigen::Quaternion<Scalar> const q = quaternion();
J.col(3) = q.coeffs() * Scalar(0.5);
Scalar const c0 = Scalar(0.5) * q.w();
Scalar const c1 = Scalar(0.5) * q.z();
Scalar const c2 = -c1;
Scalar const c3 = Scalar(0.5) * q.y();
Scalar const c4 = Scalar(0.5) * q.x();
Scalar const c5 = -c4;
Scalar const c6 = -c3;
J(0, 0) = c0;
J(0, 1) = c2;
J(0, 2) = c3;
J(1, 0) = c1;
J(1, 1) = c0;
J(1, 2) = c5;
J(2, 0) = c6;
J(2, 1) = c4;
J(2, 2) = c0;
J(3, 0) = c5;
J(3, 1) = c6;
J(3, 2) = c2;
return J;
}
/// Returns derivative of log(this^{-1} * x) by x at x=this.
///
SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
const {
auto& q = quaternion();
Matrix<Scalar, DoF, num_parameters> J;
// clang-format off
J << q.w(), q.z(), -q.y(), -q.x(),
-q.z(), q.w(), q.x(), -q.y(),
q.y(), -q.x(), q.w(), -q.z(),
q.x(), q.y(), q.z(), q.w();
// clang-format on
const Scalar scaler = Scalar(2.) / q.squaredNorm();
return J * scaler;
}
private:
/// Mutator of quaternion is private to ensure class invariant.
///
SOPHUS_FUNC QuaternionType& quaternion_nonconst() {
return static_cast<Derived*>(this)->quaternion_nonconst();
}
};
/// RxSO3 using storage; derived from RxSO3Base.
template <class Scalar_, int Options>
class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
public:
using Base = RxSO3Base<RxSO3<Scalar_, Options>>;
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using QuaternionMember = Eigen::Quaternion<Scalar, Options>;
/// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
friend class RxSO3Base<RxSO3<Scalar_, Options>>;
using Base::operator=;
/// Define copy-assignment operator explicitly. The definition of
/// implicit copy assignment operator is deprecated in presence of a
/// user-declared copy constructor (-Wdeprecated-copy in clang >= 13).
SOPHUS_FUNC RxSO3& operator=(RxSO3 const& other) = default;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes quaternion to identity rotation and scale
/// to 1.
///
SOPHUS_FUNC RxSO3()
: quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
/// Copy constructor
///
SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
/// Copy-like constructor from OtherDerived
///
template <class OtherDerived>
SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
: quaternion_(other.quaternion()) {}
/// Constructor from scaled rotation matrix
///
/// Precondition: rotation matrix need to be scaled orthogonal with
/// determinant of ``s^3``.
///
SOPHUS_FUNC explicit RxSO3(Transformation const& sR) {
this->setScaledRotationMatrix(sR);
}
/// Constructor from scale factor and rotation matrix ``R``.
///
/// Precondition: Rotation matrix ``R`` must to be orthogonal with determinant
/// of 1 and ``scale`` must not be close to either zero or
/// infinity.
///
SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
: quaternion_(R) {
SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
SOPHUS_ENSURE(scale < Scalar(1.) / Constants<Scalar>::epsilon(),
"Inverse scale factor must be greater-equal epsilon.");
using std::sqrt;
quaternion_.coeffs() *= sqrt(scale);
}
/// Constructor from scale factor and SO3
///
/// Precondition: ``scale`` must not to be close to either zero or infinity.
///
SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
: quaternion_(so3.unit_quaternion()) {
SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
SOPHUS_ENSURE(scale < Scalar(1.) / Constants<Scalar>::epsilon(),
"Inverse scale factor must be greater-equal epsilon.");
using std::sqrt;
quaternion_.coeffs() *= sqrt(scale);
}
/// Constructor from quaternion
///
/// Precondition: quaternion must not be close to either zero or infinity.
///
template <class D>
SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
: quaternion_(quat) {
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type.");
SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants<Scalar>::epsilon(),
"Scale factor must be greater-equal epsilon.");
SOPHUS_ENSURE(
quat.squaredNorm() < Scalar(1.) / Constants<Scalar>::epsilon(),
"Inverse scale factor must be greater-equal epsilon.");
}
/// Constructor from scale factor and unit quaternion
///
/// Precondition: quaternion must not be close to zero.
///
template <class D>
SOPHUS_FUNC explicit RxSO3(Scalar const& scale,
Eigen::QuaternionBase<D> const& unit_quat)
: RxSO3(scale, SO3<Scalar>(unit_quat)) {}
/// Accessor of quaternion.
///
SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; }
/// Returns derivative of exp(x) wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
static Scalar const h(0.5);
return h * Sophus::Matrix<Scalar, num_parameters, DoF>::Identity();
}
/// Returns derivative of exp(x) wrt. x.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
const Tangent& a) {
using std::exp;
using std::sqrt;
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Vector3<Scalar> const omega = a.template head<3>();
Scalar const sigma = a[3];
Eigen::Quaternion<Scalar> quat = SO3<Scalar>::exp(omega).unit_quaternion();
Scalar const scale = sqrt(exp(sigma));
Scalar const scale_half = scale * Scalar(0.5);
J.template block<4, 3>(0, 0) = SO3<Scalar>::Dx_exp_x(omega) * scale;
J.col(3) = quat.coeffs() * scale_half;
return J;
}
/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 3, DoF> j;
j << Sophus::SO3<Scalar>::hat(-point), point;
return j;
}
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Group exponential
///
/// This functions takes in an element of tangent space (= rotation 3-vector
/// plus logarithm of scale) and returns the corresponding element of the
/// group RxSO3.
///
/// To be more specific, this function computes ``expmat(hat(omega))``
/// with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
/// hat()-operator of RSO3.
///
SOPHUS_FUNC static RxSO3<Scalar> exp(Tangent const& a) {
Scalar theta;
return expAndTheta(a, &theta);
}
/// As above, but also returns ``theta = |omega|`` as out-parameter.
///
/// Precondition: ``theta`` must not be ``nullptr``.
///
SOPHUS_FUNC static RxSO3<Scalar> expAndTheta(Tangent const& a,
Scalar* theta) {
SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
using std::exp;
using std::max;
using std::min;
using std::sqrt;
Vector3<Scalar> const omega = a.template head<3>();
Scalar sigma = a[3];
Scalar scale = exp(sigma);
// Ensure that scale-factor constraint is always valid
scale = max(scale, Constants<Scalar>::epsilonPlus());
scale = min(scale, Scalar(1.) / Constants<Scalar>::epsilonPlus());
Scalar sqrt_scale = sqrt(scale);
Eigen::Quaternion<Scalar> quat =
SO3<Scalar>::expAndTheta(omega, theta).unit_quaternion();
quat.coeffs() *= sqrt_scale;
return RxSO3<Scalar>(quat);
}
/// Returns the ith infinitesimal generators of ``R+ x SO(3)``.
///
/// The infinitesimal generators of RxSO3 are:
///
/// ```
/// | 0 0 0 |
/// G_0 = | 0 0 -1 |
/// | 0 1 0 |
///
/// | 0 0 1 |
/// G_1 = | 0 0 0 |
/// | -1 0 0 |
///
/// | 0 -1 0 |
/// G_2 = | 1 0 0 |
/// | 0 0 0 |
///
/// | 1 0 0 |
/// G_2 = | 0 1 0 |
/// | 0 0 1 |
/// ```
///
/// Precondition: ``i`` must be 0, 1, 2 or 3.
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 && i <= 3, "i should be in range [0,3].");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// hat-operator
///
/// It takes in the 4-vector representation ``a`` (= rotation vector plus
/// logarithm of scale) and returns the corresponding matrix representation
/// of Lie algebra element.
///
/// Formally, the hat()-operator of RxSO3 is defined as
///
/// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2,3)
///
/// with ``G_i`` being the ith infinitesimal generator of RxSO3.
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation A;
// clang-format off
A << a(3), -a(2), a(1),
a(2), a(3), -a(0),
-a(1), a(0), a(3);
// clang-format on
return A;
}
/// Lie bracket
///
/// It computes the Lie bracket of RxSO(3). To be more specific, it computes
///
/// ``[omega_1, omega_2]_rxso3 := vee([hat(omega_1), hat(omega_2)])``
///
/// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
/// hat()-operator and ``vee(.)`` the vee()-operator of RxSO3.
///
SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
Vector3<Scalar> const omega1 = a.template head<3>();
Vector3<Scalar> const omega2 = b.template head<3>();
Vector4<Scalar> res;
res.template head<3>() = omega1.cross(omega2);
res[3] = Scalar(0);
return res;
}
/// Draw uniform sample from RxSO(3) manifold.
///
/// The scale factor is drawn uniformly in log2-space from [-1, 1],
/// hence the scale is in [0.5, 2].
///
template <class UniformRandomBitGenerator>
static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
using std::exp2;
return RxSO3(exp2(uniform(generator)),
SO3<Scalar>::sampleUniform(generator));
}
/// vee-operator
///
/// It takes the 3x3-matrix representation ``Omega`` and maps it to the
/// corresponding vector representation of Lie algebra.
///
/// This is the inverse of the hat()-operator, see above.
///
/// Precondition: ``Omega`` must have the following structure:
///
/// | d -c b |
/// | c d -a |
/// | -b a d |
///
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
using std::abs;
return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0));
}
protected:
SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quaternion_; }
QuaternionMember quaternion_;
};
} // namespace Sophus
namespace Eigen {
/// Specialization of Eigen::Map for ``RxSO3``; derived from RxSO3Base
///
/// Allows us to wrap RxSO3 objects around POD array (e.g. external c style
/// quaternion).
template <class Scalar_, int Options>
class Map<Sophus::RxSO3<Scalar_>, Options>
: public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>> {
public:
using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
/// ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
friend class Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
using Base::operator=;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC explicit Map(Scalar* coeffs) : quaternion_(coeffs) {}
/// Accessor of quaternion.
///
SOPHUS_FUNC
Map<Eigen::Quaternion<Scalar>, Options> const& quaternion() const {
return quaternion_;
}
protected:
SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>& quaternion_nonconst() {
return quaternion_;
}
Map<Eigen::Quaternion<Scalar>, Options> quaternion_;
};
/// Specialization of Eigen::Map for ``RxSO3 const``; derived from RxSO3Base.
///
/// Allows us to wrap RxSO3 objects around POD array (e.g. external c style
/// quaternion).
template <class Scalar_, int Options>
class Map<Sophus::RxSO3<Scalar_> const, Options>
: public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>> {
public:
using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>>;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using Base::operator*=;
using Base::operator*;
SOPHUS_FUNC
explicit Map(Scalar const* coeffs) : quaternion_(coeffs) {}
/// Accessor of quaternion.
///
SOPHUS_FUNC
Map<Eigen::Quaternion<Scalar> const, Options> const& quaternion() const {
return quaternion_;
}
protected:
Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_;
};
} // namespace Eigen
================================================
FILE: Thirdparty/Sophus/sophus/se2.hpp
================================================
/// @file
/// Special Euclidean group SE(2) - rotation and translation in 2d.
#pragma once
#include "so2.hpp"
namespace Sophus {
template <class Scalar_, int Options = 0>
class SE2;
using SE2d = SE2<double>;
using SE2f = SE2<float>;
} // namespace Sophus
namespace Eigen {
namespace internal {
template <class Scalar_, int Options>
struct traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Sophus::Vector2<Scalar, Options>;
using SO2Type = Sophus::SO2<Scalar, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_>, Options>>
: traits<Sophus::SE2<Scalar_, Options>> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
};
template <class Scalar_, int Options>
struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
: traits<Sophus::SE2<Scalar_, Options> const> {
using Scalar = Scalar_;
using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
using SO2Type = Map<Sophus::SO2<Scalar> const, Options>;
};
} // namespace internal
} // namespace Eigen
namespace Sophus {
/// SE2 base type - implements SE2 class but is storage agnostic.
///
/// SE(2) is the group of rotations and translation in 2d. It is the
/// semi-direct product of SO(2) and the 2d Euclidean vector space. The class
/// is represented using a composition of SO2Group for rotation and a 2-vector
/// for translation.
///
/// SE(2) is neither compact, nor a commutative group.
///
/// See SO2Group for more details of the rotation representation in 2d.
///
template <class Derived>
class SE2Base {
public:
using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
using TranslationType =
typename Eigen::internal::traits<Derived>::TranslationType;
using SO2Type = typename Eigen::internal::traits<Derived>::SO2Type;
/// Degrees of freedom of manifold, number of dimensions in tangent space
/// (two for translation, three for rotation).
static int constexpr DoF = 3;
/// Number of internal parameters used (tuple for complex, two for
/// translation).
static int constexpr num_parameters = 4;
/// Group transformations are 3x3 matrices.
static int constexpr N = 3;
/// Points are 2-dimensional
static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>;
using Point = Vector2<Scalar>;
using HomogeneousPoint = Vector3<Scalar>;
using Line = ParametrizedLine2<Scalar>;
using Hyperplane = Hyperplane2<Scalar>;
using Tangent = Vector<Scalar, DoF>;
using Adjoint = Matrix<Scalar, DoF, DoF>;
/// For binary operations the return type is determined with the
/// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map
/// types, as well as other compatible scalar types such as Ceres::Jet and
/// double scalars with SE2 operations.
template <typename OtherDerived>
using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived>
using SE2Product = SE2<ReturnScalar<OtherDerived>>;
template <typename PointDerived>
using PointProduct = Vector2<ReturnScalar<PointDerived>>;
template <typename HPointDerived>
using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
/// Adjoint transformation
///
/// This function return the adjoint transformation ``Ad`` of the group
/// element ``A`` such that for all ``x`` it holds that
/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
///
SOPHUS_FUNC Adjoint Adj() const {
Matrix<Scalar, 2, 2> const& R = so2().matrix();
Transformation res;
res.setIdentity();
res.template topLeftCorner<2, 2>() = R;
res(0, 2) = translation()[1];
res(1, 2) = -translation()[0];
return res;
}
/// Returns copy of instance casted to NewScalarType.
///
template <class NewScalarType>
SOPHUS_FUNC SE2<NewScalarType> cast() const {
return SE2<NewScalarType>(so2().template cast<NewScalarType>(),
translation().template cast<NewScalarType>());
}
/// Returns derivative of this * exp(x) wrt x at x=0.
///
SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
const {
Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector2<Scalar> const c = unit_complex();
Scalar o(0);
J(0, 0) = o;
J(0, 1) = o;
J(0, 2) = -c[1];
J(1, 0) = o;
J(1, 1) = o;
J(1, 2) = c[0];
J(2, 0) = c[0];
J(2, 1) = -c[1];
J(2, 2) = o;
J(3, 0) = c[1];
J(3, 1) = c[0];
J(3, 2) = o;
return J;
}
/// Returns derivative of log(this^{-1} * x) by x at x=this.
///
SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
const {
Matrix<Scalar, DoF, num_parameters> J;
J.template block<2, 2>(0, 0).setZero();
J.template block<2, 2>(0, 2) = so2().inverse().matrix();
J.template block<1, 2>(2, 0) = so2().Dx_log_this_inv_by_x_at_this();
J.template block<1, 2>(2, 2).setZero();
return J;
}
/// Returns group inverse.
///
SOPHUS_FUNC SE2<Scalar> inverse() const {
SO2<Scalar> const invR = so2().inverse();
return SE2<Scalar>(invR, invR * (translation() * Scalar(-1)));
}
/// Logarithmic map
///
/// Computes the logarithm, the inverse of the group exponential which maps
/// element of the group (rigid body transformations) to elements of the
/// tangent space (twist).
///
/// To be specific, this function computes ``vee(logmat(.))`` with
/// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
/// of SE(2).
///
SOPHUS_FUNC Tangent log() const {
using std::abs;
Tangent upsilon_theta;
Scalar theta = so2().log();
upsilon_theta[2] = theta;
Scalar halftheta = Scalar(0.5) * theta;
Scalar halftheta_by_tan_of_halftheta;
Vector2<Scalar> z = so2().unit_complex();
Scalar real_minus_one = z.x() - Scalar(1.);
if (abs(real_minus_one) < Constants<Scalar>::epsilon()) {
halftheta_by_tan_of_halftheta =
Scalar(1.) - Scalar(1. / 12) * theta * theta;
} else {
halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);
}
Matrix<Scalar, 2, 2> V_inv;
V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta,
halftheta_by_tan_of_halftheta;
upsilon_theta.template head<2>() = V_inv * translation();
return upsilon_theta;
}
/// Normalize SO2 element
///
/// It re-normalizes the SO2 element.
///
SOPHUS_FUNC void normalize() { so2().normalize(); }
/// Returns 3x3 matrix representation of the instance.
///
/// It has the following form:
///
/// | R t |
/// | o 1 |
///
/// where ``R`` is a 2x2 rotation matrix, ``t`` a translation 2-vector and
/// ``o`` a 2-column vector of zeros.
///
SOPHUS_FUNC Transformation matrix() const {
Transformation homogeneous_matrix;
homogeneous_matrix.template topLeftCorner<2, 3>() = matrix2x3();
homogeneous_matrix.row(2) =
Matrix<Scalar, 1, 3>(Scalar(0), Scalar(0), Scalar(1));
return homogeneous_matrix;
}
/// Returns the significant first two rows of the matrix above.
///
SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
Matrix<Scalar, 2, 3> matrix;
matrix.template topLeftCorner<2, 2>() = rotationMatrix();
matrix.col(2) = translation();
return matrix;
}
/// Assignment-like operator from OtherDerived.
///
template <class OtherDerived>
SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const& other) {
so2() = other.so2();
translation() = other.translation();
return *this;
}
/// Group multiplication, which is rotation concatenation.
///
template <typename OtherDerived>
SOPHUS_FUNC SE2Product<OtherDerived> operator*(
SE2Base<OtherDerived> const& other) const {
return SE2Product<OtherDerived>(
so2() * other.so2(), translation() + so2() * other.translation());
}
/// Group action on 2-points.
///
/// This function rotates and translates a two dimensional point ``p`` by the
/// SE(2) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body
/// transformation):
///
/// ``p_bar = bar_R_foo * p_foo + t_bar``.
///
template <typename PointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<PointDerived, 2>::value>::type>
SOPHUS_FUNC PointProduct<PointDerived> operator*(
Eigen::MatrixBase<PointDerived> const& p) const {
return so2() * p + translation();
}
/// Group action on homogeneous 2-points. See above for more details.
///
template <typename HPointDerived,
typename = typename std::enable_if<
IsFixedSizeVector<HPointDerived, 3>::value>::type>
SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
Eigen::MatrixBase<HPointDerived> const& p) const {
const PointProduct<HPointDerived> tp =
so2() * p.template head<2>() + p(2) * translation();
return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2));
}
/// Group action on lines.
///
/// This function rotates and translates a parametrized line
/// ``l(t) = o + t * d`` by the SE(2) element:
///
/// Origin ``o`` is rotated and translated using SE(2) action
/// Direction ``d`` is rotated using SO(2) action
///
SOPHUS_FUNC Line operator*(Line const& l) const {
return Line((*this) * l.origin(), so2() * l.direction());
}
/// Group action on hyper-planes.
///
/// This function rotates a hyper-plane ``n.x + d = 0`` by the SE2
/// element:
///
/// Normal vector ``n`` is rotated
/// Offset ``d`` is adjusted for translation
///
/// Note that in 2d-case hyper-planes are just another parametrization of
/// lines
///
SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
Hyperplane const rotated = so2() * p;
return Hyperplane(rotated.normal(),
rotated.offset() - translation().dot(rotated.normal()));
}
/// In-place group multiplication. This method is only valid if the return
/// type of the multiplication is compatible with this SO2's Scalar type.
///
template <typename OtherDerived,
typename = typename std::enable_if<
std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const& other) {
*static_cast<Derived*>(this) = *this * other;
return *this;
}
/// Returns internal parameters of SE(2).
///
/// It returns (c[0], c[1], t[0], t[1]),
/// with c being the unit complex number, t the translation 3-vector.
///
SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
Sophus::Vector<Scalar, num_parameters> p;
p << so2().params(), translation();
return p;
}
/// Returns rotation matrix.
///
SOPHUS_FUNC Matrix<Scalar, 2, 2> rotationMatrix() const {
return so2().matrix();
}
/// Takes in complex number, and normalizes it.
///
/// Precondition: The complex number must not be close to zero.
///
SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
return so2().setComplex(complex);
}
/// Sets ``so3`` using ``rotation_matrix``.
///
/// Precondition: ``R`` must be orthogonal and ``det(R)=1``.
///
SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}",
SOPHUS_FMT_ARG(R));
SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",
SOPHUS_FMT_ARG(R.determinant()));
typename SO2Type::ComplexTemporaryType const complex(
Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1)));
so2().setComplex(complex);
}
/// Mutator of SO3 group.
///
SOPHUS_FUNC
SO2Type& so2() { return static_cast<Derived*>(this)->so2(); }
/// Accessor of SO3 group.
///
SOPHUS_FUNC
SO2Type const& so2() const {
return static_cast<Derived const*>(this)->so2();
}
/// Mutator of translation vector.
///
SOPHUS_FUNC
TranslationType& translation() {
return static_cast<Derived*>(this)->translation();
}
/// Accessor of translation vector
///
SOPHUS_FUNC
TranslationType const& translation() const {
return static_cast<Derived const*>(this)->translation();
}
/// Accessor of unit complex number.
///
SOPHUS_FUNC
typename Eigen::internal::traits<Derived>::SO2Type::ComplexT const&
unit_complex() const {
return so2().unit_complex();
}
};
/// SE2 using default storage; derived from SE2Base.
template <class Scalar_, int Options>
class SE2 : public SE2Base<SE2<Scalar_, Options>> {
public:
using Base = SE2Base<SE2<Scalar_, Options>>;
static int constexpr DoF = Base::DoF;
static int constexpr num_parameters = Base::num_parameters;
using Scalar = Scalar_;
using Transformation = typename Base::Transformation;
using Point = typename Base::Point;
using HomogeneousPoint = typename Base::HomogeneousPoint;
using Tangent = typename Base::Tangent;
using Adjoint = typename Base::Adjoint;
using SO2Member = SO2<Scalar, Options>;
using TranslationMember = Vector2<Scalar, Options>;
using Base::operator=;
/// Define copy-assignment operator explicitly. The definition of
/// implicit copy assignment operator is deprecated in presence of a
/// user-declared copy constructor (-Wdeprecated-copy in clang >= 13).
SOPHUS_FUNC SE2& operator=(SE2 const& other) = default;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Default constructor initializes rigid body motion to the identity.
///
SOPHUS_FUNC SE2();
/// Copy constructor
///
SOPHUS_FUNC SE2(SE2 const& other) = default;
/// Copy-like constructor from OtherDerived
///
template <class OtherDerived>
SOPHUS_FUNC SE2(SE2Base<OtherDerived> const& other)
: so2_(other.so2()), translation_(other.translation()) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from SO3 and translation vector
///
template <class OtherDerived, class D>
SOPHUS_FUNC SE2(SO2Base<OtherDerived> const& so2,
Eigen::MatrixBase<D> const& translation)
: so2_(so2), translation_(translation) {
static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
"must be same Scalar type");
static_assert(std::is_same<typename D::Scalar, Scalar>::value,
"must be same Scalar type");
}
/// Constructor from rotation matrix and translation vector
///
/// Precondition: Rotation matrix needs to be orthogonal with determinant
/// of 1.
///
SOPHUS_FUNC
SE2(typename SO2<Scalar>::Transformation const& rotation_matrix,
Point const& translation)
: so2_(rotation_matrix), translation_(translation) {}
/// Constructor from rotation angle and translation vector.
///
SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation)
: so2_(theta), translation_(translation) {}
/// Constructor from complex number and translation vector
///
/// Precondition: ``complex`` must not be close to zero.
SOPHUS_FUNC SE2(Vector2<Scalar> const& complex, Point const& translation)
: so2_(complex), translation_(translation) {}
/// Constructor from 3x3 matrix
///
/// Precondition: Rotation matrix needs to be orthogonal with determinant
/// of 1. The last row must be ``(0, 0, 1)``.
///
SOPHUS_FUNC explicit SE2(Transformation const& T)
: so2_(T.template topLeftCorner<2, 2>().eval()),
translation_(T.template block<2, 1>(0, 2)) {}
/// This provides unsafe read/write access to internal data. SO(2) is
/// represented by a complex number (two parameters). When using direct write
/// access, the user needs to take care of that the complex number stays
/// normalized.
///
SOPHUS_FUNC Scalar* data() {
// so2_ and translation_ are lay out sequentially with no padding
return so2_.data();
}
/// Const version of data() above.
///
SOPHUS_FUNC Scalar const* data() const {
/// so2_ and translation_ are lay out sequentially with no padding
return so2_.data();
}
/// Accessor of SO3
///
SOPHUS_FUNC SO2Member& so2() { return so2_; }
/// Mutator of SO3
///
SOPHUS_FUNC SO2Member const& so2() const { return so2_; }
/// Mutator of translation vector
///
SOPHUS_FUNC TranslationMember& translation() { return translation_; }
/// Accessor of translation vector
///
SOPHUS_FUNC TranslationMember const& translation() const {
return translation_;
}
/// Returns derivative of exp(x) wrt. x.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
Tangent const& upsilon_theta) {
using std::abs;
using std::cos;
using std::pow;
using std::sin;
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Sophus::Vector<Scalar, 2> upsilon = upsilon_theta.template head<2>();
Scalar theta = upsilon_theta[2];
if (abs(theta) < Constants<Scalar>::epsilon()) {
Scalar const o(0);
Scalar const i(1);
// clang-format off
J << o, o, o,
o, o, i,
i, o, -Scalar(0.5) * upsilon[1],
o, i, Scalar(0.5) * upsilon[0];
// clang-format on
return J;
}
Scalar const c0 = sin(theta);
Scalar const c1 = cos(theta);
Scalar const c2 = 1.0 / theta;
Scalar const c3 = c0 * c2;
Scalar const c4 = -c1 + Scalar(1);
Scalar const c5 = c2 * c4;
Scalar const c6 = c1 * c2;
Scalar const c7 = pow(theta, -2);
Scalar const c8 = c0 * c7;
Scalar const c9 = c4 * c7;
Scalar const o = Scalar(0);
J(0, 0) = o;
J(0, 1) = o;
J(0, 2) = -c0;
J(1, 0) = o;
J(1, 1) = o;
J(1, 2) = c1;
J(2, 0) = c3;
J(2, 1) = -c5;
J(2, 2) =
-c3 * upsilon[1] + c6 * upsilon[0] - c8 * upsilon[0] + c9 * upsilon[1];
J(3, 0) = c5;
J(3, 1) = c3;
J(3, 2) =
c3 * upsilon[0] + c6 * upsilon[1] - c8 * upsilon[1] - c9 * upsilon[0];
return J;
}
/// Returns derivative of exp(x) wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
Dx_exp_x_at_0() {
Sophus::Matrix<Scalar, num_parameters, DoF> J;
Scalar const o(0);
Scalar const i(1);
// clang-format off
J << o, o, o,
o, o, i,
i, o, o,
o, i, o;
// clang-format on
return J;
}
/// Returns derivative of exp(x) * p wrt. x_i at x=0.
///
SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
Point const& point) {
Sophus::Matrix<Scalar, 2, DoF> J;
J << Sophus::Matrix2<Scalar>::Identity(),
Sophus::SO2<Scalar>::Dx_exp_x_times_point_at_0(point);
return J;
}
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``.
///
SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
return generator(i);
}
/// Group exponential
///
/// This functions takes in an element of tangent space (= twist ``a``) and
/// returns the corresponding element of the group SE(2).
///
/// The first two components of ``a`` represent the translational part
/// ``upsilon`` in the tangent space of SE(2), while the last three components
/// of ``a`` represents the rotation vector ``omega``.
/// To be more specific, this function computes ``expmat(hat(a))`` with
/// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
/// of SE(2), see below.
///
SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
Scalar theta = a[2];
SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
Scalar sin_theta_by_theta;
Scalar one_minus_cos_theta_by_theta;
using std::abs;
if (abs(theta) < Constants<Scalar>::epsilon()) {
Scalar theta_sq = theta * theta;
sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq;
one_minus_cos_theta_by_theta =
Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq;
} else {
sin_theta_by_theta = so2.unit_complex().y() / theta;
one_minus_cos_theta_by_theta =
(Scalar(1.) - so2.unit_complex().x()) / theta;
}
Vector2<Scalar> trans(
sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1],
one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]);
return SE2<Scalar>(so2, trans);
}
/// Returns closest SE3 given arbitrary 4x4 matrix.
///
template <class S = Scalar>
static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
fitToSE2(Matrix3<Scalar> const& T) {
return SE2(SO2<Scalar>::fitToSO2(T.template block<2, 2>(0, 0)),
T.template block<2, 1>(0, 2));
}
/// Returns the ith infinitesimal generators of SE(2).
///
/// The infinitesimal generators of SE(2) are:
///
/// ```
/// | 0 0 1 |
/// G_0 = | 0 0 0 |
/// | 0 0 0 |
///
/// | 0 0 0 |
/// G_1 = | 0 0 1 |
/// | 0 0 0 |
///
/// | 0 -1 0 |
/// G_2 = | 1 0 0 |
/// | 0 0 0 |
/// ```
///
/// Precondition: ``i`` must be in 0, 1 or 2.
///
SOPHUS_FUNC static Transformation generator(int i) {
SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2].");
Tangent e;
e.setZero();
e[i] = Scalar(1);
return hat(e);
}
/// hat-operator
///
/// It takes in the 3-vector representation (= twist) and returns the
/// corresponding matrix representation of Lie algebra element.
///
/// Formally, the hat()-operator of SE(3) is defined as
///
/// ``hat(.): R^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2)
///
/// with ``G_i`` being the ith infinitesimal generator of SE(2).
///
/// The corresponding inverse is the vee()-operator, see below.
///
SOPHUS_FUNC static Transformation hat(Tangent const& a) {
Transformation Omega;
Omega.setZero();
Omega.template topLeftCorner<2, 2>() = SO2<Scalar>::hat(a[2]);
Omega.col(2).template head<2>() = a.template head<2>();
return Omega;
}
/// Lie bracket
///
/// It computes the Lie bracket of SE(2). To be more specific, it computes
///
/// ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])``
///
/// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the
/// hat()-operator and ``vee(.)`` the vee()-operator of SE(2).
///
SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
Vector2<Scalar> upsilon1 = a.template head<2>();
Vector2<Scalar> upsilon2 = b.template head<2>();
Scalar theta1 = a[2];
Scalar theta2 = b[2];
return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],
theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0));
}
/// Construct pure rotation.
///
static SOPHUS_FUNC SE2 rot(Scalar const& x) {
return SE2(SO2<Scalar>(x), Sophus::Vector2<Scalar>::Zero());
}
/// Draw uniform sample from SE(2) manifold.
///
/// Translations are drawn component-wise from the range [-1, 1].
///
template <class UniformRandomBitGenerator>
static SE2 sampleUniform(UniformRandomBitGenerator& generator) {
std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
return SE2(SO2<Scalar>::sampleUniform(generator),
Vector2<Scalar>(uniform(generator), uniform(generator)));
}
/// Construct a translation only SE(2) instance.
///
template <class T0, class T1>
static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
return SE2(SO2<Scalar>(), Vector2<Scalar>(x, y));
}
static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {
return SE2(SO2<Scalar>(), xy);
}
/// Construct x-axis translation.
///
static SOPHUS_FUNC SE2 transX(Scalar const& x) {
return SE2::trans(x, Scalar(0));
}
/// Construct y-axis translation.
///
static SOPHUS_FUNC SE2 transY(Scalar const& y) {
return SE2::trans(Scalar(0), y);
}
/// vee-operator
///
/// It takes the 3x3-matrix representation ``Omega`` and maps it to the
/// corresponding 3-vector representation of Lie algebra.
///
/// This is the inverse of the hat()-operator, see above.
///
/// Precondition: ``Omega`` must have the following structure:
///
/// | 0 -d a |
/// | d 0 b |
/// | 0 0 0 |
///
SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
SOPHUS_ENSURE(
Omega.row(2).template lpNorm<1>() < Constants<Scalar>::epsilon(),
"Omega: \n{}", SOPHUS_FMT_ARG(Omega));
Tangent upsilon_omega;
upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
return upsilon_omega;
}
protected:
SO2Member so2_;
TranslationMember translation_;
};
template <class Scalar, int Options>
SOPHUS_FUNC SE2<Scalar, Options>::SE2()
: translation_(TranslationMember::Zero()) {
static_assert(std::is_standard_layout<SE2>::v
gitextract_qkvnzuum/
├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── Thirdparty/
│ ├── Sophus/
│ │ ├── .clang-format
│ │ ├── CMakeLists.txt
│ │ ├── LICENSE.txt
│ │ ├── README.rst
│ │ ├── Sophus.code-workspace
│ │ ├── SophusConfig.cmake.in
│ │ ├── appveyor.yml
│ │ ├── cmake_modules/
│ │ │ └── FindEigen3.cmake
│ │ ├── doxyfile
│ │ ├── doxyrest-config.lua
│ │ ├── examples/
│ │ │ ├── CMakeLists.txt
│ │ │ └── HelloSO3.cpp
│ │ ├── generate_stubs.py
│ │ ├── make_docs.sh
│ │ ├── package.xml
│ │ ├── rst-dir/
│ │ │ ├── conf.py
│ │ │ ├── page_index.rst
│ │ │ └── pysophus.rst
│ │ ├── run_format.sh
│ │ ├── scripts/
│ │ │ ├── install_docs_deps.sh
│ │ │ ├── install_linux_deps.sh
│ │ │ ├── install_linux_fmt_deps.sh
│ │ │ ├── install_osx_deps.sh
│ │ │ └── run_cpp_tests.sh
│ │ ├── setup.py
│ │ ├── sophus/
│ │ │ ├── average.hpp
│ │ │ ├── cartesian.hpp
│ │ │ ├── ceres_local_parameterization.hpp
│ │ │ ├── ceres_manifold.hpp
│ │ │ ├── ceres_typetraits.hpp
│ │ │ ├── common.hpp
│ │ │ ├── example_ensure_handler.cpp
│ │ │ ├── geometry.hpp
│ │ │ ├── interpolate.hpp
│ │ │ ├── interpolate_details.hpp
│ │ │ ├── num_diff.hpp
│ │ │ ├── rotation_matrix.hpp
│ │ │ ├── rxso2.hpp
│ │ │ ├── rxso3.hpp
│ │ │ ├── se2.hpp
│ │ │ ├── se3.hpp
│ │ │ ├── sim2.hpp
│ │ │ ├── sim3.hpp
│ │ │ ├── sim_details.hpp
│ │ │ ├── so2.hpp
│ │ │ ├── so3.hpp
│ │ │ ├── spline.hpp
│ │ │ ├── test_macros.hpp
│ │ │ ├── types.hpp
│ │ │ └── velocities.hpp
│ │ ├── sophus_pybind/
│ │ │ ├── README
│ │ │ ├── SE3PyBind.h
│ │ │ ├── SO3PyBind.h
│ │ │ ├── SophusPyBind.h
│ │ │ ├── bindings.cpp
│ │ │ ├── examples/
│ │ │ │ └── sophus_quickstart_tutorial.ipynb
│ │ │ └── tests/
│ │ │ └── sophusPybindTests.py
│ │ ├── sophus_pybind-stubs/
│ │ │ ├── py.typed
│ │ │ └── sophus_pybind.pyi
│ │ ├── sympy/
│ │ │ ├── cpp_gencode/
│ │ │ │ ├── Se2_Dx_exp_x.cpp
│ │ │ │ ├── Se2_Dx_log_this.cpp
│ │ │ │ ├── Se2_Dx_this_mul_exp_x_at_0.cpp
│ │ │ │ ├── Se3_Dx_exp_x.cpp
│ │ │ │ ├── Se3_Dx_log_this.cpp
│ │ │ │ ├── Se3_Dx_this_mul_exp_x_at_0.cpp
│ │ │ │ ├── So2_Dx_exp_x.cpp
│ │ │ │ ├── So2_Dx_log_exp_x_times_this_at_0.cpp
│ │ │ │ ├── So2_Dx_log_this.cpp
│ │ │ │ ├── So2_Dx_this_mul_exp_x_at_0.cpp
│ │ │ │ ├── So3_Dx_exp_x.cpp
│ │ │ │ ├── So3_Dx_log_exp_x_times_this_at_0.cpp
│ │ │ │ ├── So3_Dx_log_this.cpp
│ │ │ │ └── So3_Dx_this_mul_exp_x_at_0.cpp
│ │ │ ├── run_tests.sh
│ │ │ └── sophus/
│ │ │ ├── __init__.py
│ │ │ ├── complex.py
│ │ │ ├── cse_codegen.py
│ │ │ ├── dual_quaternion.py
│ │ │ ├── matrix.py
│ │ │ ├── quaternion.py
│ │ │ ├── se2.py
│ │ │ ├── se3.py
│ │ │ ├── so2.py
│ │ │ └── so3.py
│ │ └── test/
│ │ ├── CMakeLists.txt
│ │ ├── ceres/
│ │ │ ├── CMakeLists.txt
│ │ │ ├── test_ceres_rxso2.cpp
│ │ │ ├── test_ceres_rxso3.cpp
│ │ │ ├── test_ceres_se2.cpp
│ │ │ ├── test_ceres_se3.cpp
│ │ │ ├── test_ceres_sim2.cpp
│ │ │ ├── test_ceres_sim3.cpp
│ │ │ ├── test_ceres_so2.cpp
│ │ │ ├── test_ceres_so3.cpp
│ │ │ └── tests.hpp
│ │ └── core/
│ │ ├── CMakeLists.txt
│ │ ├── test_cartesian2.cpp
│ │ ├── test_cartesian3.cpp
│ │ ├── test_common.cpp
│ │ ├── test_geometry.cpp
│ │ ├── test_rxso2.cpp
│ │ ├── test_rxso3.cpp
│ │ ├── test_se2.cpp
│ │ ├── test_se3.cpp
│ │ ├── test_sim2.cpp
│ │ ├── test_sim3.cpp
│ │ ├── test_so2.cpp
│ │ ├── test_so3.cpp
│ │ ├── test_velocities.cpp
│ │ └── tests.hpp
│ └── tessil-src/
│ ├── CMakeLists.txt
│ ├── LICENSE
│ ├── README.md
│ ├── appveyor.yml
│ ├── cmake/
│ │ └── tsl-robin-mapConfig.cmake.in
│ ├── doxygen.conf
│ ├── include/
│ │ └── tsl/
│ │ ├── robin_growth_policy.h
│ │ ├── robin_hash.h
│ │ ├── robin_map.h
│ │ └── robin_set.h
│ ├── tests/
│ │ ├── CMakeLists.txt
│ │ ├── custom_allocator_tests.cpp
│ │ ├── main.cpp
│ │ ├── policy_tests.cpp
│ │ ├── robin_map_tests.cpp
│ │ ├── robin_set_tests.cpp
│ │ └── utils.h
│ └── tsl-robin-map.natvis
├── config/
│ ├── avia.yaml
│ ├── horizon.yaml
│ ├── mid360.yaml
│ ├── ouster.yaml
│ ├── pandar.yaml
│ ├── robosense.yaml
│ └── velodyne.yaml
├── include/
│ ├── color.h
│ ├── common_lib.h
│ ├── ikd-Tree/
│ │ ├── README.md
│ │ ├── ikd_Tree.cpp
│ │ └── ikd_Tree.h
│ ├── matplotlibcpp.h
│ ├── scope_timer.hpp
│ └── so3_math.h
├── launch/
│ ├── hesai_pandarXT.launch
│ ├── livox_avia.launch
│ ├── livox_horizon.launch
│ ├── livox_mid360.launch
│ ├── ouster.launch
│ ├── robosense.launch
│ └── velodyne.launch
├── msg/
│ ├── Pose6D.msg
│ └── States.msg
├── package.xml
├── rviz_cfg/
│ ├── .gitignore
│ ├── fast_lo.rviz
│ └── spinning.rviz
└── src/
├── IMU_Processing.hpp
├── laserMapping.cpp
├── preprocess.cpp
└── preprocess.h
SYMBOL INDEX (1496 symbols across 83 files)
FILE: Thirdparty/Sophus/examples/HelloSO3.cpp
function main (line 4) | int main() {
FILE: Thirdparty/Sophus/setup.py
class CMakeExtension (line 23) | class CMakeExtension(Extension):
method __init__ (line 24) | def __init__(self, name, sourcedir=""):
class CMakeBuild (line 29) | class CMakeBuild(build_ext):
method build_extension (line 30) | def build_extension(self, ext):
function main (line 130) | def main():
FILE: Thirdparty/Sophus/sophus/average.hpp
type Sophus (line 19) | namespace Sophus {
function iterativeMean (line 26) | optional<typename SequenceContainer::value_type> iterativeMean(
function average (line 69) | enable_if_t<std::is_same<typename SequenceContainer::value_type,
function average (line 87) | enable_if_t<
function average (line 108) | enable_if_t<
type details (line 124) | namespace details {
function getUnitQuaternion (line 129) | Eigen::Quaternion<Scalar> getUnitQuaternion(SO3<Scalar> const& R) {
function getUnitQuaternion (line 134) | Eigen::Quaternion<Scalar> getUnitQuaternion(RxSO3<Scalar> const& sR) {
function averageUnitQuaternion (line 140) | Eigen::Quaternion<Scalar> averageUnitQuaternion(
function average (line 182) | enable_if_t<
function average (line 192) | enable_if_t<
function average (line 211) | enable_if_t<
function average (line 222) | enable_if_t<
function average (line 231) | enable_if_t<
function average (line 240) | enable_if_t<
FILE: Thirdparty/Sophus/sophus/cartesian.hpp
type Sophus (line 7) | namespace Sophus {
class Cartesian (line 9) | class Cartesian
class CartesianBase (line 77) | class CartesianBase {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
method SOPHUS_FUNC (line 123) | SOPHUS_FUNC Cartesian<NewScalarType, M> cast() const {
method Dx_this_mul_exp_x_at_0 (line 129) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 138) | Dx_log_this_inv_by_x_at_this()
method inverse (line 149) | inverse() const {
method SOPHUS_FUNC (line 158) | SOPHUS_FUNC Tangent log() const { return params(); }
method matrix (line 167) | matrix() const {
method SOPHUS_FUNC (line 177) | SOPHUS_FUNC CartesianBase<Derived, M>& operator=(
method SOPHUS_FUNC (line 186) | SOPHUS_FUNC CartesianSum<OtherDerived> operator*(
method SOPHUS_FUNC (line 196) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 206) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 216) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 233) | SOPHUS_FUNC CartesianBase<Derived, M>& operator*=(
method SOPHUS_FUNC (line 241) | SOPHUS_FUNC ParamsType& params() {
method SOPHUS_FUNC (line 247) | SOPHUS_FUNC ParamsType const& params() const {
function SOPHUS_FUNC (line 275) | SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default;
function Cartesian (line 281) | Cartesian() { params_.setZero(); }
function SOPHUS_FUNC (line 285) | SOPHUS_FUNC Cartesian(Cartesian const& other) = default;
type Eigen (line 22) | namespace Eigen {
type internal (line 23) | namespace internal {
type traits<Sophus::Cartesian<Scalar_, M, Options>> (line 26) | struct traits<Sophus::Cartesian<Scalar_, M, Options>> {
type traits<Map<Sophus::Cartesian<Scalar_, M>, Options>> (line 32) | struct traits<Map<Sophus::Cartesian<Scalar_, M>, Options>>
type traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>> (line 39) | struct traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>>
class Map<Sophus::Cartesian<Scalar_, M>, Options> (line 458) | class Map<Sophus::Cartesian<Scalar_, M>, Options>
method Map (line 474) | Map(Scalar* coeffs) : params_(coeffs) {}
method SOPHUS_FUNC (line 478) | SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>>& params() {
type Sophus (line 47) | namespace Sophus {
class Cartesian (line 9) | class Cartesian
class CartesianBase (line 77) | class CartesianBase {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
method SOPHUS_FUNC (line 123) | SOPHUS_FUNC Cartesian<NewScalarType, M> cast() const {
method Dx_this_mul_exp_x_at_0 (line 129) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 138) | Dx_log_this_inv_by_x_at_this()
method inverse (line 149) | inverse() const {
method SOPHUS_FUNC (line 158) | SOPHUS_FUNC Tangent log() const { return params(); }
method matrix (line 167) | matrix() const {
method SOPHUS_FUNC (line 177) | SOPHUS_FUNC CartesianBase<Derived, M>& operator=(
method SOPHUS_FUNC (line 186) | SOPHUS_FUNC CartesianSum<OtherDerived> operator*(
method SOPHUS_FUNC (line 196) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 206) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 216) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 233) | SOPHUS_FUNC CartesianBase<Derived, M>& operator*=(
method SOPHUS_FUNC (line 241) | SOPHUS_FUNC ParamsType& params() {
method SOPHUS_FUNC (line 247) | SOPHUS_FUNC ParamsType const& params() const {
function SOPHUS_FUNC (line 275) | SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default;
function Cartesian (line 281) | Cartesian() { params_.setZero(); }
function SOPHUS_FUNC (line 285) | SOPHUS_FUNC Cartesian(Cartesian const& other) = default;
function SOPHUS_FUNC (line 314) | SOPHUS_FUNC Scalar* data() { return params_.data(); }
function SOPHUS_FUNC (line 318) | SOPHUS_FUNC Scalar const* data() const { return params_.data(); }
function SOPHUS_FUNC (line 322) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 331) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 338) | SOPHUS_FUNC static Sophus::Matrix<Scalar, Dim, DoF> Dx_exp_x_times_point...
function SOPHUS_FUNC (line 347) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 353) | SOPHUS_FUNC ParamsMember& params() { return params_; }
function SOPHUS_FUNC (line 357) | SOPHUS_FUNC ParamsMember const& params() const { return params_; }
function SOPHUS_FUNC (line 382) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 396) | SOPHUS_FUNC static Cartesian<Scalar, M> exp(Tangent const& a) {
function SOPHUS_FUNC (line 411) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 421) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
function Cartesian (line 428) | static Cartesian sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 441) | SOPHUS_FUNC static Tangent vee(Transformation const& m) {
type Eigen (line 451) | namespace Eigen {
type internal (line 23) | namespace internal {
type traits<Sophus::Cartesian<Scalar_, M, Options>> (line 26) | struct traits<Sophus::Cartesian<Scalar_, M, Options>> {
type traits<Map<Sophus::Cartesian<Scalar_, M>, Options>> (line 32) | struct traits<Map<Sophus::Cartesian<Scalar_, M>, Options>>
type traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>> (line 39) | struct traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>>
class Map<Sophus::Cartesian<Scalar_, M>, Options> (line 458) | class Map<Sophus::Cartesian<Scalar_, M>, Options>
method Map (line 474) | Map(Scalar* coeffs) : params_(coeffs) {}
method SOPHUS_FUNC (line 478) | SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>>& params() {
class Map<Sophus::Cartesian<Scalar_, M> const, Options> (line 497) | class Map<Sophus::Cartesian<Scalar_, M> const, Options>
method SOPHUS_FUNC (line 512) | SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {}
FILE: Thirdparty/Sophus/sophus/ceres_local_parameterization.hpp
type Sophus (line 7) | namespace Sophus {
class LocalParameterization (line 12) | class LocalParameterization : public ceres::LocalParameterization {
method Plus (line 24) | bool Plus(double const* T_raw, double const* delta_raw,
method ComputeJacobian (line 37) | bool ComputeJacobian(double const* T_raw,
method GlobalSize (line 47) | int GlobalSize() const override { return LieGroupd::num_parameters; }
method LocalSize (line 49) | int LocalSize() const override { return LieGroupd::DoF; }
FILE: Thirdparty/Sophus/sophus/ceres_manifold.hpp
type Sophus (line 6) | namespace Sophus {
class Manifold (line 11) | class Manifold : public ceres::Manifold {
method Plus (line 24) | bool Plus(double const* T_raw, double const* delta_raw,
method PlusJacobian (line 37) | bool PlusJacobian(double const* T_raw, double* jacobian_raw) const o...
method Minus (line 46) | bool Minus(double const* y_raw, double const* x_raw,
method MinusJacobian (line 55) | bool MinusJacobian(double const* x_raw, double* jacobian_raw) const ...
method AmbientSize (line 63) | int AmbientSize() const override { return LieGroupd::num_parameters; }
method TangentSize (line 65) | int TangentSize() const override { return LieGroupd::DoF; }
FILE: Thirdparty/Sophus/sophus/ceres_typetraits.hpp
type Sophus (line 4) | namespace Sophus {
type Mapper (line 35) | struct Mapper {
method Map (line 40) | static Map map(Scalar* ptr) noexcept { return *ptr; }
method ConstMap (line 41) | static ConstMap map(const Scalar* ptr) noexcept { return *ptr; }
type Mapper<T, typename std::enable_if<IsMappableV<T>>::type> (line 45) | struct Mapper<T, typename std::enable_if<IsMappableV<T>>::type> {
method Map (line 50) | static Map map(Scalar* ptr) noexcept { return Map(ptr); }
method ConstMap (line 51) | static ConstMap map(const Scalar* ptr) noexcept { return ConstMap(pt...
FILE: Thirdparty/Sophus/sophus/common.hpp
type Sophus (line 107) | namespace Sophus {
type Constants (line 156) | struct Constants {
method SOPHUS_FUNC (line 157) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 159) | SOPHUS_FUNC static Scalar epsilonPlus() {
method SOPHUS_FUNC (line 163) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 168) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 174) | struct Constants<float> {
method epsilon (line 175) | constexpr epsilon() {
method SOPHUS_FUNC (line 178) | SOPHUS_FUNC static float epsilonPlus() {
method SOPHUS_FUNC (line 182) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 184) | constexpr pi() {
type nullopt_t (line 190) | struct nullopt_t {
method nullopt_t (line 191) | explicit constexpr nullopt_t() {}
class optional (line 202) | class optional {
method optional (line 204) | optional() : is_valid_(false) {}
method optional (line 206) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 208) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 212) | T const* operator->() const {
method T (line 217) | T* operator->() {
method T (line 222) | T const& operator*() const {
method T (line 227) | T& operator*() {
type IsUniformRandomBitGenerator (line 241) | struct IsUniformRandomBitGenerator {
type Sophus (line 153) | namespace Sophus {
type Constants (line 156) | struct Constants {
method SOPHUS_FUNC (line 157) | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); }
method SOPHUS_FUNC (line 159) | SOPHUS_FUNC static Scalar epsilonPlus() {
method SOPHUS_FUNC (line 163) | SOPHUS_FUNC static Scalar epsilonSqrt() {
method SOPHUS_FUNC (line 168) | SOPHUS_FUNC static Scalar pi() {
type Constants<float> (line 174) | struct Constants<float> {
method epsilon (line 175) | constexpr epsilon() {
method SOPHUS_FUNC (line 178) | SOPHUS_FUNC static float epsilonPlus() {
method SOPHUS_FUNC (line 182) | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); }
method pi (line 184) | constexpr pi() {
type nullopt_t (line 190) | struct nullopt_t {
method nullopt_t (line 191) | explicit constexpr nullopt_t() {}
class optional (line 202) | class optional {
method optional (line 204) | optional() : is_valid_(false) {}
method optional (line 206) | optional(nullopt_t) : is_valid_(false) {}
method optional (line 208) | optional(T const& type) : type_(type), is_valid_(true) {}
method T (line 212) | T const* operator->() const {
method T (line 217) | T* operator->() {
method T (line 222) | T const& operator*() const {
method T (line 227) | T& operator*() {
type IsUniformRandomBitGenerator (line 241) | struct IsUniformRandomBitGenerator {
FILE: Thirdparty/Sophus/sophus/example_ensure_handler.cpp
type Sophus (line 6) | namespace Sophus {
function ensureFailed (line 7) | void ensureFailed(char const* function, char const* file, int line,
FILE: Thirdparty/Sophus/sophus/geometry.hpp
type Sophus (line 12) | namespace Sophus {
function normalFromSO2 (line 18) | Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
function SO2FromNormal (line 28) | SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
function normalFromSO3 (line 40) | Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
function rotationFromNormal (line 57) | Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
function SO3FromNormal (line 116) | SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
function lineFromSE2 (line 126) | Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
function SE2FromLine (line 135) | SE2<T> SE2FromLine(Line2<T> const& line_foo) {
function planeFromSE3 (line 148) | Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
function SE3FromPlane (line 157) | SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
function makeHyperplaneUnique (line 168) | Eigen::Hyperplane<T, N> makeHyperplaneUnique(
FILE: Thirdparty/Sophus/sophus/interpolate.hpp
type Sophus (line 10) | namespace Sophus {
function interpolate (line 26) | enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
FILE: Thirdparty/Sophus/sophus/interpolate_details.hpp
type Sophus (line 13) | namespace Sophus {
type interp_details (line 14) | namespace interp_details {
type Traits (line 17) | struct Traits
type Traits<Cartesian<Scalar, Dim>> (line 20) | struct Traits<Cartesian<Scalar, Dim>> {
method hasShortestPathAmbiguity (line 23) | static bool hasShortestPathAmbiguity(Cartesian<Scalar, Dim> const&) {
type Traits<SO2<Scalar>> (line 29) | struct Traits<SO2<Scalar>> {
method hasShortestPathAmbiguity (line 32) | static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {
type Traits<RxSO2<Scalar>> (line 41) | struct Traits<RxSO2<Scalar>> {
method hasShortestPathAmbiguity (line 44) | static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_ba...
type Traits<SO3<Scalar>> (line 50) | struct Traits<SO3<Scalar>> {
method hasShortestPathAmbiguity (line 53) | static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {
type Traits<RxSO3<Scalar>> (line 62) | struct Traits<RxSO3<Scalar>> {
method hasShortestPathAmbiguity (line 65) | static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_ba...
type Traits<SE2<Scalar>> (line 71) | struct Traits<SE2<Scalar>> {
method hasShortestPathAmbiguity (line 74) | static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {
type Traits<SE3<Scalar>> (line 80) | struct Traits<SE3<Scalar>> {
method hasShortestPathAmbiguity (line 83) | static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {
type Traits<Sim2<Scalar>> (line 89) | struct Traits<Sim2<Scalar>> {
method hasShortestPathAmbiguity (line 92) | static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {
type Traits<Sim3<Scalar>> (line 100) | struct Traits<Sim3<Scalar>> {
method hasShortestPathAmbiguity (line 103) | static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {
FILE: Thirdparty/Sophus/sophus/num_diff.hpp
type Sophus (line 12) | namespace Sophus {
type details (line 14) | namespace details {
class Curve (line 16) | class Curve {
method num_diff (line 19) | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(cur...
class VectorField (line 32) | class VectorField {
method num_diff (line 34) | static Eigen::Matrix<Scalar, N, M> num_diff(
class VectorField<Scalar, N, 1> (line 55) | class VectorField<Scalar, N, 1> {
method num_diff (line 57) | static Eigen::Matrix<Scalar, N, 1> num_diff(
function curveNumDiff (line 71) | auto curveNumDiff(Fn curve, Scalar t,
function vectorFieldNumDiff (line 83) | Eigen::Matrix<Scalar, N, M> vectorFieldNumDiff(
FILE: Thirdparty/Sophus/sophus/rotation_matrix.hpp
type Sophus (line 11) | namespace Sophus {
function SOPHUS_FUNC (line 16) | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
function SOPHUS_FUNC (line 32) | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> co...
function SOPHUS_FUNC (line 58) | SOPHUS_FUNC enable_if_t<
FILE: Thirdparty/Sophus/sophus/rxso2.hpp
type Sophus (line 8) | namespace Sophus {
class RxSO2 (line 10) | class RxSO2
method SOPHUS_FUNC (line 446) | SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
method RxSO2 (line 456) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 460) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 465) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 483) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 490) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 497) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 517) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 522) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_ex...
function setComplex (line 79) | class RxSO2Base {
function SOPHUS_FUNC (line 356) | SOPHUS_FUNC ComplexType const& complex() const {
function SOPHUS_FUNC (line 362) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 370) | SOPHUS_FUNC Scalar scale() const {
function SOPHUS_FUNC (line 377) | SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(th...
function SOPHUS_FUNC (line 383) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 389) | SOPHUS_FUNC void setScale(Scalar const& scale) {
function SOPHUS_FUNC (line 400) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 408) | SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
function SOPHUS_FUNC (line 415) | SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
class RxSO2 (line 427) | class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
method SOPHUS_FUNC (line 446) | SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
method RxSO2 (line 456) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 460) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 465) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 483) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 490) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 497) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 517) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 522) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_ex...
function SOPHUS_FUNC (line 561) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 575) | SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 605) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 627) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 645) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
function RxSO2 (line 657) | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 446) | SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
method RxSO2 (line 456) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 460) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 465) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 483) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 490) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 497) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 517) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 522) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_ex...
function SOPHUS_FUNC (line 676) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 15) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::RxSO2<Scalar_, Options_>> (line 19) | struct traits<Sophus::RxSO2<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO2<Scalar_>, Options_>> (line 26) | struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO2<Scalar_> const, Options_>> (line 34) | struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
class Map<Sophus::RxSO2<Scalar_>, Options> (line 696) | class Map<Sophus::RxSO2<Scalar_>, Options>
method Map (line 715) | Map(Scalar* coeffs) : complex_(coeffs) {}
type Sophus (line 43) | namespace Sophus {
class RxSO2 (line 10) | class RxSO2
method SOPHUS_FUNC (line 446) | SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
method RxSO2 (line 456) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 460) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 465) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 483) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 490) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 497) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 517) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 522) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_ex...
function setComplex (line 79) | class RxSO2Base {
function SOPHUS_FUNC (line 356) | SOPHUS_FUNC ComplexType const& complex() const {
function SOPHUS_FUNC (line 362) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 370) | SOPHUS_FUNC Scalar scale() const {
function SOPHUS_FUNC (line 377) | SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(th...
function SOPHUS_FUNC (line 383) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 389) | SOPHUS_FUNC void setScale(Scalar const& scale) {
function SOPHUS_FUNC (line 400) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 408) | SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
function SOPHUS_FUNC (line 415) | SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }
class RxSO2 (line 427) | class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
method SOPHUS_FUNC (line 446) | SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
method RxSO2 (line 456) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 460) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 465) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 483) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 490) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 497) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 517) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 522) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_ex...
function SOPHUS_FUNC (line 561) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 575) | SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 605) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 627) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 645) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
function RxSO2 (line 657) | static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 446) | SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
method RxSO2 (line 456) | RxSO2() : complex_(Scalar(1), Scalar(0)) {}
method SOPHUS_FUNC (line 460) | SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
method SOPHUS_FUNC (line 465) | SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 483) | SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
method SOPHUS_FUNC (line 490) | SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
method RxSO2 (line 497) | RxSO2(Vector2<Scalar> const& z) : complex_(z) {
method RxSO2 (line 517) | RxSO2(Scalar const& real, Scalar const& imag)
method SOPHUS_FUNC (line 522) | SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
method SOPHUS_FUNC (line 526) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_ex...
function SOPHUS_FUNC (line 676) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 689) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::RxSO2<Scalar_, Options_>> (line 19) | struct traits<Sophus::RxSO2<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO2<Scalar_>, Options_>> (line 26) | struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO2<Scalar_> const, Options_>> (line 34) | struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
class Map<Sophus::RxSO2<Scalar_>, Options> (line 696) | class Map<Sophus::RxSO2<Scalar_>, Options>
method Map (line 715) | Map(Scalar* coeffs) : complex_(coeffs) {}
class Map<Sophus::RxSO2<Scalar_> const, Options> (line 737) | class Map<Sophus::RxSO2<Scalar_> const, Options>
method Map (line 752) | Map(Scalar const* coeffs) : complex_(coeffs) {}
FILE: Thirdparty/Sophus/sophus/rxso3.hpp
type Sophus (line 8) | namespace Sophus {
class RxSO3 (line 10) | class RxSO3
function setQuaternion (line 70) | class RxSO3Base {
function SOPHUS_FUNC (line 365) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 371) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 379) | SOPHUS_FUNC
function SOPHUS_FUNC (line 384) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 396) | SOPHUS_FUNC
function SOPHUS_FUNC (line 408) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 426) | SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
function SOPHUS_FUNC (line 433) | SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
function SOPHUS_FUNC (line 437) | SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 466) | SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_a...
function Dxi_exp_x_matrix_at_0 (line 490) | class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
function SOPHUS_FUNC (line 650) | SOPHUS_FUNC static RxSO3<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 659) | SOPHUS_FUNC static RxSO3<Scalar> expAndTheta(Tangent const& a,
function SOPHUS_FUNC (line 704) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 726) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 745) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const&...
function RxSO3 (line 760) | static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 780) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 15) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::RxSO3<Scalar_, Options_>> (line 19) | struct traits<Sophus::RxSO3<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO3<Scalar_>, Options_>> (line 26) | struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO3<Scalar_> const, Options_>> (line 34) | struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
class Map<Sophus::RxSO3<Scalar_>, Options> (line 800) | class Map<Sophus::RxSO3<Scalar_>, Options>
method Map (line 818) | Map(Scalar* coeffs) : quaternion_(coeffs) {}
type Sophus (line 43) | namespace Sophus {
class RxSO3 (line 10) | class RxSO3
function setQuaternion (line 70) | class RxSO3Base {
function SOPHUS_FUNC (line 365) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 371) | SOPHUS_FUNC Transformation rotationMatrix() const {
function SOPHUS_FUNC (line 379) | SOPHUS_FUNC
function SOPHUS_FUNC (line 384) | SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
function SOPHUS_FUNC (line 396) | SOPHUS_FUNC
function SOPHUS_FUNC (line 408) | SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
function SOPHUS_FUNC (line 426) | SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
function SOPHUS_FUNC (line 433) | SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
function SOPHUS_FUNC (line 437) | SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 466) | SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_a...
function Dxi_exp_x_matrix_at_0 (line 490) | class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
function SOPHUS_FUNC (line 650) | SOPHUS_FUNC static RxSO3<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 659) | SOPHUS_FUNC static RxSO3<Scalar> expAndTheta(Tangent const& a,
function SOPHUS_FUNC (line 704) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 726) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 745) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const&...
function RxSO3 (line 760) | static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 780) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 793) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::RxSO3<Scalar_, Options_>> (line 19) | struct traits<Sophus::RxSO3<Scalar_, Options_>> {
type traits<Map<Sophus::RxSO3<Scalar_>, Options_>> (line 26) | struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
type traits<Map<Sophus::RxSO3<Scalar_> const, Options_>> (line 34) | struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
class Map<Sophus::RxSO3<Scalar_>, Options> (line 800) | class Map<Sophus::RxSO3<Scalar_>, Options>
method Map (line 818) | Map(Scalar* coeffs) : quaternion_(coeffs) {}
class Map<Sophus::RxSO3<Scalar_> const, Options> (line 840) | class Map<Sophus::RxSO3<Scalar_> const, Options>
method Map (line 855) | Map(Scalar const* coeffs) : quaternion_(coeffs) {}
FILE: Thirdparty/Sophus/sophus/se2.hpp
type Sophus (line 8) | namespace Sophus {
class SE2 (line 10) | class SE2
class SE2Base (line 57) | class SE2Base {
method SOPHUS_FUNC (line 105) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC SE2<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 125) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 147) | Dx_log_this_inv_by_x_at_this()
method inverse (line 159) | inverse() const {
method SOPHUS_FUNC (line 174) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 202) | SOPHUS_FUNC void normalize() { so2().normalize(); }
method matrix (line 214) | matrix() const {
method matrix2x3 (line 224) | matrix2x3() const {
method SOPHUS_FUNC (line 234) | SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const&...
method SOPHUS_FUNC (line 243) | SOPHUS_FUNC SE2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 260) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 270) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 285) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 312) | SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const...
function SOPHUS_FUNC (line 338) | SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
function SOPHUS_FUNC (line 346) | SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC
function SOPHUS_FUNC (line 363) | SOPHUS_FUNC
function SOPHUS_FUNC (line 370) | SOPHUS_FUNC
function SOPHUS_FUNC (line 377) | SOPHUS_FUNC
function SOPHUS_FUNC (line 384) | SOPHUS_FUNC
type Eigen (line 15) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::SE2<Scalar_, Options>> (line 19) | struct traits<Sophus::SE2<Scalar_, Options>> {
type traits<Map<Sophus::SE2<Scalar_>, Options>> (line 26) | struct traits<Map<Sophus::SE2<Scalar_>, Options>>
type traits<Map<Sophus::SE2<Scalar_> const, Options>> (line 34) | struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
class Map<Sophus::SE2<Scalar_>, Options> (line 800) | class Map<Sophus::SE2<Scalar_>, Options>
method Map (line 816) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 822) | SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
function SOPHUS_FUNC (line 838) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
type Sophus (line 43) | namespace Sophus {
class SE2 (line 10) | class SE2
class SE2Base (line 57) | class SE2Base {
method SOPHUS_FUNC (line 105) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 118) | SOPHUS_FUNC SE2<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 125) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 147) | Dx_log_this_inv_by_x_at_this()
method inverse (line 159) | inverse() const {
method SOPHUS_FUNC (line 174) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 202) | SOPHUS_FUNC void normalize() { so2().normalize(); }
method matrix (line 214) | matrix() const {
method matrix2x3 (line 224) | matrix2x3() const {
method SOPHUS_FUNC (line 234) | SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const&...
method SOPHUS_FUNC (line 243) | SOPHUS_FUNC SE2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 260) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 270) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 285) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 312) | SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const...
function SOPHUS_FUNC (line 338) | SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {
function SOPHUS_FUNC (line 346) | SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC
function SOPHUS_FUNC (line 363) | SOPHUS_FUNC
function SOPHUS_FUNC (line 370) | SOPHUS_FUNC
function SOPHUS_FUNC (line 377) | SOPHUS_FUNC
function SOPHUS_FUNC (line 384) | SOPHUS_FUNC
function SOPHUS_FUNC (line 413) | SOPHUS_FUNC SE2& operator=(SE2 const& other) = default;
function SOPHUS_FUNC (line 423) | SOPHUS_FUNC SE2(SE2 const& other) = default;
function SOPHUS_FUNC (line 481) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 488) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 495) | SOPHUS_FUNC SO2Member& so2() { return so2_; }
function SOPHUS_FUNC (line 499) | SOPHUS_FUNC SO2Member const& so2() const { return so2_; }
function SOPHUS_FUNC (line 503) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 507) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 513) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 567) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 584) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_a...
function SOPHUS_FUNC (line 594) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 610) | SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 636) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SE2>
function SOPHUS_FUNC (line 662) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 683) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 700) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function SOPHUS_FUNC (line 712) | static SOPHUS_FUNC SE2 rot(Scalar const& x) {
function SE2 (line 721) | static SE2 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 730) | static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
function SOPHUS_FUNC (line 734) | static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {
function SOPHUS_FUNC (line 740) | static SOPHUS_FUNC SE2 transX(Scalar const& x) {
function SOPHUS_FUNC (line 746) | static SOPHUS_FUNC SE2 transY(Scalar const& y) {
function SOPHUS_FUNC (line 763) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
function SOPHUS_FUNC (line 779) | SOPHUS_FUNC SE2<Scalar, Options>::SE2()
type Eigen (line 794) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::SE2<Scalar_, Options>> (line 19) | struct traits<Sophus::SE2<Scalar_, Options>> {
type traits<Map<Sophus::SE2<Scalar_>, Options>> (line 26) | struct traits<Map<Sophus::SE2<Scalar_>, Options>>
type traits<Map<Sophus::SE2<Scalar_> const, Options>> (line 34) | struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
class Map<Sophus::SE2<Scalar_>, Options> (line 800) | class Map<Sophus::SE2<Scalar_>, Options>
method Map (line 816) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 822) | SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
function SOPHUS_FUNC (line 838) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
class Map<Sophus::SE2<Scalar_> const, Options> (line 851) | class Map<Sophus::SE2<Scalar_> const, Options>
method Map (line 865) | Map(Scalar const* coeffs)
FILE: Thirdparty/Sophus/sophus/se3.hpp
type Sophus (line 8) | namespace Sophus {
class SE3 (line 10) | class SE3
class SE3Base (line 57) | class SE3Base {
method SOPHUS_FUNC (line 105) | SOPHUS_FUNC Adjoint Adj() const {
method Scalar (line 117) | Scalar angleX() const { return so3().angleX(); }
method Scalar (line 121) | Scalar angleY() const { return so3().angleY(); }
method Scalar (line 125) | Scalar angleZ() const { return so3().angleZ(); }
method SOPHUS_FUNC (line 130) | SOPHUS_FUNC SE3<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 137) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 210) | Dx_log_this_inv_by_x_at_this()
method inverse (line 222) | inverse() const {
method SOPHUS_FUNC (line 237) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 261) | SOPHUS_FUNC void normalize() { so3().normalize(); }
method matrix (line 273) | matrix() const {
method matrix3x4 (line 283) | matrix3x4() const {
method SOPHUS_FUNC (line 293) | SOPHUS_FUNC SE3Base<Derived>& operator=(SE3Base<OtherDerived> const&...
method SOPHUS_FUNC (line 302) | SOPHUS_FUNC SE3Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 319) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 329) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 344) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 368) | SOPHUS_FUNC SE3Base<Derived>& operator*=(SE3Base<OtherDerived> const...
method rotationMatrix (line 375) | rotationMatrix() const { return so3().matrix(); }
method SOPHUS_FUNC (line 379) | SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3...
method SOPHUS_FUNC (line 383) | SOPHUS_FUNC SO3Type const& so3() const {
method SOPHUS_FUNC (line 391) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
method SOPHUS_FUNC (line 399) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& R) {
function SOPHUS_FUNC (line 426) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 432) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
type Eigen (line 15) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::SE3<Scalar_, Options>> (line 19) | struct traits<Sophus::SE3<Scalar_, Options>> {
type traits<Map<Sophus::SE3<Scalar_>, Options>> (line 26) | struct traits<Map<Sophus::SE3<Scalar_>, Options>>
type traits<Map<Sophus::SE3<Scalar_> const, Options>> (line 34) | struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
class Map<Sophus::SE3<Scalar_>, Options> (line 1069) | class Map<Sophus::SE3<Scalar_>, Options>
method Map (line 1084) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 1090) | SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options>& so3() { return so3_; }
function SOPHUS_FUNC (line 1106) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar, Options>> const& translation()...
type Sophus (line 43) | namespace Sophus {
class SE3 (line 10) | class SE3
class SE3Base (line 57) | class SE3Base {
method SOPHUS_FUNC (line 105) | SOPHUS_FUNC Adjoint Adj() const {
method Scalar (line 117) | Scalar angleX() const { return so3().angleX(); }
method Scalar (line 121) | Scalar angleY() const { return so3().angleY(); }
method Scalar (line 125) | Scalar angleZ() const { return so3().angleZ(); }
method SOPHUS_FUNC (line 130) | SOPHUS_FUNC SE3<NewScalarType> cast() const {
method Dx_this_mul_exp_x_at_0 (line 137) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 210) | Dx_log_this_inv_by_x_at_this()
method inverse (line 222) | inverse() const {
method SOPHUS_FUNC (line 237) | SOPHUS_FUNC Tangent log() const {
method SOPHUS_FUNC (line 261) | SOPHUS_FUNC void normalize() { so3().normalize(); }
method matrix (line 273) | matrix() const {
method matrix3x4 (line 283) | matrix3x4() const {
method SOPHUS_FUNC (line 293) | SOPHUS_FUNC SE3Base<Derived>& operator=(SE3Base<OtherDerived> const&...
method SOPHUS_FUNC (line 302) | SOPHUS_FUNC SE3Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 319) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 329) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 344) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 368) | SOPHUS_FUNC SE3Base<Derived>& operator*=(SE3Base<OtherDerived> const...
method rotationMatrix (line 375) | rotationMatrix() const { return so3().matrix(); }
method SOPHUS_FUNC (line 379) | SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3...
method SOPHUS_FUNC (line 383) | SOPHUS_FUNC SO3Type const& so3() const {
method SOPHUS_FUNC (line 391) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
method SOPHUS_FUNC (line 399) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& R) {
function SOPHUS_FUNC (line 426) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 432) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
function SOPHUS_FUNC (line 460) | SOPHUS_FUNC SE3& operator=(SE3 const& other) = default;
function SOPHUS_FUNC (line 470) | SOPHUS_FUNC SE3(SE3 const& other) = default;
function SOPHUS_FUNC (line 530) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 537) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 544) | SOPHUS_FUNC SO3Member& so3() { return so3_; }
function SOPHUS_FUNC (line 548) | SOPHUS_FUNC SO3Member const& so3() const { return so3_; }
function SOPHUS_FUNC (line 552) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 556) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 560) | SOPHUS_FUNC static Matrix3<Scalar> jacobianUpperRightBlock(
function SOPHUS_FUNC (line 599) | SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobian(
function SOPHUS_FUNC (line 610) | SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobianInverse(
function SOPHUS_FUNC (line 623) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 805) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 826) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 832) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_a...
function SOPHUS_FUNC (line 852) | SOPHUS_FUNC static SE3<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 866) | SOPHUS_FUNC static enable_if_t<std::is_floating_point<S>::value, SE3>
function SOPHUS_FUNC (line 910) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 931) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 949) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function SOPHUS_FUNC (line 964) | static SOPHUS_FUNC SE3 rotX(Scalar const& x) {
function SOPHUS_FUNC (line 970) | static SOPHUS_FUNC SE3 rotY(Scalar const& y) {
function SOPHUS_FUNC (line 976) | static SOPHUS_FUNC SE3 rotZ(Scalar const& z) {
function SE3 (line 985) | static SE3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 995) | static SOPHUS_FUNC SE3 trans(T0 const& x, T1 const& y, T2 const& z) {
function SOPHUS_FUNC (line 999) | static SOPHUS_FUNC SE3 trans(Vector3<Scalar> const& xyz) {
function SOPHUS_FUNC (line 1005) | static SOPHUS_FUNC SE3 transX(Scalar const& x) {
function SOPHUS_FUNC (line 1011) | static SOPHUS_FUNC SE3 transY(Scalar const& y) {
function SOPHUS_FUNC (line 1017) | static SOPHUS_FUNC SE3 transZ(Scalar const& z) {
function SOPHUS_FUNC (line 1035) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
function SOPHUS_FUNC (line 1049) | SOPHUS_FUNC SE3<Scalar, Options>::SE3()
type Eigen (line 1063) | namespace Eigen {
type internal (line 16) | namespace internal {
type traits<Sophus::SE3<Scalar_, Options>> (line 19) | struct traits<Sophus::SE3<Scalar_, Options>> {
type traits<Map<Sophus::SE3<Scalar_>, Options>> (line 26) | struct traits<Map<Sophus::SE3<Scalar_>, Options>>
type traits<Map<Sophus::SE3<Scalar_> const, Options>> (line 34) | struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
class Map<Sophus::SE3<Scalar_>, Options> (line 1069) | class Map<Sophus::SE3<Scalar_>, Options>
method Map (line 1084) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 1090) | SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options>& so3() { return so3_; }
function SOPHUS_FUNC (line 1106) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar, Options>> const& translation()...
class Map<Sophus::SE3<Scalar_> const, Options> (line 1119) | class Map<Sophus::SE3<Scalar_> const, Options>
method Map (line 1133) | Map(Scalar const* coeffs)
FILE: Thirdparty/Sophus/sophus/sim2.hpp
type Sophus (line 9) | namespace Sophus {
class Sim2 (line 11) | class Sim2
class Sim2Base (line 58) | class Sim2Base {
method SOPHUS_FUNC (line 106) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 123) | SOPHUS_FUNC Sim2<NewScalarType> cast() const {
method inverse (line 130) | inverse() const {
method SOPHUS_FUNC (line 145) | SOPHUS_FUNC Tangent log() const {
method matrix (line 176) | matrix() const {
method matrix2x3 (line 186) | matrix2x3() const {
method SOPHUS_FUNC (line 196) | SOPHUS_FUNC Sim2Base<Derived>& operator=(
method SOPHUS_FUNC (line 209) | SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 226) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 236) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 251) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 298) | SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 310) | SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_a...
function SOPHUS_FUNC (line 324) | SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC
function SOPHUS_FUNC (line 338) | SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 344) | SOPHUS_FUNC RxSO2Type& rxso2() {
function SOPHUS_FUNC (line 350) | SOPHUS_FUNC RxSO2Type const& rxso2() const {
function SOPHUS_FUNC (line 356) | SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); }
function SOPHUS_FUNC (line 360) | SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar>& R) {
function SOPHUS_FUNC (line 369) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scal...
function SOPHUS_FUNC (line 376) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) {
function SOPHUS_FUNC (line 382) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 388) | SOPHUS_FUNC TranslationType const& translation() const {
type Eigen (line 16) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::Sim2<Scalar_, Options>> (line 20) | struct traits<Sophus::Sim2<Scalar_, Options>> {
type traits<Map<Sophus::Sim2<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
type traits<Map<Sophus::Sim2<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
class Map<Sophus::Sim2<Scalar_>, Options> (line 747) | class Map<Sophus::Sim2<Scalar_>, Options>
method Map (line 762) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 768) | SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rx...
function SOPHUS_FUNC (line 783) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
type Sophus (line 44) | namespace Sophus {
class Sim2 (line 11) | class Sim2
class Sim2Base (line 58) | class Sim2Base {
method SOPHUS_FUNC (line 106) | SOPHUS_FUNC Adjoint Adj() const {
method SOPHUS_FUNC (line 123) | SOPHUS_FUNC Sim2<NewScalarType> cast() const {
method inverse (line 130) | inverse() const {
method SOPHUS_FUNC (line 145) | SOPHUS_FUNC Tangent log() const {
method matrix (line 176) | matrix() const {
method matrix2x3 (line 186) | matrix2x3() const {
method SOPHUS_FUNC (line 196) | SOPHUS_FUNC Sim2Base<Derived>& operator=(
method SOPHUS_FUNC (line 209) | SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 226) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 236) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 251) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 298) | SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 310) | SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_a...
function SOPHUS_FUNC (line 324) | SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC
function SOPHUS_FUNC (line 338) | SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 344) | SOPHUS_FUNC RxSO2Type& rxso2() {
function SOPHUS_FUNC (line 350) | SOPHUS_FUNC RxSO2Type const& rxso2() const {
function SOPHUS_FUNC (line 356) | SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); }
function SOPHUS_FUNC (line 360) | SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar>& R) {
function SOPHUS_FUNC (line 369) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scal...
function SOPHUS_FUNC (line 376) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) {
function SOPHUS_FUNC (line 382) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 388) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 412) | SOPHUS_FUNC Sim2& operator=(Sim2 const& other) = default;
function SOPHUS_FUNC (line 425) | SOPHUS_FUNC Sim2(Sim2 const& other) = default;
function SOPHUS_FUNC (line 474) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 481) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 488) | SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; }
function SOPHUS_FUNC (line 492) | SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; }
function SOPHUS_FUNC (line 496) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 500) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 506) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 518) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 555) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_a...
function SOPHUS_FUNC (line 565) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 590) | SOPHUS_FUNC static Sim2<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 628) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 649) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 667) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function Sim2 (line 693) | static Sim2 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 712) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
function SOPHUS_FUNC (line 726) | SOPHUS_FUNC Sim2<Scalar, Options>::Sim2()
type Eigen (line 741) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::Sim2<Scalar_, Options>> (line 20) | struct traits<Sophus::Sim2<Scalar_, Options>> {
type traits<Map<Sophus::Sim2<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
type traits<Map<Sophus::Sim2<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
class Map<Sophus::Sim2<Scalar_>, Options> (line 747) | class Map<Sophus::Sim2<Scalar_>, Options>
method Map (line 762) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 768) | SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rx...
function SOPHUS_FUNC (line 783) | SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation()...
class Map<Sophus::Sim2<Scalar_> const, Options> (line 796) | class Map<Sophus::Sim2<Scalar_> const, Options>
method Map (line 810) | Map(Scalar const* coeffs)
FILE: Thirdparty/Sophus/sophus/sim3.hpp
type Sophus (line 9) | namespace Sophus {
class Sim3 (line 11) | class Sim3
function setQuaternion (line 58) | class Sim3Base {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 336) | SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 342) | SOPHUS_FUNC RxSO3Type& rxso3() {
function SOPHUS_FUNC (line 348) | SOPHUS_FUNC RxSO3Type const& rxso3() const {
function SOPHUS_FUNC (line 354) | SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
function SOPHUS_FUNC (line 367) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scal...
function SOPHUS_FUNC (line 374) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
function SOPHUS_FUNC (line 380) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 386) | SOPHUS_FUNC TranslationType const& translation() const {
type Eigen (line 16) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::Sim3<Scalar_, Options>> (line 20) | struct traits<Sophus::Sim3<Scalar_, Options>> {
type traits<Map<Sophus::Sim3<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
type traits<Map<Sophus::Sim3<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
class Map<Sophus::Sim3<Scalar_>, Options> (line 790) | class Map<Sophus::Sim3<Scalar_>, Options>
method Map (line 805) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 811) | SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rx...
function SOPHUS_FUNC (line 826) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation()...
type Sophus (line 44) | namespace Sophus {
class Sim3 (line 11) | class Sim3
function setQuaternion (line 58) | class Sim3Base {
function SOPHUS_FUNC (line 330) | SOPHUS_FUNC QuaternionType const& quaternion() const {
function SOPHUS_FUNC (line 336) | SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
function SOPHUS_FUNC (line 342) | SOPHUS_FUNC RxSO3Type& rxso3() {
function SOPHUS_FUNC (line 348) | SOPHUS_FUNC RxSO3Type const& rxso3() const {
function SOPHUS_FUNC (line 354) | SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
function SOPHUS_FUNC (line 367) | SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scal...
function SOPHUS_FUNC (line 374) | SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
function SOPHUS_FUNC (line 380) | SOPHUS_FUNC TranslationType& translation() {
function SOPHUS_FUNC (line 386) | SOPHUS_FUNC TranslationType const& translation() const {
function SOPHUS_FUNC (line 413) | SOPHUS_FUNC Sim3& operator=(Sim3 const& other) = default;
function SOPHUS_FUNC (line 423) | SOPHUS_FUNC Sim3(Sim3 const& other) = default;
function SOPHUS_FUNC (line 465) | SOPHUS_FUNC explicit Sim3(Scalar const& scale,
function SOPHUS_FUNC (line 484) | SOPHUS_FUNC Scalar* data() {
function SOPHUS_FUNC (line 491) | SOPHUS_FUNC Scalar const* data() const {
function SOPHUS_FUNC (line 498) | SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }
function SOPHUS_FUNC (line 502) | SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }
function SOPHUS_FUNC (line 506) | SOPHUS_FUNC TranslationMember& translation() { return translation_; }
function SOPHUS_FUNC (line 510) | SOPHUS_FUNC TranslationMember const& translation() const {
function SOPHUS_FUNC (line 516) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 528) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 583) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_a...
function SOPHUS_FUNC (line 593) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 610) | SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) {
function SOPHUS_FUNC (line 670) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 691) | SOPHUS_FUNC static Transformation hat(Tangent const& a) {
function SOPHUS_FUNC (line 709) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
function Sim3 (line 734) | static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 755) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
function SOPHUS_FUNC (line 769) | SOPHUS_FUNC Sim3<Scalar, Options>::Sim3()
type Eigen (line 784) | namespace Eigen {
type internal (line 17) | namespace internal {
type traits<Sophus::Sim3<Scalar_, Options>> (line 20) | struct traits<Sophus::Sim3<Scalar_, Options>> {
type traits<Map<Sophus::Sim3<Scalar_>, Options>> (line 27) | struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
type traits<Map<Sophus::Sim3<Scalar_> const, Options>> (line 35) | struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
class Map<Sophus::Sim3<Scalar_>, Options> (line 790) | class Map<Sophus::Sim3<Scalar_>, Options>
method Map (line 805) | Map(Scalar* coeffs)
method SOPHUS_FUNC (line 811) | SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rx...
function SOPHUS_FUNC (line 826) | SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation()...
class Map<Sophus::Sim3<Scalar_> const, Options> (line 839) | class Map<Sophus::Sim3<Scalar_> const, Options>
method Map (line 854) | Map(Scalar const* coeffs)
FILE: Thirdparty/Sophus/sophus/sim_details.hpp
type Sophus (line 5) | namespace Sophus {
type details (line 6) | namespace details {
function calcW (line 9) | Matrix<Scalar, N, N> calcW(Matrix<Scalar, N, N> const &Omega,
function calcW_derivatives (line 51) | void calcW_derivatives(Scalar const theta, Scalar const sigma, Scala...
function calcWInv (line 142) | Matrix<Scalar, N, N> calcWInv(Matrix<Scalar, N, N> const &Omega,
FILE: Thirdparty/Sophus/sophus/so2.hpp
type Sophus (line 15) | namespace Sophus {
class SO2 (line 17) | class SO2
class SO2Base (line 78) | class SO2Base {
method SOPHUS_FUNC (line 127) | SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); }
method SOPHUS_FUNC (line 132) | SOPHUS_FUNC SO2<NewScalarType> cast() const {
method SOPHUS_FUNC (line 141) | SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
method SOPHUS_FUNC (line 145) | SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(...
method inverse (line 149) | inverse() const {
method SOPHUS_FUNC (line 163) | SOPHUS_FUNC Scalar log() const {
method SOPHUS_FUNC (line 173) | SOPHUS_FUNC void normalize() {
method matrix (line 187) | matrix() const {
method SOPHUS_FUNC (line 202) | SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const&...
method SOPHUS_FUNC (line 210) | SOPHUS_FUNC SO2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 245) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 261) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 276) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 301) | SOPHUS_FUNC SO2Base<Derived> operator*=(SO2Base<OtherDerived> const&...
method Dx_this_mul_exp_x_at_0 (line 308) | Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 334) | SOPHUS_FUNC void setComplex(Point const& complex) {
function SOPHUS_FUNC (line 341) | SOPHUS_FUNC
function SOPHUS_FUNC (line 380) | SOPHUS_FUNC SO2& operator=(SO2 const& other) = default;
function SO2 (line 386) | SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
function SOPHUS_FUNC (line 390) | SOPHUS_FUNC SO2(SO2 const& other) = default;
type Eigen (line 22) | namespace Eigen {
type internal (line 23) | namespace internal {
type traits<Sophus::SO2<Scalar_, Options_>> (line 26) | struct traits<Sophus::SO2<Scalar_, Options_>> {
type traits<Map<Sophus::SO2<Scalar_>, Options_>> (line 33) | struct traits<Map<Sophus::SO2<Scalar_>, Options_>>
type traits<Map<Sophus::SO2<Scalar_> const, Options_>> (line 41) | struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>
class Map<Sophus::SO2<Scalar_>, Options> (line 582) | class Map<Sophus::SO2<Scalar_>, Options>
method Map (line 602) | Map(Scalar* coeffs) : unit_complex_(coeffs) {}
type Sophus (line 50) | namespace Sophus {
class SO2 (line 17) | class SO2
class SO2Base (line 78) | class SO2Base {
method SOPHUS_FUNC (line 127) | SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); }
method SOPHUS_FUNC (line 132) | SOPHUS_FUNC SO2<NewScalarType> cast() const {
method SOPHUS_FUNC (line 141) | SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
method SOPHUS_FUNC (line 145) | SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(...
method inverse (line 149) | inverse() const {
method SOPHUS_FUNC (line 163) | SOPHUS_FUNC Scalar log() const {
method SOPHUS_FUNC (line 173) | SOPHUS_FUNC void normalize() {
method matrix (line 187) | matrix() const {
method SOPHUS_FUNC (line 202) | SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const&...
method SOPHUS_FUNC (line 210) | SOPHUS_FUNC SO2Product<OtherDerived> operator*(
method SOPHUS_FUNC (line 245) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
method SOPHUS_FUNC (line 261) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
method SOPHUS_FUNC (line 276) | SOPHUS_FUNC Line operator*(Line const& l) const {
method SOPHUS_FUNC (line 301) | SOPHUS_FUNC SO2Base<Derived> operator*=(SO2Base<OtherDerived> const&...
method Dx_this_mul_exp_x_at_0 (line 308) | Dx_this_mul_exp_x_at_0()
function SOPHUS_FUNC (line 334) | SOPHUS_FUNC void setComplex(Point const& complex) {
function SOPHUS_FUNC (line 341) | SOPHUS_FUNC
function SOPHUS_FUNC (line 380) | SOPHUS_FUNC SO2& operator=(SO2 const& other) = default;
function SO2 (line 386) | SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
function SOPHUS_FUNC (line 390) | SOPHUS_FUNC SO2(SO2 const& other) = default;
function SOPHUS_FUNC (line 415) | SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
function SOPHUS_FUNC (line 425) | SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
function SOPHUS_FUNC (line 434) | SOPHUS_FUNC explicit SO2(Scalar theta) {
function SOPHUS_FUNC (line 440) | SOPHUS_FUNC ComplexMember const& unit_complex() const {
function SOPHUS_FUNC (line 453) | SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
function SOPHUS_FUNC (line 461) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 470) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 477) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_a...
function SOPHUS_FUNC (line 484) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {
function SOPHUS_FUNC (line 495) | SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); }
function SOPHUS_FUNC (line 510) | SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
function SOPHUS_FUNC (line 523) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO2>
function SOPHUS_FUNC (line 533) | SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
function SO2 (line 540) | static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
function SOPHUS_FUNC (line 560) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 575) | namespace Eigen {
type internal (line 23) | namespace internal {
type traits<Sophus::SO2<Scalar_, Options_>> (line 26) | struct traits<Sophus::SO2<Scalar_, Options_>> {
type traits<Map<Sophus::SO2<Scalar_>, Options_>> (line 33) | struct traits<Map<Sophus::SO2<Scalar_>, Options_>>
type traits<Map<Sophus::SO2<Scalar_> const, Options_>> (line 41) | struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>
class Map<Sophus::SO2<Scalar_>, Options> (line 582) | class Map<Sophus::SO2<Scalar_>, Options>
method Map (line 602) | Map(Scalar* coeffs) : unit_complex_(coeffs) {}
class Map<Sophus::SO2<Scalar_> const, Options> (line 627) | class Map<Sophus::SO2<Scalar_> const, Options>
method Map (line 641) | Map(Scalar const* coeffs) : unit_complex_(coeffs) {}
FILE: Thirdparty/Sophus/sophus/so3.hpp
type Sophus (line 16) | namespace Sophus {
class SO3 (line 18) | class SO3
method SOPHUS_FUNC (line 492) | SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
method SO3 (line 498) | SO3()
method SOPHUS_FUNC (line 503) | SOPHUS_FUNC SO3(SO3 const& other) = default;
method SOPHUS_FUNC (line 508) | SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 528) | SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 538) | SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
method if (line 561) | if (theta_sq <
method else (line 564) | else {
class SO3Base (line 76) | class SO3Base {
type TangentAndTheta (line 101) | struct TangentAndTheta {
method SOPHUS_FUNC (line 133) | SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
method SOPHUS_FUNC (line 138) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX(...
method SOPHUS_FUNC (line 147) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY(...
method SOPHUS_FUNC (line 161) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ(...
method SOPHUS_FUNC (line 170) | SOPHUS_FUNC SO3<NewScalarType> cast() const {
method SOPHUS_FUNC (line 182) | SOPHUS_FUNC Scalar* data() {
method SOPHUS_FUNC (line 188) | SOPHUS_FUNC Scalar const* data() const {
method Dx_this_mul_exp_x_at_0 (line 194) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 223) | Dx_log_this_inv_by_x_at_this()
function SOPHUS_FUNC (line 260) | SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
function SOPHUS_FUNC (line 264) | SOPHUS_FUNC TangentAndTheta logAndTheta() const {
function SOPHUS_FUNC (line 318) | SOPHUS_FUNC void normalize() {
function SOPHUS_FUNC (line 332) | SOPHUS_FUNC Transformation matrix() const {
function SOPHUS_FUNC (line 339) | SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& o...
function SOPHUS_FUNC (line 346) | SOPHUS_FUNC static QuaternionProductType QuaternionProduct(
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC SO3Product<OtherDerived> operator*(
function SOPHUS_FUNC (line 388) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
function SOPHUS_FUNC (line 403) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
function SOPHUS_FUNC (line 416) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 428) | SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
function SOPHUS_FUNC (line 438) | SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& ...
function SOPHUS_FUNC (line 447) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quater...
function SOPHUS_FUNC (line 454) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
class SO3 (line 469) | class SO3 : public SO3Base<SO3<Scalar_, Options>> {
method SOPHUS_FUNC (line 492) | SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
method SO3 (line 498) | SO3()
method SOPHUS_FUNC (line 503) | SOPHUS_FUNC SO3(SO3 const& other) = default;
method SOPHUS_FUNC (line 508) | SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 528) | SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 538) | SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
method if (line 561) | if (theta_sq <
method else (line 564) | else {
function SOPHUS_FUNC (line 601) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 651) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 665) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point...
function SOPHUS_FUNC (line 672) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 685) | SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
function SOPHUS_FUNC (line 694) | SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
function SOPHUS_FUNC (line 736) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3>
function SOPHUS_FUNC (line 761) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 783) | SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
function SOPHUS_FUNC (line 807) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
function SOPHUS_FUNC (line 814) | static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
function SOPHUS_FUNC (line 820) | static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
function SOPHUS_FUNC (line 826) | static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
function SO3 (line 834) | static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 492) | SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
method SO3 (line 498) | SO3()
method SOPHUS_FUNC (line 503) | SOPHUS_FUNC SO3(SO3 const& other) = default;
method SOPHUS_FUNC (line 508) | SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 528) | SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 538) | SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
method if (line 561) | if (theta_sq <
method else (line 564) | else {
function SOPHUS_FUNC (line 866) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 23) | namespace Eigen {
type internal (line 24) | namespace internal {
type traits<Sophus::SO3<Scalar_, Options_>> (line 27) | struct traits<Sophus::SO3<Scalar_, Options_>> {
type traits<Map<Sophus::SO3<Scalar_>, Options_>> (line 34) | struct traits<Map<Sophus::SO3<Scalar_>, Options_>>
type traits<Map<Sophus::SO3<Scalar_> const, Options_>> (line 42) | struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>
class Map<Sophus::SO3<Scalar_>, Options> (line 888) | class Map<Sophus::SO3<Scalar_>, Options>
method Map (line 907) | Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
type Sophus (line 51) | namespace Sophus {
class SO3 (line 18) | class SO3
method SOPHUS_FUNC (line 492) | SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
method SO3 (line 498) | SO3()
method SOPHUS_FUNC (line 503) | SOPHUS_FUNC SO3(SO3 const& other) = default;
method SOPHUS_FUNC (line 508) | SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 528) | SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 538) | SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
method if (line 561) | if (theta_sq <
method else (line 564) | else {
class SO3Base (line 76) | class SO3Base {
type TangentAndTheta (line 101) | struct TangentAndTheta {
method SOPHUS_FUNC (line 133) | SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
method SOPHUS_FUNC (line 138) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX(...
method SOPHUS_FUNC (line 147) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY(...
method SOPHUS_FUNC (line 161) | SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ(...
method SOPHUS_FUNC (line 170) | SOPHUS_FUNC SO3<NewScalarType> cast() const {
method SOPHUS_FUNC (line 182) | SOPHUS_FUNC Scalar* data() {
method SOPHUS_FUNC (line 188) | SOPHUS_FUNC Scalar const* data() const {
method Dx_this_mul_exp_x_at_0 (line 194) | Dx_this_mul_exp_x_at_0()
method Dx_log_this_inv_by_x_at_this (line 223) | Dx_log_this_inv_by_x_at_this()
function SOPHUS_FUNC (line 260) | SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
function SOPHUS_FUNC (line 264) | SOPHUS_FUNC TangentAndTheta logAndTheta() const {
function SOPHUS_FUNC (line 318) | SOPHUS_FUNC void normalize() {
function SOPHUS_FUNC (line 332) | SOPHUS_FUNC Transformation matrix() const {
function SOPHUS_FUNC (line 339) | SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& o...
function SOPHUS_FUNC (line 346) | SOPHUS_FUNC static QuaternionProductType QuaternionProduct(
function SOPHUS_FUNC (line 358) | SOPHUS_FUNC SO3Product<OtherDerived> operator*(
function SOPHUS_FUNC (line 388) | SOPHUS_FUNC PointProduct<PointDerived> operator*(
function SOPHUS_FUNC (line 403) | SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
function SOPHUS_FUNC (line 416) | SOPHUS_FUNC Line operator*(Line const& l) const {
function SOPHUS_FUNC (line 428) | SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
function SOPHUS_FUNC (line 438) | SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& ...
function SOPHUS_FUNC (line 447) | SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quater...
function SOPHUS_FUNC (line 454) | SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
class SO3 (line 469) | class SO3 : public SO3Base<SO3<Scalar_, Options>> {
method SOPHUS_FUNC (line 492) | SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
method SO3 (line 498) | SO3()
method SOPHUS_FUNC (line 503) | SOPHUS_FUNC SO3(SO3 const& other) = default;
method SOPHUS_FUNC (line 508) | SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 528) | SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 538) | SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
method if (line 561) | if (theta_sq <
method else (line 564) | else {
function SOPHUS_FUNC (line 601) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
function SOPHUS_FUNC (line 651) | SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
function SOPHUS_FUNC (line 665) | SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point...
function SOPHUS_FUNC (line 672) | SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
function SOPHUS_FUNC (line 685) | SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
function SOPHUS_FUNC (line 694) | SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
function SOPHUS_FUNC (line 736) | static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3>
function SOPHUS_FUNC (line 761) | SOPHUS_FUNC static Transformation generator(int i) {
function SOPHUS_FUNC (line 783) | SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
function SOPHUS_FUNC (line 807) | SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
function SOPHUS_FUNC (line 814) | static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
function SOPHUS_FUNC (line 820) | static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
function SOPHUS_FUNC (line 826) | static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
function SO3 (line 834) | static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
method SOPHUS_FUNC (line 492) | SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
method SO3 (line 498) | SO3()
method SOPHUS_FUNC (line 503) | SOPHUS_FUNC SO3(SO3 const& other) = default;
method SOPHUS_FUNC (line 508) | SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
method SOPHUS_FUNC (line 528) | SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
method SOPHUS_FUNC (line 538) | SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
method if (line 561) | if (theta_sq <
method else (line 564) | else {
function SOPHUS_FUNC (line 866) | SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
type Eigen (line 882) | namespace Eigen {
type internal (line 24) | namespace internal {
type traits<Sophus::SO3<Scalar_, Options_>> (line 27) | struct traits<Sophus::SO3<Scalar_, Options_>> {
type traits<Map<Sophus::SO3<Scalar_>, Options_>> (line 34) | struct traits<Map<Sophus::SO3<Scalar_>, Options_>>
type traits<Map<Sophus::SO3<Scalar_> const, Options_>> (line 42) | struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>
class Map<Sophus::SO3<Scalar_>, Options> (line 888) | class Map<Sophus::SO3<Scalar_>, Options>
method Map (line 907) | Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
class Map<Sophus::SO3<Scalar_> const, Options> (line 932) | class Map<Sophus::SO3<Scalar_> const, Options>
method Map (line 946) | Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {}
FILE: Thirdparty/Sophus/sophus/spline.hpp
type Sophus (line 10) | namespace Sophus {
class SplineBasisFunction (line 13) | class SplineBasisFunction {
function SOPHUS_FUNC (line 34) | static SOPHUS_FUNC Vector3<Scalar> Dt_B(Scalar const& u,
function SOPHUS_FUNC (line 43) | static SOPHUS_FUNC Vector3<Scalar> Dt2_B(Scalar const& u,
class BasisSplineFn (line 53) | class BasisSplineFn {
method LieGroup (line 60) | static LieGroup parent_T_spline(
method Transformation (line 69) | static Transformation Dt_parent_T_spline(
method Transformation (line 84) | static Transformation Dt2_parent_T_spline(
method A (line 109) | static std::tuple<LieGroup, LieGroup, LieGroup> A(
method Dt_A (line 119) | static std::tuple<Transformation, Transformation, Transformation> Dt_A(
method Dt2_A (line 133) | static std::tuple<Transformation, Transformation, Transformation> Dt2_A(
type SegmentCase (line 154) | enum class SegmentCase { first, normal, last }
type BasisSplineSegment (line 157) | struct BasisSplineSegment {
method BasisSplineSegment (line 162) | BasisSplineSegment(SegmentCase segment_case, T const* const raw_ptr0,
method world_pose_foo_prev (line 171) | Eigen::Map<LieGroup const> const world_pose_foo_prev() const {
method world_pose_foo_0 (line 174) | Eigen::Map<LieGroup const> const world_pose_foo_0() const {
method world_pose_foo_1 (line 178) | Eigen::Map<LieGroup const> const world_pose_foo_1() const {
method world_pose_foo_2 (line 182) | Eigen::Map<LieGroup const> const world_pose_foo_2() const {
method LieGroup (line 185) | LieGroup parent_T_spline(double u) {
method Transformation (line 215) | Transformation Dt_parent_T_spline(double u, double delta_t) {
method Transformation (line 245) | Transformation Dt2_parent_T_spline(double u, double delta_t) {
class BasisSplineImpl (line 284) | class BasisSplineImpl {
method BasisSplineImpl (line 291) | BasisSplineImpl(const std::vector<LieGroup>& parent_Ts_control_point,
method LieGroup (line 298) | LieGroup parent_T_spline(int i, double u) const {
method Transformation (line 323) | Transformation Dt_parent_T_spline(int i, double u) const {
method Transformation (line 348) | Transformation Dt2_parent_T_spline(int i, double u) const {
method getNumSegments (line 381) | int getNumSegments() const {
method delta_t (line 385) | double delta_t() const { return delta_t_; }
type IndexAndU (line 392) | struct IndexAndU {
class BasisSpline (line 398) | class BasisSpline {
method BasisSpline (line 405) | BasisSpline(std::vector<LieGroup> parent_Ts_control_point, double t0,
method LieGroup (line 409) | LieGroup parent_T_spline(double t) const {
method Transformation (line 415) | Transformation Dt_parent_T_spline(double t) const {
method Transformation (line 420) | Transformation Dt2_parent_T_spline(double t) const {
method t0 (line 425) | double t0() const { return t0_; }
method tmax (line 427) | double tmax() const { return t0_ + impl_.delta_t() * getNumSegments(); }
method getNumSegments (line 437) | int getNumSegments() const { return impl_.getNumSegments(); }
method s (line 439) | double s(double t) const { return (t - t0_) / impl_.delta_t(); }
method delta_t (line 441) | double delta_t() const { return impl_.delta_t(); }
method IndexAndU (line 443) | IndexAndU index_and_u(double t) const {
FILE: Thirdparty/Sophus/sophus/test_macros.hpp
type Sophus (line 8) | namespace Sophus {
type details (line 9) | namespace details {
class Pretty (line 12) | class Pretty {
method impl (line 14) | static std::string impl(Scalar s) {
class Pretty<Ptr, enable_if_t<std::is_pointer<Ptr>::value>> (line 22) | class Pretty<Ptr, enable_if_t<std::is_pointer<Ptr>::value>> {
method impl (line 24) | static std::string impl(Ptr ptr) {
class Pretty<Eigen::Matrix<Scalar, M, N>, void> (line 32) | class Pretty<Eigen::Matrix<Scalar, M, N>, void> {
method impl (line 34) | static std::string impl(Matrix<Scalar, M, N> const& v) {
function pretty (line 42) | std::string pretty(T const& v) {
function testFailed (line 47) | void testFailed(bool& passed, char const* func, char const* file, in...
function processTestResult (line 59) | void processTestResult(bool passed) {
FILE: Thirdparty/Sophus/sophus/types.hpp
type Sophus (line 9) | namespace Sophus {
type details (line 93) | namespace details {
class MaxMetric (line 95) | class MaxMetric {
method Scalar (line 97) | static Scalar impl(Scalar s0, Scalar s1) {
class MaxMetric<Matrix<Scalar, M, N>> (line 104) | class MaxMetric<Matrix<Scalar, M, N>> {
method Scalar (line 106) | static Scalar impl(Matrix<Scalar, M, N> const& p0,
class SetToZero (line 113) | class SetToZero {
method impl (line 115) | static void impl(Scalar& s) { s = Scalar(0); }
class SetToZero<Matrix<Scalar, M, N>> (line 119) | class SetToZero<Matrix<Scalar, M, N>> {
method impl (line 121) | static void impl(Matrix<Scalar, M, N>& v) { v.setZero(); }
class SetElementAt (line 125) | class SetElementAt
class SetElementAt<Scalar, Scalar> (line 128) | class SetElementAt<Scalar, Scalar> {
method impl (line 130) | static void impl(Scalar& s, Scalar value, int at) {
class SetElementAt<Vector<Scalar, N>, Scalar> (line 137) | class SetElementAt<Vector<Scalar, N>, Scalar> {
method impl (line 139) | static void impl(Vector<Scalar, N>& v, Scalar value, int at) {
class SquaredNorm (line 146) | class SquaredNorm {
method Scalar (line 148) | static Scalar impl(Scalar const& s) { return s * s; }
class SquaredNorm<Matrix<Scalar, N, 1>> (line 152) | class SquaredNorm<Matrix<Scalar, N, 1>> {
method Scalar (line 154) | static Scalar impl(Matrix<Scalar, N, 1> const& s) { return s.squar...
class Transpose (line 158) | class Transpose {
method Scalar (line 160) | static Scalar impl(Scalar const& s) { return s; }
class Transpose<Matrix<Scalar, M, N>> (line 164) | class Transpose<Matrix<Scalar, M, N>> {
method impl (line 166) | static Matrix<Scalar, M, N> impl(Matrix<Scalar, M, N> const& s) {
function maxMetric (line 176) | auto maxMetric(T const& p0, T const& p1)
function setToZero (line 184) | void setToZero(T& p) {
function setElementAt (line 192) | void setElementAt(T& p, Scalar value, int i) {
function squaredNorm (line 199) | auto squaredNorm(T const& p) -> decltype(details::SquaredNorm<T>::impl...
function transpose (line 207) | auto transpose(T const& p) -> decltype(details::Transpose<T>::impl(T()...
type IsFloatingPoint (line 212) | struct IsFloatingPoint {
type IsFloatingPoint<Matrix<Scalar, M, N>> (line 217) | struct IsFloatingPoint<Matrix<Scalar, M, N>> {
type GetScalar (line 222) | struct GetScalar {
type GetScalar<Matrix<Scalar_, M, N>> (line 227) | struct GetScalar<Matrix<Scalar_, M, N>> {
type IsFixedSizeVector (line 237) | struct IsFixedSizeVector : std::true_type {}
FILE: Thirdparty/Sophus/sophus/velocities.hpp
type Sophus (line 8) | namespace Sophus {
type experimental (line 9) | namespace experimental {
function transformVelocity (line 17) | Vector3<Scalar> transformVelocity(SO3<Scalar> const& foo_R_bar,
function transformVelocity (line 34) | Vector3<Scalar> transformVelocity(SE3<Scalar> const& foo_T_bar,
function finiteDifferenceRotationalVelocity (line 42) | Vector3<Scalar> finiteDifferenceRotationalVelocity(
function finiteDifferenceRotationalVelocity (line 62) | Vector3<Scalar> finiteDifferenceRotationalVelocity(
FILE: Thirdparty/Sophus/sophus_pybind-stubs/sophus_pybind.pyi
class SE3 (line 5) | class SE3:
method exp (line 8) | def exp(arg0: numpy.ndarray[numpy.float64[3, 1]], arg1: numpy.ndarray[...
method exp (line 14) | def exp(arg0: numpy.ndarray[numpy.float64[m, 3]], arg1: numpy.ndarray[...
method from_matrix (line 20) | def from_matrix(arg0: numpy.ndarray[numpy.float64[4, 4]]) -> SE3:
method from_matrix (line 24) | def from_matrix(arg0: numpy.ndarray[numpy.float64]) -> SE3:
method from_matrix3x4 (line 28) | def from_matrix3x4(arg0: numpy.ndarray[numpy.float64[3, 4]]) -> SE3:
method from_matrix3x4 (line 32) | def from_matrix3x4(arg0: numpy.ndarray[numpy.float64]) -> SE3:
method __copy__ (line 34) | def __copy__(self) -> SE3:
method __getitem__ (line 36) | def __getitem__(self, arg0: typing.Any) -> SE3:
method __imatmul__ (line 38) | def __imatmul__(self, arg0: SE3) -> SE3:
method __init__ (line 41) | def __init__(self) -> None:
method __init__ (line 46) | def __init__(self, arg0: SE3) -> None:
method __len__ (line 50) | def __len__(self) -> int:
method __matmul__ (line 53) | def __matmul__(self, arg0: SE3) -> SE3:
method __matmul__ (line 56) | def __matmul__(self, arg0: numpy.ndarray[numpy.float64[3, n]]) -> nump...
method __repr__ (line 58) | def __repr__(self) -> str:
method __setitem__ (line 60) | def __setitem__(self, arg0: typing.Any, arg1: SE3) -> None:
method __str__ (line 62) | def __str__(self) -> str:
method from_quat_and_translation (line 65) | def from_quat_and_translation(self, arg0: numpy.ndarray[numpy.float64[...
method from_quat_and_translation (line 70) | def from_quat_and_translation(self, arg0: numpy.ndarray[numpy.float64[...
method inverse (line 74) | def inverse(self) -> SE3:
method log (line 78) | def log(self) -> numpy.ndarray[numpy.float64[m, 6]]:
method rotation (line 82) | def rotation(self) -> SO3:
method to_matrix (line 86) | def to_matrix(self) -> numpy.ndarray[numpy.float64]:
method to_matrix3x4 (line 90) | def to_matrix3x4(self) -> numpy.ndarray[numpy.float64]:
method to_quat_and_translation (line 94) | def to_quat_and_translation(self) -> numpy.ndarray[numpy.float64[m, 7]]:
method translation (line 98) | def translation(self) -> numpy.ndarray[numpy.float64[m, 3]]:
class SO3 (line 102) | class SO3:
method exp (line 104) | def exp(arg0: numpy.ndarray[numpy.float64[m, 3]]) -> SO3:
method from_matrix (line 110) | def from_matrix(arg0: numpy.ndarray[numpy.float64[3, 3]]) -> SO3:
method from_matrix (line 114) | def from_matrix(arg0: numpy.ndarray[numpy.float64]) -> SO3:
method from_quat (line 118) | def from_quat(arg0: float, arg1: numpy.ndarray[numpy.float64[3, 1]]) -...
method from_quat (line 124) | def from_quat(arg0: list[float], arg1: numpy.ndarray[numpy.float64[m, ...
method __copy__ (line 128) | def __copy__(self) -> SO3:
method __getitem__ (line 130) | def __getitem__(self, arg0: typing.Any) -> SO3:
method __imatmul__ (line 132) | def __imatmul__(self, arg0: SO3) -> SO3:
method __init__ (line 135) | def __init__(self) -> None:
method __init__ (line 140) | def __init__(self, arg0: SO3) -> None:
method __len__ (line 144) | def __len__(self) -> int:
method __matmul__ (line 147) | def __matmul__(self, arg0: SO3) -> SO3:
method __matmul__ (line 150) | def __matmul__(self, arg0: numpy.ndarray[numpy.float64[3, n]]) -> nump...
method __repr__ (line 152) | def __repr__(self) -> str:
method __setitem__ (line 154) | def __setitem__(self, arg0: typing.Any, arg1: SO3) -> None:
method __str__ (line 156) | def __str__(self) -> str:
method inverse (line 158) | def inverse(self) -> SO3:
method log (line 162) | def log(self) -> numpy.ndarray[numpy.float64[m, 3]]:
method to_matrix (line 166) | def to_matrix(self) -> numpy.ndarray[numpy.float64]:
method to_quat (line 170) | def to_quat(self) -> numpy.ndarray[numpy.float64[m, 4]]:
function interpolate (line 174) | def interpolate(arg0: SE3, arg1: SE3, arg2: float) -> SE3:
function iterativeMean (line 178) | def iterativeMean(arg0: SE3) -> SE3:
FILE: Thirdparty/Sophus/sophus_pybind/SE3PyBind.h
function namespace (line 15) | namespace Sophus {
function namespace (line 39) | namespace pybind11 {
function namespace (line 69) | namespace Sophus {
function else (line 404) | else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
FILE: Thirdparty/Sophus/sophus_pybind/SO3PyBind.h
function namespace (line 12) | namespace Sophus {
function namespace (line 38) | namespace pybind11 {
function namespace (line 68) | namespace Sophus {
function else (line 298) | else if (pybind11::isinstance<pybind11::list>(index_or_slice_or_list)) {
FILE: Thirdparty/Sophus/sophus_pybind/SophusPyBind.h
function namespace (line 16) | namespace Sophus {
FILE: Thirdparty/Sophus/sophus_pybind/bindings.cpp
function PYBIND11_MODULE (line 3) | PYBIND11_MODULE(sophus_pybind, m) { Sophus::exportSophus(m); }
FILE: Thirdparty/Sophus/sophus_pybind/tests/sophusPybindTests.py
class SophusPybindTest (line 7) | class SophusPybindTest(unittest.TestCase):
method test_sophus (line 8) | def test_sophus(self) -> None:
FILE: Thirdparty/Sophus/sympy/sophus/complex.py
class Complex (line 6) | class Complex:
method __init__ (line 9) | def __init__(self, real, imag):
method __mul__ (line 13) | def __mul__(self, right):
method __add__ (line 18) | def __add__(self, right):
method __neg__ (line 21) | def __neg__(self):
method __truediv__ (line 24) | def __truediv__(self, scalar):
method __repr__ (line 28) | def __repr__(self):
method __getitem__ (line 31) | def __getitem__(self, key):
method squared_norm (line 38) | def squared_norm(self):
method conj (line 42) | def conj(self):
method inv (line 46) | def inv(self):
method identity (line 51) | def identity():
method zero (line 55) | def zero():
method __eq__ (line 58) | def __eq__(self, other):
method subs (line 63) | def subs(self, x, y):
method simplify (line 66) | def simplify(self):
method Da_a_mul_b (line 71) | def Da_a_mul_b(a, b):
method Db_a_mul_b (line 77) | def Db_a_mul_b(a, b):
class TestComplex (line 83) | class TestComplex(unittest.TestCase):
method setUp (line 84) | def setUp(self):
method test_multiplications (line 90) | def test_multiplications(self):
method test_derivatives (line 98) | def test_derivatives(self):
FILE: Thirdparty/Sophus/sympy/sophus/cse_codegen.py
function cse_codegen (line 6) | def cse_codegen(symbols):
FILE: Thirdparty/Sophus/sympy/sophus/dual_quaternion.py
class DualQuaternion (line 9) | class DualQuaternion:
method __init__ (line 12) | def __init__(self, real_q, inf_q):
method __mul__ (line 18) | def __mul__(self, right):
method __truediv__ (line 24) | def __truediv__(self, scalar):
method __repr__ (line 28) | def __repr__(self):
method __getitem__ (line 32) | def __getitem__(self, key):
method squared_norm (line 39) | def squared_norm(self):
method conj (line 43) | def conj(self):
method inv (line 47) | def inv(self):
method simplify (line 53) | def simplify(self):
method identity (line 58) | def identity():
method __eq__ (line 62) | def __eq__(self, other):
class TestDualQuaternion (line 68) | class TestDualQuaternion(unittest.TestCase):
method setUp (line 69) | def setUp(self):
method test_multiplications (line 84) | def test_multiplications(self):
FILE: Thirdparty/Sophus/sympy/sophus/matrix.py
function dot (line 8) | def dot(left, right):
function squared_norm (line 19) | def squared_norm(m):
function Vector2 (line 24) | def Vector2(x, y):
function ZeroVector2 (line 28) | def ZeroVector2():
function Vector3 (line 32) | def Vector3(x, y, z):
function ZeroVector3 (line 36) | def ZeroVector3():
function Vector4 (line 40) | def Vector4(a, b, c, d):
function ZeroVector4 (line 44) | def ZeroVector4():
function Vector5 (line 48) | def Vector5(a, b, c, d, e):
function ZeroVector5 (line 52) | def ZeroVector5():
function Vector6 (line 56) | def Vector6(a, b, c, d, e, f):
function ZeroVector6 (line 60) | def ZeroVector6():
function proj (line 64) | def proj(v):
function unproj (line 73) | def unproj(v):
FILE: Thirdparty/Sophus/sympy/sophus/quaternion.py
class Quaternion (line 11) | class Quaternion:
method __init__ (line 14) | def __init__(self, real, vec):
method __mul__ (line 21) | def __mul__(self, right):
method __add__ (line 27) | def __add__(self, right):
method __neg__ (line 31) | def __neg__(self):
method __truediv__ (line 34) | def __truediv__(self, scalar):
method __repr__ (line 38) | def __repr__(self):
method __getitem__ (line 41) | def __getitem__(self, key):
method squared_norm (line 49) | def squared_norm(self):
method conj (line 53) | def conj(self):
method inv (line 57) | def inv(self):
method identity (line 62) | def identity():
method zero (line 66) | def zero():
method subs (line 69) | def subs(self, x, y):
method simplify (line 72) | def simplify(self):
method __eq__ (line 77) | def __eq__(self, other):
method Da_a_mul_b (line 83) | def Da_a_mul_b(a, b):
method Db_a_mul_b (line 95) | def Db_a_mul_b(a, b):
class TestQuaternion (line 107) | class TestQuaternion(unittest.TestCase):
method setUp (line 108) | def setUp(self):
method test_multiplications (line 116) | def test_multiplications(self):
method test_derivatives (line 124) | def test_derivatives(self):
FILE: Thirdparty/Sophus/sympy/sophus/se2.py
class Se2 (line 17) | class Se2:
method __init__ (line 20) | def __init__(self, so2, t):
method exp (line 27) | def exp(v):
method log (line 39) | def log(self):
method calc_Dx_log_this (line 49) | def calc_Dx_log_this(self):
method __repr__ (line 53) | def __repr__(self):
method hat (line 57) | def hat(v):
method matrix (line 64) | def matrix(self):
method __mul__ (line 69) | def __mul__(self, right):
method __getitem__ (line 80) | def __getitem__(self, key):
method calc_Dx_exp_x (line 89) | def calc_Dx_exp_x(x):
method Dx_exp_x_at_0 (line 94) | def Dx_exp_x_at_0():
method calc_Dx_this_mul_exp_x_at_0 (line 100) | def calc_Dx_this_mul_exp_x_at_0(self, x):
method calc_Dx_exp_x_at_0 (line 106) | def calc_Dx_exp_x_at_0(x):
method Dxi_x_matrix (line 110) | def Dxi_x_matrix(x, i):
method calc_Dxi_x_matrix (line 120) | def calc_Dxi_x_matrix(x, i):
method Dxi_exp_x_matrix (line 125) | def Dxi_exp_x_matrix(x, i):
method calc_Dxi_exp_x_matrix (line 132) | def calc_Dxi_exp_x_matrix(x, i):
method Dxi_exp_x_matrix_at_0 (line 137) | def Dxi_exp_x_matrix_at_0(i):
method calc_Dxi_exp_x_matrix_at_0 (line 143) | def calc_Dxi_exp_x_matrix_at_0(x, i):
class TestSe2 (line 149) | class TestSe2(unittest.TestCase):
method setUp (line 150) | def setUp(self):
method test_exp_log (line 163) | def test_exp_log(self):
method test_matrix (line 171) | def test_matrix(self):
method test_derivatives (line 180) | def test_derivatives(self):
method test_codegen (line 199) | def test_codegen(self):
FILE: Thirdparty/Sophus/sympy/sophus/se3.py
class Se3 (line 18) | class Se3:
method __init__ (line 21) | def __init__(self, so3, t):
method exp (line 32) | def exp(v):
method log (line 45) | def log(self):
method calc_Dx_log_this (line 59) | def calc_Dx_log_this(self):
method __repr__ (line 63) | def __repr__(self):
method inverse (line 66) | def inverse(self):
method hat (line 71) | def hat(v):
method vee (line 81) | def vee(Omega):
method matrix (line 92) | def matrix(self):
method __mul__ (line 97) | def __mul__(self, right):
method __getitem__ (line 109) | def __getitem__(self, key):
method calc_Dx_exp_x (line 118) | def calc_Dx_exp_x(x):
method Dx_exp_x_at_0 (line 123) | def Dx_exp_x_at_0():
method calc_Dx_this_mul_exp_x_at_0 (line 132) | def calc_Dx_this_mul_exp_x_at_0(self, x):
method calc_Dx_exp_x_at_0 (line 139) | def calc_Dx_exp_x_at_0(x):
method Dxi_x_matrix (line 144) | def Dxi_x_matrix(x, i):
method calc_Dxi_x_matrix (line 154) | def calc_Dxi_x_matrix(x, i):
method Dxi_exp_x_matrix (line 159) | def Dxi_exp_x_matrix(x, i):
method calc_Dxi_exp_x_matrix (line 166) | def calc_Dxi_exp_x_matrix(x, i):
method Dxi_exp_x_matrix_at_0 (line 171) | def Dxi_exp_x_matrix_at_0(i):
method calc_Dxi_exp_x_matrix_at_0 (line 177) | def calc_Dxi_exp_x_matrix_at_0(x, i):
class TestSe3 (line 184) | class TestSe3(unittest.TestCase):
method setUp (line 185) | def setUp(self):
method test_exp_log (line 199) | def test_exp_log(self):
method test_matrix (line 207) | def test_matrix(self):
method test_derivatives (line 216) | def test_derivatives(self):
method test_codegen (line 236) | def test_codegen(self):
FILE: Thirdparty/Sophus/sympy/sophus/so2.py
class So2 (line 12) | class So2:
method __init__ (line 15) | def __init__(self, z):
method exp (line 20) | def exp(theta):
method log (line 27) | def log(self):
method calc_Dx_log_this (line 31) | def calc_Dx_log_this(self):
method calc_Dx_log_exp_x_times_this_at_0 (line 34) | def calc_Dx_log_exp_x_times_this_at_0(self, x):
method __repr__ (line 37) | def __repr__(self):
method hat (line 41) | def hat(theta):
method matrix (line 45) | def matrix(self):
method __mul__ (line 51) | def __mul__(self, right):
method __getitem__ (line 61) | def __getitem__(self, key):
method calc_Dx_exp_x (line 65) | def calc_Dx_exp_x(x):
method Dx_exp_x_at_0 (line 70) | def Dx_exp_x_at_0():
method calc_Dx_exp_x_at_0 (line 74) | def calc_Dx_exp_x_at_0(x):
method calc_Dx_this_mul_exp_x_at_0 (line 77) | def calc_Dx_this_mul_exp_x_at_0(self, x):
method Dxi_x_matrix (line 83) | def Dxi_x_matrix(x, i):
method calc_Dxi_x_matrix (line 92) | def calc_Dxi_x_matrix(x, i):
method Dx_exp_x_matrix (line 97) | def Dx_exp_x_matrix(x):
method calc_Dx_exp_x_matrix (line 104) | def calc_Dx_exp_x_matrix(x):
method Dx_exp_x_matrix_at_0 (line 109) | def Dx_exp_x_matrix_at_0():
method calc_Dx_exp_x_matrix_at_0 (line 113) | def calc_Dx_exp_x_matrix_at_0(x):
class TestSo2 (line 119) | class TestSo2(unittest.TestCase):
method setUp (line 120) | def setUp(self):
method test_exp_log (line 128) | def test_exp_log(self):
method test_matrix (line 133) | def test_matrix(self):
method test_derivatives (line 142) | def test_derivatives(self):
method test_codegen (line 160) | def test_codegen(self):
FILE: Thirdparty/Sophus/sympy/sophus/so3.py
class So3 (line 13) | class So3:
method __init__ (line 16) | def __init__(self, q):
method exp (line 21) | def exp(v):
method log (line 30) | def log(self):
method calc_Dx_log_this (line 35) | def calc_Dx_log_this(self):
method calc_Dx_log_exp_x_times_this_at_0 (line 39) | def calc_Dx_log_exp_x_times_this_at_0(self, x):
method __repr__ (line 44) | def __repr__(self):
method inverse (line 47) | def inverse(self):
method hat (line 51) | def hat(o):
method vee (line 70) | def vee(Omega):
method matrix (line 76) | def matrix(self):
method __mul__ (line 98) | def __mul__(self, right):
method __getitem__ (line 108) | def __getitem__(self, key):
method calc_Dx_exp_x (line 112) | def calc_Dx_exp_x(x):
method Dx_exp_x_at_0 (line 117) | def Dx_exp_x_at_0():
method calc_Dx_exp_x_at_0 (line 124) | def calc_Dx_exp_x_at_0(x):
method calc_Dx_this_mul_exp_x_at_0 (line 127) | def calc_Dx_this_mul_exp_x_at_0(self, x):
method calc_Dx_exp_x_mul_this_at_0 (line 132) | def calc_Dx_exp_x_mul_this_at_0(self, x):
method Dxi_x_matrix (line 138) | def Dxi_x_matrix(x, i):
method calc_Dxi_x_matrix (line 157) | def calc_Dxi_x_matrix(x, i):
method Dxi_exp_x_matrix (line 162) | def Dxi_exp_x_matrix(x, i):
method calc_Dxi_exp_x_matrix (line 169) | def calc_Dxi_exp_x_matrix(x, i):
method Dxi_exp_x_matrix_at_0 (line 174) | def Dxi_exp_x_matrix_at_0(i):
method calc_Dxi_exp_x_matrix_at_0 (line 180) | def calc_Dxi_exp_x_matrix_at_0(x, i):
class TestSo3 (line 186) | class TestSo3(unittest.TestCase):
method setUp (line 187) | def setUp(self):
method test_exp_log (line 197) | def test_exp_log(self):
method test_matrix (line 205) | def test_matrix(self):
method test_derivatives (line 214) | def test_derivatives(self):
method test_codegen (line 233) | def test_codegen(self):
FILE: Thirdparty/Sophus/test/ceres/test_ceres_rxso2.cpp
type RotationalPart<Sophus::RxSO2d> (line 11) | struct RotationalPart<Sophus::RxSO2d> {
method Norm (line 12) | static double Norm(const typename Sophus::RxSO2d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_rxso3.cpp
type RotationalPart<Sophus::RxSO3d> (line 11) | struct RotationalPart<Sophus::RxSO3d> {
method Norm (line 12) | static double Norm(const typename Sophus::RxSO3d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_se2.cpp
type RotationalPart<Sophus::SE2d> (line 11) | struct RotationalPart<Sophus::SE2d> {
method Norm (line 12) | static double Norm(const typename Sophus::SE2d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp
type RotationalPart<Sophus::SE3d> (line 11) | struct RotationalPart<Sophus::SE3d> {
method Norm (line 12) | static double Norm(const typename Sophus::SE3d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_sim2.cpp
type RotationalPart<Sophus::Sim2d> (line 11) | struct RotationalPart<Sophus::Sim2d> {
method Norm (line 12) | static double Norm(const typename Sophus::Sim2d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_sim3.cpp
type RotationalPart<Sophus::Sim3d> (line 11) | struct RotationalPart<Sophus::Sim3d> {
method Norm (line 12) | static double Norm(const typename Sophus::Sim3d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_so2.cpp
type RotationalPart<Sophus::SO2d> (line 11) | struct RotationalPart<Sophus::SO2d> {
method Norm (line 12) | static double Norm(const typename Sophus::SO2d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/test_ceres_so3.cpp
type RotationalPart<Sophus::SO3d> (line 11) | struct RotationalPart<Sophus::SO3d> {
method Norm (line 12) | static double Norm(const typename Sophus::SO3d::Tangent &t) {
function main (line 17) | int main(int, char **) {
FILE: Thirdparty/Sophus/test/ceres/tests.hpp
type RotationalPart (line 9) | struct RotationalPart
type Sophus (line 11) | namespace Sophus {
function dot (line 14) | double dot(const Vector<double, N>& v1, const Vector<double, N>& v2) {
function dot (line 18) | double dot(const double& a, const double& b) { return a * b; }
function squaredNorm (line 21) | double squaredNorm(const Vector<double, N>& vec) {
function squaredNorm (line 25) | double squaredNorm(const double& scalar) { return scalar * scalar; }
function T (line 28) | T Zero() {
type Random (line 52) | struct Random {
method T (line 54) | T static sample(R& rng) {
type Random<double> (line 67) | struct Random<double> {
method T (line 71) | T static sample(R& rng) {
type LieGroupCeresTests (line 78) | struct LieGroupCeresTests {
type TestLieGroupCostFunctor (line 90) | struct TestLieGroupCostFunctor {
method EIGEN_MAKE_ALIGNED_OPERATOR_NEW (line 91) | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
type TestPointCostFunctor (line 124) | struct TestPointCostFunctor {
type TestGraphFunctor (line 157) | struct TestGraphFunctor {
method TestGraphFunctor (line 171) | TestGraphFunctor(const LieGroupd& diff) : diff(diff) {}
method testAll (line 175) | bool testAll() {
method testAveraging (line 207) | bool testAveraging(const size_t num_vertices, const double sigma_init,
method test (line 281) | bool test(LieGroupd const& T_w_targ, LieGroupd const& T_w_init,
method testManifold (line 329) | bool testManifold(const LieGroupd& x, const LieGroupd& y) {
method xPlusZeroIsXAt (line 362) | bool xPlusZeroIsXAt(const LieGroupd& x) {
method xMinusXIsZeroAt (line 377) | bool xMinusXIsZeroAt(const LieGroupd& x) {
method minusPlusIsIdentityAt (line 392) | bool minusPlusIsIdentityAt(const LieGroupd& x, const Tangentd& delta) {
method plusMinusIsIdentityAt (line 416) | bool plusMinusIsIdentityAt(const LieGroupd& x, const LieGroupd& y) {
method minusPlusJacobianIsIdentityAt (line 435) | bool minusPlusJacobianIsIdentityAt(const LieGroupd& x) {
method LieGroupCeresTests (line 462) | LieGroupCeresTests(const StdVector<LieGroupd>& group_vec,
FILE: Thirdparty/Sophus/test/core/test_cartesian2.cpp
type Eigen (line 8) | namespace Eigen {
class Map<Sophus::Cartesian<double, 2>> (line 9) | class Map<Sophus::Cartesian<double, 2>>
class Map<Sophus::Cartesian<double, 2> const> (line 10) | class Map<Sophus::Cartesian<double, 2> const>
type Sophus (line 13) | namespace Sophus {
class Cartesian<double, 2, Eigen::AutoAlign> (line 15) | class Cartesian<double, 2, Eigen::AutoAlign>
class Cartesian<float, 2, Eigen::DontAlign> (line 16) | class Cartesian<float, 2, Eigen::DontAlign>
class Cartesian<ceres::Jet<double, 2>, 2> (line 18) | class Cartesian<ceres::Jet<double, 2>, 2>
class Tests (line 22) | class Tests {
method Tests (line 28) | Tests() {
method runAll (line 54) | void runAll() {
method testLieProperties (line 60) | bool testLieProperties() {
function test_cartesian (line 73) | int test_cartesian() {
function main (line 93) | int main() { return Sophus::test_cartesian(); }
FILE: Thirdparty/Sophus/test/core/test_cartesian3.cpp
type Eigen (line 8) | namespace Eigen {
class Map<Sophus::Cartesian<double, 3>> (line 9) | class Map<Sophus::Cartesian<double, 3>>
class Map<Sophus::Cartesian<double, 3> const> (line 10) | class Map<Sophus::Cartesian<double, 3> const>
type Sophus (line 13) | namespace Sophus {
class Cartesian<double, 3, Eigen::AutoAlign> (line 15) | class Cartesian<double, 3, Eigen::AutoAlign>
class Cartesian<float, 3, Eigen::DontAlign> (line 16) | class Cartesian<float, 3, Eigen::DontAlign>
class Cartesian<ceres::Jet<double, 3>, 3> (line 18) | class Cartesian<ceres::Jet<double, 3>, 3>
class Tests (line 22) | class Tests {
method Tests (line 28) | Tests() {
method runAll (line 54) | void runAll() {
method testLieProperties (line 60) | bool testLieProperties() {
function test_cartesian (line 73) | int test_cartesian() {
function main (line 93) | int main() { return Sophus::test_cartesian(); }
FILE: Thirdparty/Sophus/test/core/test_common.cpp
type Sophus (line 9) | namespace Sophus {
function testSmokeDetails (line 13) | bool testSmokeDetails() {
function testSpline (line 34) | bool testSpline() {
function runAll (line 113) | void runAll() {
function main (line 123) | int main() { Sophus::runAll(); }
FILE: Thirdparty/Sophus/test/core/test_geometry.cpp
type Sophus (line 7) | namespace Sophus {
function test2dGeometry (line 12) | bool test2dGeometry() {
function test3dGeometry (line 52) | bool test3dGeometry() {
function runAll (line 116) | void runAll() {
function main (line 131) | int main() { Sophus::runAll(); }
FILE: Thirdparty/Sophus/test/core/test_rxso2.cpp
type Eigen (line 8) | namespace Eigen {
class Map<Sophus::RxSO2<double>> (line 9) | class Map<Sophus::RxSO2<double>>
class Map<Sophus::RxSO2<double> const> (line 10) | class Map<Sophus::RxSO2<double> const>
type Sophus (line 13) | namespace Sophus {
class RxSO2<double, Eigen::AutoAlign> (line 15) | class RxSO2<double, Eigen::AutoAlign>
class RxSO2<float, Eigen::DontAlign> (line 16) | class RxSO2<float, Eigen::DontAlign>
class RxSO2<ceres::Jet<double, 3>> (line 18) | class RxSO2<ceres::Jet<double, 3>>
class Tests (line 22) | class Tests {
method Tests (line 31) | Tests() {
method testFit (line 66) | enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
method testFit (line 87) | enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
method runAll (line 91) | void runAll() {
method testLieProperties (line 101) | bool testLieProperties() {
method testSaturation (line 106) | bool testSaturation() {
method testRawDataAcces (line 159) | bool testRawDataAcces() {
method testConstructors (line 238) | bool testConstructors() {
function test_rxso2 (line 284) | int test_rxso2() {
function main (line 303) | int main() { return Sophus::test_rxso2(); }
FILE: Thirdparty/Sophus/test/core/test_rxso3.cpp
type Eigen (line 8) | namespace Eigen {
class Map<Sophus::RxSO3<double>> (line 9) | class Map<Sophus::RxSO3<double>>
class Map<Sophus::RxSO3<double> const> (line 10) | class Map<Sophus::RxSO3<double> const>
type Sophus (line 13) | namespace Sophus {
class RxSO3<double, Eigen::AutoAlign> (line 15) | class RxSO3<double, Eigen::AutoAlign>
class RxSO3<float, Eigen::DontAlign> (line 16) | class RxSO3<float, Eigen::DontAlign>
class RxSO3<ceres::Jet<double, 3>> (line 18) | class RxSO3<ceres::Jet<double, 3>>
class Tests (line 22) | class Tests {
method Tests (line 32) | Tests() {
method runAll (line 81) | void runAll() {
method testLieProperties (line 91) | bool testLieProperties() {
method testSaturation (line 96) | bool testSaturation() {
method testRawDataAcces (line 145) | bool testRawDataAcces() {
method testConstructors (line 235) | bool testConstructors() {
method testFit (line 276) | enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
method testFit (line 297) | enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
function test_rxso3 (line 306) | int test_rxso3() {
function main (line 326) | int main() { return Sophus::test_rxso3(); }
FILE: Thirdparty/Sophus/test/core/test_se2.cpp
type Eigen (line 9) | namespace Eigen {
class Map<Sophus::SE2<double>> (line 10) | class Map<Sophus::SE2<double>>
class Map<Sophus::SE2<double> const> (line 11) | class Map<Sophus::SE2<double> const>
type Sophus (line 14) | namespace Sophus {
class SE2<double, Eigen::AutoAlign> (line 16) | class SE2<double, Eigen::AutoAlign>
class SE2<double, Eigen::DontAlign> (line 17) | class SE2<double, Eigen::DontAlign>
class SE2<ceres::Jet<double, 3>> (line 19) | class SE2<ceres::Jet<double, 3>>
class Tests (line 23) | class Tests {
method Tests (line 31) | Tests() {
method runAll (line 70) | void runAll() {
method testLieProperties (line 80) | bool testLieProperties() {
method testRawDataAcces (line 85) | bool testRawDataAcces() {
method testMutatingAccessors (line 177) | bool testMutatingAccessors() {
method testConstructors (line 195) | bool testConstructors() {
method testFit (line 220) | enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
method testFit (line 234) | enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
function test_se2 (line 243) | int test_se2() {
function main (line 262) | int main() { return Sophus::test_se2(); }
FILE: Thirdparty/Sophus/test/core/test_se3.cpp
type Eigen (line 8) | namespace Eigen {
class Map<Sophus::SE3<double>> (line 9) | class Map<Sophus::SE3<double>>
class Map<Sophus::SE3<double> const> (line 10) | class Map<Sophus::SE3<double> const>
type Sophus (line 13) | namespace Sophus {
class SE3<double, Eigen::AutoAlign> (line 15) | class SE3<double, Eigen::AutoAlign>
class SE3<float, Eigen::DontAlign> (line 16) | class SE3<float, Eigen::DontAlign>
class SE3<ceres::Jet<double, 3>> (line 18) | class SE3<ceres::Jet<double, 3>>
class Tests (line 22) | class Tests {
method Tests (line 30) | Tests() {
method runAll (line 54) | void runAll() {
method testLieProperties (line 64) | bool testLieProperties() {
method testRawDataAcces (line 69) | bool testRawDataAcces() {
method testMutatingAccessors (line 184) | bool testMutatingAccessors() {
method testConstructors (line 195) | bool testConstructors() {
method testFit (line 218) | enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
method testFit (line 242) | enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
function test_se3 (line 251) | int test_se3() {
function main (line 270) | int main() { return Sophus::test_se3(); }
FILE: Thirdparty/Sophus/test/core/test_sim2.cpp
type Eigen (line 10) | namespace Eigen {
class Map<Sophus::Sim2<double>> (line 11) | class Map<Sophus::Sim2<double>>
class Map<Sophus::Sim2<double> const> (line 12) | class Map<Sophus::Sim2<double> const>
type Sophus (line 15) | namespace Sophus {
class Sim2<double, Eigen::AutoAlign> (line 17) | class Sim2<double, Eigen::AutoAlign>
class Sim2<float, Eigen::DontAlign> (line 18) | class Sim2<float, Eigen::DontAlign>
class Sim2<ceres::Jet<double, 3>> (line 20) | class Sim2<ceres::Jet<double, 3>>
class Tests (line 24) | class Tests {
method Tests (line 33) | Tests() {
method runAll (line 75) | void runAll() {
method testLieProperties (line 83) | bool testLieProperties() {
method testRawDataAcces (line 88) | bool testRawDataAcces() {
method testConstructors (line 175) | bool testConstructors() {
function test_sim3 (line 208) | int test_sim3() {
function main (line 227) | int main() { return Sophus::test_sim3(); }
FILE: Thirdparty/Sophus/test/core/test_sim3.cpp
type Eigen (line 10) | namespace Eigen {
class Map<Sophus::Sim3<double>> (line 11) | class Map<Sophus::Sim3<double>>
class Map<Sophus::Sim3<double> const> (line 12) | class Map<Sophus::Sim3<double> const>
type Sophus (line 15) | namespace Sophus {
class Sim3<double, Eigen::AutoAlign> (line 17) | class Sim3<double, Eigen::AutoAlign>
class Sim3<float, Eigen::DontAlign> (line 18) | class Sim3<float, Eigen::DontAlign>
class Sim3<ceres::Jet<double, 3>> (line 20) | class Sim3<ceres::Jet<double, 3>>
class Tests (line 24) | class Tests {
method Tests (line 33) | Tests() {
method runAll (line 113) | void runAll() {
method testLieProperties (line 121) | bool testLieProperties() {
method testRawDataAcces (line 126) | bool testRawDataAcces() {
method testConstructors (line 220) | bool testConstructors() {
function test_sim3 (line 262) | int test_sim3() {
function main (line 281) | int main() { return Sophus::test_sim3(); }
FILE: Thirdparty/Sophus/test/core/test_so2.cpp
type Eigen (line 8) | namespace Eigen {
class Map<Sophus::SO2<double>> (line 9) | class Map<Sophus::SO2<double>>
class Map<Sophus::SO2<double> const> (line 10) | class Map<Sophus::SO2<double> const>
type Sophus (line 13) | namespace Sophus {
class SO2<double, Eigen::AutoAlign> (line 15) | class SO2<double, Eigen::AutoAlign>
class SO2<float, Eigen::DontAlign> (line 16) | class SO2<float, Eigen::DontAlign>
class SO2<ceres::Jet<double, 3>> (line 18) | class SO2<ceres::Jet<double, 3>>
class Tests (line 22) | class Tests {
method Tests (line 29) | Tests() {
method runAll (line 51) | void runAll() {
method testLieProperties (line 61) | bool testLieProperties() {
method testUnity (line 66) | bool testUnity() {
method testRawDataAcces (line 80) | bool testRawDataAcces() {
method testConstructors (line 137) | bool testConstructors() {
method testFit (line 148) | enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
method testFit (line 163) | enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
function test_so2 (line 172) | int test_so2() {
function main (line 191) | int main() { return Sophus::test_so2(); }
FILE: Thirdparty/Sophus/test/core/test_so3.cpp
type Eigen (line 9) | namespace Eigen {
class Map<Sophus::SO3<double>> (line 10) | class Map<Sophus::SO3<double>>
class Map<Sophus::SO3<double> const> (line 11) | class Map<Sophus::SO3<double> const>
type Sophus (line 14) | namespace Sophus {
class SO3<double, Eigen::AutoAlign> (line 16) | class SO3<double, Eigen::AutoAlign>
class SO3<float, Eigen::DontAlign> (line 17) | class SO3<float, Eigen::DontAlign>
class SO3<ceres::Jet<double, 3>> (line 19) | class SO3<ceres::Jet<double, 3>>
class Tests (line 23) | class Tests {
method Tests (line 30) | Tests() {
method runAll (line 65) | void runAll() {
method testLieProperties (line 76) | bool testLieProperties() {
method testUnity (line 81) | bool testUnity() {
method testRawDataAcces (line 95) | bool testRawDataAcces() {
method testConstructors (line 172) | bool testConstructors() {
method testSampleUniformSymmetry (line 183) | enable_if_t<std::is_floating_point<S>::value, bool>
method testSampleUniformSymmetry (line 233) | enable_if_t<!std::is_floating_point<S>::value, bool>
method testFit (line 239) | enable_if_t<std::is_floating_point<S>::value, bool> testFit() {
method testFit (line 264) | enable_if_t<!std::is_floating_point<S>::value, bool> testFit() {
function test_so3 (line 273) | int test_so3() {
function main (line 292) | int main() { return Sophus::test_so3(); }
FILE: Thirdparty/Sophus/test/core/test_velocities.cpp
type Sophus (line 7) | namespace Sophus {
type experimental (line 8) | namespace experimental {
function tests_linear_velocities (line 11) | bool tests_linear_velocities() {
function tests_rotational_velocities (line 57) | bool tests_rotational_velocities() {
function test_velocities (line 110) | int test_velocities() {
function main (line 130) | int main() { return Sophus::experimental::test_velocities(); }
FILE: Thirdparty/Sophus/test/core/tests.hpp
type Sophus (line 19) | namespace Sophus {
function through (line 29) | Hyperplane<Scalar, 2> through(const Vector<Scalar, 2>* points) {
function through (line 34) | Hyperplane<Scalar, 3> through(const Vector<Scalar, 3>* points) {
class LieGroupTests (line 39) | class LieGroupTests {
method LieGroupTests (line 56) | LieGroupTests(
method adjointTest (line 66) | bool adjointTest() {
method leftJacobianTest (line 89) | enable_if_t<std::is_same<G, SO3<Scalar>>::value ||
method leftJacobianTest (line 122) | enable_if_t<!(std::is_same<G, SO3<Scalar>>::value ||
method moreJacobiansTest (line 129) | bool moreJacobiansTest() {
method contructorAndAssignmentTest (line 146) | bool contructorAndAssignmentTest() {
method derivativeTest (line 197) | bool derivativeTest() {
method additionalDerivativeTest (line 219) | bool additionalDerivativeTest() {
method productTest (line 275) | bool productTest() {
method expLogTest (line 289) | bool expLogTest() {
method expMapTest (line 301) | bool expMapTest() {
method groupActionTest (line 313) | bool groupActionTest() {
method lineActionTest (line 342) | bool lineActionTest() {
method planeActionTest (line 368) | bool planeActionTest() {
method lieBracketTest (line 396) | bool lieBracketTest() {
method veeHatTest (line 413) | bool veeHatTest() {
method newDeleteSmokeTest (line 423) | bool newDeleteSmokeTest() {
method interpolateAndMeanTest (line 432) | bool interpolateAndMeanTest() {
method testRandomSmoke (line 557) | bool testRandomSmoke() {
method testSpline (line 568) | enable_if_t<std::is_same<S, float>::value, bool> testSpline() {
method testSpline (line 574) | enable_if_t<!std::is_same<S, float>::value, bool> testSpline() {
method doAllTestsPass (line 645) | enable_if_t<std::is_floating_point<S>::value, bool> doAllTestsPass() {
method doAllTestsPass (line 650) | enable_if_t<!std::is_floating_point<S>::value, bool> doAllTestsPass() {
method doesSmallTestSetPass (line 655) | bool doesSmallTestSetPass() {
method doesLargeTestSetPass (line 670) | bool doesLargeTestSetPass() {
method map (line 687) | Eigen::Matrix<Scalar, N - 1, 1> map(
method map (line 694) | Eigen::Matrix<Scalar, N, 1> map(Eigen::Matrix<Scalar, N, N> const& T,
function getTestSE3s (line 705) | std::vector<SE3<Scalar>, Eigen::aligned_allocator<SE3<Scalar>>> getTes...
function getTestSE2s (line 744) | std::vector<SE2<T>, Eigen::aligned_allocator<SE2<T>>> getTestSE2s() {
FILE: Thirdparty/tessil-src/include/tsl/robin_growth_policy.h
function namespace (line 70) | namespace tsl {
FILE: Thirdparty/tessil-src/include/tsl/robin_hash.h
function namespace (line 45) | namespace tsl {
function clear (line 222) | void clear() noexcept {
function value_type (line 238) | const value_type& value() const noexcept {
function set_as_last_bucket (line 251) | void set_as_last_bucket() noexcept {
function truncated_hash_type (line 287) | static truncated_hash_type truncate_hash(std::size_t hash) noexcept {
function USE_STORED_HASH_ON_REHASH (line 395) | static bool USE_STORED_HASH_ON_REHASH(size_type bucket_count) {
function m_bucket (line 437) | robin_iterator(bucket_entry_ptr bucket) noexcept: m_bucket(bucket) {
function reference (line 475) | reference operator*() const {
function pointer (line 479) | pointer operator->() const {
function iterator (line 667) | iterator begin() noexcept {
function const_iterator (line 676) | const_iterator begin() const noexcept {
function iterator (line 689) | iterator end() noexcept {
function const_iterator (line 693) | const_iterator end() const noexcept {
function clear (line 720) | void clear() noexcept {
function iterator (line 826) | iterator erase(iterator pos) {
function iterator (line 842) | iterator erase(const_iterator pos) {
function iterator (line 846) | iterator erase(const_iterator first, const_iterator last) {
function swap (line 926) | void swap(robin_hash& other) {
function typename (line 959) | typename U::value_type& at(const K& key) const {
function typename (line 964) | typename U::value_type& at(const K& key, std::size_t hash) const {
function hash (line 1002) | size_t hash) {
function size_type (line 1059) | size_type max_bucket_count() const {
function min_load_factor (line 1082) | void min_load_factor(float ml) {
function max_load_factor (line 1087) | void max_load_factor(float ml) {
function rehash (line 1093) | void rehash(size_type count) {
function reserve (line 1098) | void reserve(size_type count) {
function iterator (line 1117) | iterator mutable_iterator(const_iterator pos) {
function next_bucket (line 1140) | size_t next_bucket(std::size_t index) const noexcept {
function next_bucket (line 1147) | size_t next_bucket(std::size_t index) const noexcept {
function hash (line 1157) | size_t hash) {
function erase_from_bucket (line 1180) | void erase_from_bucket(iterator pos) {
function insert_value (line 1262) | void insert_value(std::size_t ibucket, distance_type dist_from_ideal_buc...
function insert_value_impl (line 1275) | void insert_value_impl(std::size_t ibucket, distance_type dist_from_idea...
function rehash_impl (line 1303) | void rehash_impl(size_type count) {
FILE: Thirdparty/tessil-src/include/tsl/robin_map.h
function namespace (line 37) | namespace tsl {
function iterator (line 229) | iterator begin() noexcept { return m_ht.begin(); }
function iterator (line 233) | iterator end() noexcept { return m_ht.end(); }
function clear (line 248) | void clear() noexcept { m_ht.clear(); }
function iterator (line 266) | iterator insert(const_iterator hint, const value_type& value) {
function iterator (line 275) | iterator insert(const_iterator hint, value_type&& value) {
function insert (line 285) | void insert(std::initializer_list<value_type> ilist) {
function iterator (line 364) | iterator erase(iterator pos) { return m_ht.erase(pos); }
function iterator (line 365) | iterator erase(const_iterator pos) { return m_ht.erase(pos); }
function iterator (line 366) | iterator erase(const_iterator first, const_iterator last) { return m_ht....
function size_type (line 367) | size_type erase(const key_type& key) { return m_ht.erase(key); }
function size_type (line 373) | size_type erase(const key_type& key, std::size_t precalculated_hash) {
function precalculated_hash (line 391) | size_t precalculated_hash) {
function swap (line 397) | void swap(robin_map& other) { other.m_ht.swap(m_ht); }
function T (line 413) | const T& at(const Key& key) const { return m_ht.at(key); }
function T (line 418) | const T& at(const Key& key, std::size_t precalculated_hash) const { retu...
function size_type (line 459) | size_type count(const Key& key) const { return m_ht.count(key); }
function size_type (line 465) | size_type count(const Key& key, std::size_t precalculated_hash) const {
function iterator (line 488) | iterator find(const Key& key) { return m_ht.find(key); }
function iterator (line 494) | iterator find(const Key& key, std::size_t precalculated_hash) { return m...
function const_iterator (line 496) | const_iterator find(const Key& key) const { return m_ht.find(key); }
function const_iterator (line 501) | const_iterator find(const Key& key, std::size_t precalculated_hash) const {
function precalculated_hash (line 519) | size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }
function contains (line 541) | bool contains(const Key& key) const { return m_ht.contains(key); }
function contains (line 547) | bool contains(const Key& key, std::size_t precalculated_hash) const {
function min_load_factor (line 650) | void min_load_factor(float ml) { m_ht.min_load_factor(ml); }
function max_load_factor (line 651) | void max_load_factor(float ml) { m_ht.max_load_factor(ml); }
function rehash (line 653) | void rehash(size_type count) { m_ht.rehash(count); }
function reserve (line 654) | void reserve(size_type count) { m_ht.reserve(count); }
function iterator (line 670) | iterator mutable_iterator(const_iterator pos) {
FILE: Thirdparty/tessil-src/include/tsl/robin_set.h
function namespace (line 37) | namespace tsl {
function explicit (line 127) | explicit robin_set(size_type bucket_count,
function iterator (line 214) | iterator begin() noexcept { return m_ht.begin(); }
function iterator (line 218) | iterator end() noexcept { return m_ht.end(); }
function clear (line 233) | void clear() noexcept { m_ht.clear(); }
function iterator (line 246) | iterator insert(const_iterator hint, const value_type& value) {
function iterator (line 250) | iterator insert(const_iterator hint, value_type&& value) {
function insert (line 259) | void insert(std::initializer_list<value_type> ilist) {
function iterator (line 292) | iterator erase(iterator pos) { return m_ht.erase(pos); }
function iterator (line 293) | iterator erase(const_iterator pos) { return m_ht.erase(pos); }
function iterator (line 294) | iterator erase(const_iterator first, const_iterator last) { return m_ht....
function size_type (line 295) | size_type erase(const key_type& key) { return m_ht.erase(key); }
function size_type (line 301) | size_type erase(const key_type& key, std::size_t precalculated_hash) {
function precalculated_hash (line 319) | size_t precalculated_hash) {
function swap (line 325) | void swap(robin_set& other) { other.m_ht.swap(m_ht); }
function size_type (line 332) | size_type count(const Key& key) const { return m_ht.count(key); }
function size_type (line 338) | size_type count(const Key& key, std::size_t precalculated_hash) const { ...
function iterator (line 359) | iterator find(const Key& key) { return m_ht.find(key); }
function iterator (line 365) | iterator find(const Key& key, std::size_t precalculated_hash) { return m...
function const_iterator (line 367) | const_iterator find(const Key& key) const { return m_ht.find(key); }
function const_iterator (line 372) | const_iterator find(const Key& key, std::size_t precalculated_hash) cons...
function precalculated_hash (line 388) | size_t precalculated_hash) { return m_ht.find(key, precalculated_hash); }
function contains (line 408) | bool contains(const Key& key) const { return m_ht.contains(key); }
function contains (line 414) | bool contains(const Key& key, std::size_t precalculated_hash) const {
function min_load_factor (line 516) | void min_load_factor(float ml) { m_ht.min_load_factor(ml); }
function max_load_factor (line 517) | void max_load_factor(float ml) { m_ht.max_load_factor(ml); }
function rehash (line 519) | void rehash(size_type count) { m_ht.rehash(count); }
function reserve (line 520) | void reserve(size_type count) { m_ht.reserve(count); }
function iterator (line 537) | iterator mutable_iterator(const_iterator pos) {
FILE: Thirdparty/tessil-src/tests/custom_allocator_tests.cpp
class custom_allocator (line 43) | class custom_allocator {
type rebind (line 56) | struct rebind {
method custom_allocator (line 60) | custom_allocator() = default;
method custom_allocator (line 61) | custom_allocator(const custom_allocator&) = default;
method custom_allocator (line 64) | custom_allocator(const custom_allocator<U>&) {
method pointer (line 67) | pointer address(reference x) const noexcept {
method const_pointer (line 71) | const_pointer address(const_reference x) const noexcept {
method pointer (line 75) | pointer allocate(size_type n, const void* /*hint*/ = 0) {
method deallocate (line 90) | void deallocate(T* p, size_type /*n*/) {
method size_type (line 94) | size_type max_size() const noexcept {
method construct (line 99) | void construct(U* p, Args&&... args) {
method destroy (line 104) | void destroy(U* p) {
function BOOST_AUTO_TEST_CASE (line 134) | BOOST_AUTO_TEST_CASE(test_custom_allocator_1) {
FILE: Thirdparty/tessil-src/tests/policy_tests.cpp
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 46) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_policy, Policy, test_types) {
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 75) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_policy_min_bucket_count, Policy, test...
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 83) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_policy_max_bucket_count, Policy, test...
FILE: Thirdparty/tessil-src/tests/robin_map_tests.cpp
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 81) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_insert, HMap, test_types) {
function BOOST_AUTO_TEST_CASE (line 117) | BOOST_AUTO_TEST_CASE(test_range_insert) {
function BOOST_AUTO_TEST_CASE (line 141) | BOOST_AUTO_TEST_CASE(test_insert_with_hint) {
function BOOST_AUTO_TEST_CASE (line 168) | BOOST_AUTO_TEST_CASE(test_emplace_hint) {
function BOOST_AUTO_TEST_CASE (line 200) | BOOST_AUTO_TEST_CASE(test_emplace) {
function BOOST_AUTO_TEST_CASE (line 226) | BOOST_AUTO_TEST_CASE(test_try_emplace) {
function BOOST_AUTO_TEST_CASE (line 244) | BOOST_AUTO_TEST_CASE(test_try_emplace_2) {
function BOOST_AUTO_TEST_CASE (line 276) | BOOST_AUTO_TEST_CASE(test_try_emplace_hint) {
function BOOST_AUTO_TEST_CASE (line 299) | BOOST_AUTO_TEST_CASE(test_insert_or_assign) {
function BOOST_AUTO_TEST_CASE (line 318) | BOOST_AUTO_TEST_CASE(test_insert_or_assign_hint) {
function BOOST_AUTO_TEST_CASE (line 342) | BOOST_AUTO_TEST_CASE(test_range_erase_all) {
function BOOST_AUTO_TEST_CASE (line 354) | BOOST_AUTO_TEST_CASE(test_range_erase) {
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 374) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_erase_loop, HMap, test_types) {
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 396) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_erase_loop_range, HMap, test_types) {
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 416) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_insert_erase_insert, HMap, test_types) {
function BOOST_AUTO_TEST_CASE (line 473) | BOOST_AUTO_TEST_CASE(test_range_erase_same_iterators) {
function BOOST_AUTO_TEST_CASE (line 493) | BOOST_AUTO_TEST_CASE(test_max_load_factor_extreme_factors) {
function BOOST_AUTO_TEST_CASE (line 506) | BOOST_AUTO_TEST_CASE(test_min_load_factor_extreme_factors) {
function BOOST_AUTO_TEST_CASE (line 523) | BOOST_AUTO_TEST_CASE(test_min_load_factor) {
function BOOST_AUTO_TEST_CASE (line 554) | BOOST_AUTO_TEST_CASE(test_min_load_factor_range_erase) {
function BOOST_AUTO_TEST_CASE (line 588) | BOOST_AUTO_TEST_CASE(test_rehash_empty) {
function BOOST_AUTO_TEST_CASE (line 615) | BOOST_AUTO_TEST_CASE(test_compare) {
function BOOST_AUTO_TEST_CASE (line 663) | BOOST_AUTO_TEST_CASE(test_clear) {
function BOOST_AUTO_TEST_CASE (line 680) | BOOST_AUTO_TEST_CASE(test_clear_with_min_load_factor) {
function BOOST_AUTO_TEST_CASE (line 703) | BOOST_AUTO_TEST_CASE(test_modify_value_through_iterator) {
function BOOST_AUTO_TEST_CASE (line 724) | BOOST_AUTO_TEST_CASE(test_modify_value_through_iterator_with_const_quali...
function BOOST_AUTO_TEST_CASE (line 736) | BOOST_AUTO_TEST_CASE(test_extreme_bucket_count_value_construction) {
function BOOST_AUTO_TEST_CASE (line 767) | BOOST_AUTO_TEST_CASE(test_range_construct) {
function BOOST_AUTO_TEST_CASE (line 778) | BOOST_AUTO_TEST_CASE(test_assign_operator) {
function BOOST_AUTO_TEST_CASE (line 796) | BOOST_AUTO_TEST_CASE(test_move_constructor) {
function BOOST_AUTO_TEST_CASE (line 818) | BOOST_AUTO_TEST_CASE(test_move_constructor_empty) {
function BOOST_AUTO_TEST_CASE (line 829) | BOOST_AUTO_TEST_CASE(test_move_operator) {
function BOOST_AUTO_TEST_CASE (line 852) | BOOST_AUTO_TEST_CASE(test_move_operator_empty) {
function BOOST_AUTO_TEST_CASE (line 864) | BOOST_AUTO_TEST_CASE(test_reassign_moved_object_move_constructor) {
function BOOST_AUTO_TEST_CASE (line 877) | BOOST_AUTO_TEST_CASE(test_reassign_moved_object_move_operator) {
function BOOST_AUTO_TEST_CASE (line 890) | BOOST_AUTO_TEST_CASE(test_use_after_move_constructor) {
function BOOST_AUTO_TEST_CASE (line 912) | BOOST_AUTO_TEST_CASE(test_use_after_move_operator) {
function BOOST_AUTO_TEST_CASE (line 935) | BOOST_AUTO_TEST_CASE(test_copy_constructor_and_operator) {
function BOOST_AUTO_TEST_CASE (line 954) | BOOST_AUTO_TEST_CASE(test_copy_constructor_empty) {
function BOOST_AUTO_TEST_CASE (line 965) | BOOST_AUTO_TEST_CASE(test_copy_operator_empty) {
function BOOST_AUTO_TEST_CASE (line 981) | BOOST_AUTO_TEST_CASE(test_at) {
function BOOST_AUTO_TEST_CASE (line 993) | BOOST_AUTO_TEST_CASE(test_contains) {
function BOOST_AUTO_TEST_CASE (line 1004) | BOOST_AUTO_TEST_CASE(test_equal_range) {
function BOOST_AUTO_TEST_CASE (line 1020) | BOOST_AUTO_TEST_CASE(test_access_operator) {
function BOOST_AUTO_TEST_CASE (line 1036) | BOOST_AUTO_TEST_CASE(test_swap) {
function BOOST_AUTO_TEST_CASE (line 1053) | BOOST_AUTO_TEST_CASE(test_swap_empty) {
function BOOST_AUTO_TEST_CASE (line 1073) | BOOST_AUTO_TEST_CASE(test_key_equal) {
function BOOST_AUTO_TEST_CASE (line 1115) | BOOST_AUTO_TEST_CASE(test_heterogeneous_lookups) {
function BOOST_AUTO_TEST_CASE (line 1203) | BOOST_AUTO_TEST_CASE(test_empty_map) {
function BOOST_AUTO_TEST_CASE (line 1239) | BOOST_AUTO_TEST_CASE(test_precalculated_hash) {
FILE: Thirdparty/tessil-src/tests/robin_set_tests.cpp
function BOOST_AUTO_TEST_CASE_TEMPLATE (line 70) | BOOST_AUTO_TEST_CASE_TEMPLATE(test_insert, HSet, test_types) {
function BOOST_AUTO_TEST_CASE (line 101) | BOOST_AUTO_TEST_CASE(test_compare) {
function BOOST_AUTO_TEST_CASE (line 130) | BOOST_AUTO_TEST_CASE(test_insert_pointer) {
FILE: Thirdparty/tessil-src/tests/utils.h
function class (line 61) | class self_reference_member_test {
function class (line 116) | class move_only_test {
function class (line 174) | class copy_only_test {
function namespace (line 220) | namespace std {
function class (line 244) | class utils {
function counter (line 259) | size_t counter) {
function counter (line 264) | size_t counter) {
function counter (line 274) | size_t counter) {
function counter (line 279) | size_t counter) {
function counter (line 287) | size_t counter) {
function counter (line 292) | size_t counter) {
function counter (line 302) | size_t counter) {
function counter (line 307) | size_t counter) {
function HMap (line 314) | HMap utils::get_filled_hash_map(std::size_t nb_elements) {
FILE: include/common_lib.h
type i2ekf_lo (line 43) | typedef i2ekf_lo::Pose6D Pose6D;
type pcl (line 44) | typedef pcl::PointXYZINormal PointType;
type pcl (line 45) | typedef pcl::PointXYZRGB PointTypeRGB;
type pcl (line 46) | typedef pcl::PointCloud<PointType> PointCloudXYZI;
type pcl (line 47) | typedef pcl::PointCloud<PointTypeRGB> PointCloudXYZRGB;
type vector (line 48) | typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVec...
type Vector3d (line 49) | typedef Vector3d V3D;
type Matrix3d (line 50) | typedef Matrix3d M3D;
type Vector3f (line 51) | typedef Vector3f V3F;
type LID_TYPE (line 62) | enum LID_TYPE{AVIA = 1, VELO, OUSTER, L515, PANDAR, ROBOSENSE}
type POINT_TYPE (line 64) | enum POINT_TYPE{RAW = 1, UNDISTORT, WORLD}
type StatesGroup (line 66) | struct StatesGroup
function resetpose (line 151) | void resetpose()
type Point3D (line 269) | struct Point3D
type voxel (line 294) | struct voxel
function voxel (line 308) | inline static voxel coordinates(const Eigen::Vector3d &point, double vox...
type voxelBlock (line 320) | struct voxelBlock
function AddPoint (line 329) | void AddPoint(const Eigen::Vector3d &point) {
function Capacity (line 336) | inline int Capacity() { return num_points; }
type tsl (line 342) | typedef tsl::robin_map<voxel, voxelBlock> voxelHashMap;
function namespace (line 344) | namespace std
FILE: include/ikd-Tree/ikd_Tree.cpp
function BoxPointType (line 90) | BoxPointType KD_TREE::tree_range(){
type timespec (line 636) | struct timespec
type timespec (line 683) | struct timespec
type timespec (line 740) | struct timespec
type timespec (line 786) | struct timespec
function PointType_CMP (line 1314) | PointType_CMP MANUAL_HEAP::top(){
function Operation_Logger_Type (line 1382) | Operation_Logger_Type MANUAL_Q::front(){
function Operation_Logger_Type (line 1386) | Operation_Logger_Type MANUAL_Q::back(){
FILE: include/ikd-Tree/ikd_Tree.h
type pcl (line 21) | typedef pcl::PointXYZINormal PointType;
type vector (line 22) | typedef vector<PointType, Eigen::aligned_allocator<PointType>> PointVec...
type KD_TREE_NODE (line 26) | struct KD_TREE_NODE
type PointType_CMP (line 50) | struct PointType_CMP{
function operator (line 57) | bool operator < (const PointType_CMP &a)const{
type BoxPointType (line 63) | struct BoxPointType{
type operation_set (line 68) | enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSA...
type delete_point_storage_set (line 70) | enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THRE...
type Operation_Logger_Type (line 72) | struct Operation_Logger_Type{
function class (line 79) | class MANUAL_Q{
function class (line 94) | class MANUAL_HEAP
function class (line 113) | class KD_TREE
FILE: include/matplotlibcpp.h
function namespace (line 42) | namespace matplotlibcpp {
function backend (line 265) | inline void backend(const std::string& name)
function annotate (line 270) | inline bool annotate(std::string annotation, double x, double y)
function namespace (line 296) | namespace detail {
function namespace (line 779) | namespace detail {
function fignum_exists (line 1518) | inline bool fignum_exists(long number)
function figure_size (line 1534) | inline void figure_size(size_t w, size_t h)
function legend (line 1556) | inline void legend()
function legend (line 1566) | inline void legend(const std::map<std::string, std::string>& keywords)
function xticks (line 1706) | void xticks(const std::vector<Numeric> &ticks, const std::map<std::strin...
function yticks (line 1755) | void yticks(const std::vector<Numeric> &ticks, const std::map<std::strin...
function margins (line 1760) | void margins(Numeric margin)
function margins (line 1775) | void margins(Numeric margin_x, Numeric margin_y)
function subplot (line 1818) | inline void subplot(long nrows, long ncols, long plot_number)
function axis (line 1904) | inline void axis(const std::string &axisstr)
function grid (line 2064) | inline void grid(bool flag)
function close (line 2106) | inline void close()
function xkcd (line 2119) | inline void xkcd() {
function draw (line 2136) | inline void draw()
function pause (line 2150) | void pause(Numeric interval)
function save (line 2164) | inline void save(const std::string& filename)
function clf (line 2181) | inline void clf() {
function cla (line 2193) | inline void cla() {
function ion (line 2205) | inline void ion() {
function tight_layout (line 2254) | inline void tight_layout() {
function namespace (line 2268) | namespace detail {
type Fallback (line 2285) | struct Fallback { void operator()(); }
type Derived (line 2286) | struct Derived
type dtype (line 2298) | typedef decltype(&Fallback::operator()) dtype;
type typename (line 2306) | typedef typename is_callable_impl<std::is_class<T>::value, T>::type type;
function false_type (line 2313) | struct plot_impl<std::false_type>
function true_type (line 2352) | struct plot_impl<std::true_type>
function class (line 2398) | class Plot
FILE: include/scope_timer.hpp
class TimeConsuming (line 16) | class TimeConsuming {
method TimeConsuming (line 20) | TimeConsuming(string msg, int repeat_time) {
method TimeConsuming (line 27) | TimeConsuming(string msg) {
method set_enbale (line 53) | void set_enbale(bool enable){
method start (line 57) | void start() {
method stop (line 61) | void stop() {
FILE: src/IMU_Processing.hpp
function time_list (line 33) | const bool time_list(PointType &x, PointType &y) {return (x.curvature < ...
class ImuProcess (line 36) | class ImuProcess
FILE: src/laserMapping.cpp
function calc_dist (line 168) | float calc_dist(PointType p1, PointType p2) {
function calcBodyVar (line 173) | void calcBodyVar(Eigen::Vector3d &pb, const float range_inc,
function SigHandle (line 198) | void SigHandle(int sig) {
function dump_lio_state_to_log (line 208) | inline void dump_lio_state_to_log(FILE *fp) {
function pointBodyToWorld (line 224) | void pointBodyToWorld(PointType const *const pi, PointType *const po) {
function pointBodyToWorld (line 238) | void pointBodyToWorld(const Matrix<T, 3, 1> &pi, Matrix<T, 3, 1> &po) {
function RGBpointBodyToWorld (line 246) | void RGBpointBodyToWorld(PointType const *const pi, PointTypeRGB *const ...
function points_cache_collect (line 264) | void points_cache_collect()
function lasermap_fov_segment (line 276) | void lasermap_fov_segment()
function livox_pcl_cbk (line 337) | void livox_pcl_cbk(const livox_ros_driver::CustomMsg::ConstPtr &msg)
function standard_pcl_cbk (line 388) | void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg)
function sync_packages (line 436) | bool sync_packages(MeasureGroup &meas)
function sync_packages_only_lidar (line 476) | bool sync_packages_only_lidar(MeasureGroup &meas) {
function PointCloudXYZI (line 507) | PointCloudXYZI point3DtoPCL(std::vector<Point3D> &v_point_temp, int type)
function PointType (line 578) | PointType point3DtoPCLPoint(Point3D& point_3d, int type)
function iterCurrentPoint3D (line 611) | void iterCurrentPoint3D(Point3D& current_point)
function iterCurrentPoint3D (line 629) | void iterCurrentPoint3D(Point3D& current_point, int iter_num)
function iterCurrentPoint3D (line 653) | void iterCurrentPoint3D(Point3D& current_point, bool converge)
function map_incremental (line 678) | void map_incremental()
function publish_frame_world (line 729) | void publish_frame_world(const ros::Publisher &pubLaserCloudFullRes) {
function publish_frame_body (line 786) | void publish_frame_body(const ros::Publisher &pubLaserCloudFullRes_body) {
function publish_effect_world (line 795) | void publish_effect_world(const ros::Publisher &pubLaserCloudEffect) {
function publish_map (line 808) | void publish_map(const ros::Publisher &pubLaserCloudMap) {
function set_posestamp (line 817) | void set_posestamp(T &out)
function publish_odometry (line 836) | void publish_odometry(const ros::Publisher &pubOdomAftMapped) {
function publish_mavros (line 858) | void publish_mavros(const ros::Publisher &mavros_pose_publisher) {
function publish_path (line 866) | void publish_path(const ros::Publisher pubPath) {
function fileout_calib_result (line 874) | void fileout_calib_result() {
function print_refine_result (line 893) | void print_refine_result() {
function printProgress (line 911) | void printProgress(double percentage) {
function subSampleFrame (line 930) | void subSampleFrame(std::vector<Point3D>& frame, double size_voxel)
function adjustCVCov (line 952) | void adjustCVCov()
function main (line 972) | int main(int argc, char **argv)
FILE: src/preprocess.cpp
function time_list_cut_frame (line 6) | const bool time_list_cut_frame(PointType &x, PointType &y) {
function time_list_cut_frame3D (line 10) | const bool time_list_cut_frame3D(Point3D &x, Point3D &y) {
FILE: src/preprocess.h
type Feature (line 12) | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, Z...
type Surround (line 13) | enum Surround{Prev, Next}
type E_jump (line 14) | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}
type orgtype (line 18) | struct orgtype
function Point (line 36) | struct EIGEN_ALIGN16 Point {
function Point (line 54) | struct EIGEN_ALIGN16 Point {
function Point (line 79) | struct EIGEN_ALIGN16 Point {
function Point (line 98) | struct EIGEN_ALIGN16 Point {
Condensed preview — 164 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,398K chars).
[
{
"path": ".gitignore",
"chars": 0,
"preview": ""
},
{
"path": "CMakeLists.txt",
"chars": 2533,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(i2ekf_lo)\n\nSET(CMAKE_BUILD_TYPE \"Release\")\n\nADD_COMPILE_OPTIONS(-std=c++14"
},
{
"path": "LICENSE",
"chars": 18092,
"preview": " GNU GENERAL PUBLIC LICENSE\n Version 2, June 1991\n\n Copyright (C) 1989, 1991 Fr"
},
{
"path": "README.md",
"chars": 3770,
"preview": "<div align=\"center\">\n <h1>I2EKF-LO</h1>\n <i>A Dual-Iteration Extended Kalman Filter Based LiDAR Odometry</i>\n "
},
{
"path": "Thirdparty/Sophus/.clang-format",
"chars": 2581,
"preview": "Language: Cpp\n# BasedOnStyle: Google\nAccessModifierOffset: -1\nAlignAfterOpenBracket: Align\nAlignConsecutiveAssig"
},
{
"path": "Thirdparty/Sophus/CMakeLists.txt",
"chars": 6884,
"preview": "cmake_minimum_required(VERSION 3.4)\nproject(Sophus VERSION 1.22.10)\n\ninclude(CMakePackageConfigHelpers)\ninclude(GNUInsta"
},
{
"path": "Thirdparty/Sophus/LICENSE.txt",
"chars": 1107,
"preview": "Copyright 2011-2017 Hauke Strasdat \n 2012-2017 Steven Lovegrove\n\nPermission is hereby granted, free o"
},
{
"path": "Thirdparty/Sophus/README.rst",
"chars": 1969,
"preview": "|GithubCICpp|_ windows: |AppVeyor|_ |GithubCISympy|_ |ci_cov|_\n\n\nSophus\n======\n\nOverview\n--------\n\nThis is a c++ impleme"
},
{
"path": "Thirdparty/Sophus/Sophus.code-workspace",
"chars": 60,
"preview": "{\n\t\"folders\": [\n\t\t{\n\t\t\t\"path\": \".\"\n\t\t}\n\t],\n\t\"settings\": {}\n}"
},
{
"path": "Thirdparty/Sophus/SophusConfig.cmake.in",
"chars": 148,
"preview": "@PACKAGE_INIT@\n\ninclude (CMakeFindDependencyMacro)\n\n@Eigen3_DEPENDENCY@\n@fmt_DEPENDENCY@\n\ninclude (\"${CMAKE_CURRENT_LIST"
},
{
"path": "Thirdparty/Sophus/appveyor.yml",
"chars": 822,
"preview": "branches:\n only:\n - master\n\nos: Visual Studio 2015\n\nclone_folder: c:\\projects\\sophus\n\nplatform: x64\nconfiguration: D"
},
{
"path": "Thirdparty/Sophus/cmake_modules/FindEigen3.cmake",
"chars": 3509,
"preview": "# - Try to find Eigen3 lib\n#\n# This module supports requiring a minimum version, e.g. you can do\n# find_package(Eigen3"
},
{
"path": "Thirdparty/Sophus/doxyfile",
"chars": 691,
"preview": "DOXYFILE_ENCODING = UTF-8\nPROJECT_NAME = \"Sophus\"\nINPUT = sophus\nEXTRACT_ALL "
},
{
"path": "Thirdparty/Sophus/doxyrest-config.lua",
"chars": 658,
"preview": "FRAME_DIR_LIST = { \"doxyrest_b/doxyrest/frame/cfamily\", \"doxyrest_b/doxyrest/frame/common\" }\nFRAME_FILE = \"index.rst.in\""
},
{
"path": "Thirdparty/Sophus/examples/CMakeLists.txt",
"chars": 212,
"preview": "# Tests to run\nSET( EXAMPLE_SOURCES HelloSO3)\n\nFOREACH(example_src ${EXAMPLE_SOURCES})\n ADD_EXECUTABLE( ${example_src} "
},
{
"path": "Thirdparty/Sophus/examples/HelloSO3.cpp",
"chars": 1511,
"preview": "#include <iostream>\n#include \"sophus/geometry.hpp\"\n\nint main() {\n // The following demonstrates the group multiplicatio"
},
{
"path": "Thirdparty/Sophus/generate_stubs.py",
"chars": 209,
"preview": "import subprocess\n\nsubprocess.run(\n \"pybind11-stubgen sophus_pybind -o sophus_pybind-stubs/\",\n shell=True,\n che"
},
{
"path": "Thirdparty/Sophus/make_docs.sh",
"chars": 124,
"preview": "doxygen doxyfile\ndoxyrest_b/build/doxyrest/bin/Release/doxyrest -c doxyrest-config.lua\nsphinx-build -b html rst-dir html"
},
{
"path": "Thirdparty/Sophus/package.xml",
"chars": 649,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>sophus</name>\n <version>1.22.10</version>\n <description>\n C++ imp"
},
{
"path": "Thirdparty/Sophus/rst-dir/conf.py",
"chars": 1875,
"preview": "# Configuration file for the Sphinx documentation builder.\n#\n# This file only contains a selection of the most common op"
},
{
"path": "Thirdparty/Sophus/rst-dir/page_index.rst",
"chars": 197,
"preview": "Sophus - Lie groups for 2d/3d Geometry\n=======================================\n\n.. toctree::\n :maxdepth: 2\n :captio"
},
{
"path": "Thirdparty/Sophus/rst-dir/pysophus.rst",
"chars": 322,
"preview": "Python API\n==========\n\n.. automodule:: sophus.matrix\n :members:\n\n.. automodule:: sophus.complex\n :members:\n\n.. autom"
},
{
"path": "Thirdparty/Sophus/run_format.sh",
"chars": 165,
"preview": "find . -type d \\( -path ./sympy -o -path ./doxyrest_b -o -path \"./*/CMakeFiles/*\" \\) -prune -o \\( -iname \"*.hpp\" -o -ina"
},
{
"path": "Thirdparty/Sophus/scripts/install_docs_deps.sh",
"chars": 409,
"preview": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\nsudo apt-get -qq update\nsudo apt-get install doxygen liblua5.3-dev"
},
{
"path": "Thirdparty/Sophus/scripts/install_linux_deps.sh",
"chars": 703,
"preview": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\ncmake --version\n\nsudo apt-get -qq update\nsudo apt-get install gfor"
},
{
"path": "Thirdparty/Sophus/scripts/install_linux_fmt_deps.sh",
"chars": 227,
"preview": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\ngit clone https://github.com/fmtlib/fmt.git\ncd fmt\ngit checkout 5."
},
{
"path": "Thirdparty/Sophus/scripts/install_osx_deps.sh",
"chars": 1913,
"preview": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\nbrew update\nbrew install fmt\nbrew install ccache\n\n# Build a specif"
},
{
"path": "Thirdparty/Sophus/scripts/run_cpp_tests.sh",
"chars": 306,
"preview": "#!/bin/bash\n\nset -x # echo on\nset -e # exit on error\n\nmkdir build\ncd build\npwd\ncmake -DCMAKE_CXX_COMPILER_LAUNCHER=ccach"
},
{
"path": "Thirdparty/Sophus/setup.py",
"chars": 5876,
"preview": "import os\nimport re\nimport shutil\nimport subprocess\nimport sys\n\nfrom setuptools import Extension, find_packages, setup\nf"
},
{
"path": "Thirdparty/Sophus/sophus/average.hpp",
"chars": 8963,
"preview": "/// @file\n/// Calculation of biinvariant means.\n\n#pragma once\n\n#include <complex>\n\n#include \"cartesian.hpp\"\n#include \"co"
},
{
"path": "Thirdparty/Sophus/sophus/cartesian.hpp",
"chars": 16104,
"preview": "/// @file\n/// Cartesian - Euclidean vector space as Lie group\n\n#pragma once\n#include <sophus/types.hpp>\n\nnamespace Sophu"
},
{
"path": "Thirdparty/Sophus/sophus/ceres_local_parameterization.hpp",
"chars": 1676,
"preview": "#pragma once\n\n#include <ceres/local_parameterization.h>\n\n#include <sophus/ceres_typetraits.hpp>\n\nnamespace Sophus {\n\n///"
},
{
"path": "Thirdparty/Sophus/sophus/ceres_manifold.hpp",
"chars": 2280,
"preview": "#pragma once\n\n#include <ceres/manifold.h>\n#include <sophus/ceres_typetraits.hpp>\n\nnamespace Sophus {\n\n/// Templated loca"
},
{
"path": "Thirdparty/Sophus/sophus/ceres_typetraits.hpp",
"chars": 1875,
"preview": "#ifndef SOPHUS_CERES_TYPETRAITS_HPP\n#define SOPHUS_CERES_TYPETRAITS_HPP\n\nnamespace Sophus {\n\ntemplate <class T, std::siz"
},
{
"path": "Thirdparty/Sophus/sophus/common.hpp",
"chars": 7530,
"preview": "/// @file\n/// Common functionality.\n\n#pragma once\n\n#include <cmath>\n#include <cstdio>\n#include <cstdlib>\n#include <rando"
},
{
"path": "Thirdparty/Sophus/sophus/example_ensure_handler.cpp",
"chars": 398,
"preview": "#include \"common.hpp\"\n\n#include <cstdio>\n#include <cstdlib>\n\nnamespace Sophus {\nvoid ensureFailed(char const* function, "
},
{
"path": "Thirdparty/Sophus/sophus/geometry.hpp",
"chars": 6239,
"preview": "/// @file\n/// Transformations between poses and hyperplanes.\n\n#pragma once\n\n#include \"se2.hpp\"\n#include \"se3.hpp\"\n#inclu"
},
{
"path": "Thirdparty/Sophus/sophus/interpolate.hpp",
"chars": 1224,
"preview": "/// @file\n/// Interpolation for Lie groups.\n\n#pragma once\n\n#include <Eigen/Eigenvalues>\n\n#include \"interpolate_details.h"
},
{
"path": "Thirdparty/Sophus/sophus/interpolate_details.hpp",
"chars": 2844,
"preview": "#pragma once\n\n#include \"cartesian.hpp\"\n#include \"rxso2.hpp\"\n#include \"rxso3.hpp\"\n#include \"se2.hpp\"\n#include \"se3.hpp\"\n#"
},
{
"path": "Thirdparty/Sophus/sophus/num_diff.hpp",
"chars": 2775,
"preview": "/// @file\n/// Numerical differentiation using finite differences\n\n#pragma once\n\n#include <functional>\n#include <type_tra"
},
{
"path": "Thirdparty/Sophus/sophus/rotation_matrix.hpp",
"chars": 2589,
"preview": "/// @file\n/// Rotation matrix helper functions.\n\n#pragma once\n\n#include <Eigen/Dense>\n#include <Eigen/SVD>\n\n#include \"ty"
},
{
"path": "Thirdparty/Sophus/sophus/rxso2.hpp",
"chars": 26206,
"preview": "/// @file\n/// Direct product R X SO(2) - rotation and scaling in 2d.\n\n#pragma once\n\n#include \"so2.hpp\"\n\nnamespace Sophus"
},
{
"path": "Thirdparty/Sophus/sophus/rxso3.hpp",
"chars": 30327,
"preview": "/// @file\n/// Direct product R X SO(3) - rotation and scaling in 3d.\n\n#pragma once\n\n#include \"so3.hpp\"\n\nnamespace Sophus"
},
{
"path": "Thirdparty/Sophus/sophus/se2.hpp",
"chars": 28437,
"preview": "/// @file\n/// Special Euclidean group SE(2) - rotation and translation in 2d.\n\n#pragma once\n\n#include \"so2.hpp\"\n\nnamespa"
},
{
"path": "Thirdparty/Sophus/sophus/se3.hpp",
"chars": 38809,
"preview": "/// @file\n/// Special Euclidean group SE(3) - rotation and translation in 3d.\n\n#pragma once\n\n#include \"so3.hpp\"\n\nnamespa"
},
{
"path": "Thirdparty/Sophus/sophus/sim2.hpp",
"chars": 28299,
"preview": "/// @file\n/// Similarity group Sim(2) - scaling, rotation and translation in 2d.\n\n#pragma once\n\n#include \"rxso2.hpp\"\n#in"
},
{
"path": "Thirdparty/Sophus/sophus/sim3.hpp",
"chars": 30109,
"preview": "/// @file\n/// Similarity group Sim(3) - scaling, rotation and translation in 3d.\n\n#pragma once\n\n#include \"rxso3.hpp\"\n#in"
},
{
"path": "Thirdparty/Sophus/sophus/sim_details.hpp",
"chars": 7022,
"preview": "#pragma once\n\n#include \"types.hpp\"\n\nnamespace Sophus {\nnamespace details {\n\ntemplate <class Scalar, int N>\nMatrix<Scalar"
},
{
"path": "Thirdparty/Sophus/sophus/so2.hpp",
"chars": 22448,
"preview": "/// @file\n/// Special orthogonal group SO(2) - rotation in 2d.\n\n#pragma once\n\n#include <type_traits>\n\n// Include only th"
},
{
"path": "Thirdparty/Sophus/sophus/so3.hpp",
"chars": 33104,
"preview": "/// @file\n/// Special orthogonal group SO(3) - rotation in 3d.\n\n#pragma once\n\n#include \"rotation_matrix.hpp\"\n#include \"s"
},
{
"path": "Thirdparty/Sophus/sophus/spline.hpp",
"chars": 17817,
"preview": "/// @file\n// Basis spline implementation on Lie Group following:\n// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013\n"
},
{
"path": "Thirdparty/Sophus/sophus/test_macros.hpp",
"chars": 7103,
"preview": "#pragma once\n\n#include <iostream>\n#include <sstream>\n\n#include <sophus/types.hpp>\n\nnamespace Sophus {\nnamespace details "
},
{
"path": "Thirdparty/Sophus/sophus/types.hpp",
"chars": 6766,
"preview": "/// @file\n/// Common type aliases.\n\n#pragma once\n\n#include <type_traits>\n#include \"common.hpp\"\n\nnamespace Sophus {\n\ntemp"
},
{
"path": "Thirdparty/Sophus/sophus/velocities.hpp",
"chars": 2320,
"preview": "#pragma once\n\n#include <functional>\n\n#include \"num_diff.hpp\"\n#include \"se3.hpp\"\n\nnamespace Sophus {\nnamespace experiment"
},
{
"path": "Thirdparty/Sophus/sophus_pybind/README",
"chars": 2880,
"preview": "# Python binding for Sophus library: sophus-pybind \nsophus-pybind implement python binding for sophus that provides acce"
},
{
"path": "Thirdparty/Sophus/sophus_pybind/SE3PyBind.h",
"chars": 20054,
"preview": "#pragma once\n\n#include \"SO3PyBind.h\"\n\n#include <sophus/average.hpp>\n#include <sophus/common.hpp>\n#include <sophus/interp"
},
{
"path": "Thirdparty/Sophus/sophus_pybind/SO3PyBind.h",
"chars": 14544,
"preview": "#pragma once\n\n#include <fmt/format.h>\n#include <sophus/common.hpp>\n#include <sophus/so3.hpp>\n\n#include <pybind11/eigen.h"
},
{
"path": "Thirdparty/Sophus/sophus_pybind/SophusPyBind.h",
"chars": 1012,
"preview": "#pragma once\n\n#include \"SE3PyBind.h\"\n#include \"SO3PyBind.h\"\n\n#include <sstream>\n// By default, Sophus calls std::abort w"
},
{
"path": "Thirdparty/Sophus/sophus_pybind/bindings.cpp",
"chars": 90,
"preview": "#include \"SophusPyBind.h\"\n\nPYBIND11_MODULE(sophus_pybind, m) { Sophus::exportSophus(m); }\n"
},
{
"path": "Thirdparty/Sophus/sophus_pybind/examples/sophus_quickstart_tutorial.ipynb",
"chars": 11523,
"preview": "{\n \"cells\": [\n {\n \"attachments\": {},\n \"cell_type\": \"markdown\",\n \"id\": \"85e0ca79-32de-4fb2-b92f-a9177d7d5652\",\n "
},
{
"path": "Thirdparty/Sophus/sophus_pybind/tests/sophusPybindTests.py",
"chars": 6274,
"preview": "import unittest\n\nimport numpy as np\nfrom sophus_pybind import interpolate, iterativeMean, SE3, SO3\n\n\nclass SophusPybindT"
},
{
"path": "Thirdparty/Sophus/sophus_pybind-stubs/py.typed",
"chars": 0,
"preview": ""
},
{
"path": "Thirdparty/Sophus/sophus_pybind-stubs/sophus_pybind.pyi",
"chars": 6245,
"preview": "from __future__ import annotations\nimport numpy\nimport typing\n__all__ = ['SE3', 'SO3', 'interpolate', 'iterativeMean']\nc"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/Se2_Dx_exp_x.cpp",
"chars": 585,
"preview": "Scalar const c0 = sin(theta);\nScalar const c1 = cos(theta);\nScalar const c2 = 1.0/theta;\nScalar const c3 = c0*c2;\nScalar"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/Se2_Dx_log_this.cpp",
"chars": 770,
"preview": "Scalar const c0 = c[0] - 1;\nScalar const c1 = 0.5*atan2(c[1], c[0]);\nScalar const c2 = c1*c[1]/pow(c0, 2);\nScalar const "
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/Se2_Dx_this_mul_exp_x_at_0.cpp",
"chars": 221,
"preview": "Scalar const c0 = -c[1];\nresult[0] = 0;\nresult[1] = 0;\nresult[2] = c0;\nresult[3] = 0;\nresult[4] = 0;\nresult[5] = c[0];\nr"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/Se3_Dx_exp_x.cpp",
"chars": 4714,
"preview": "Scalar const c0 = pow(omega[0], 2);\nScalar const c1 = pow(omega[1], 2);\nScalar const c2 = pow(omega[2], 2);\nScalar const"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/Se3_Dx_log_this.cpp",
"chars": 9209,
"preview": "Scalar const c0 = pow(q.z(), 2);\nScalar const c1 = 1.0/q.w();\nScalar const c2 = pow(q.x(), 2);\nScalar const c3 = pow(q.y"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/Se3_Dx_this_mul_exp_x_at_0.cpp",
"chars": 1405,
"preview": "Scalar const c0 = 0.5*q.w();\nScalar const c1 = 0.5*q.z();\nScalar const c2 = -c1;\nScalar const c3 = 0.5*q.y();\nScalar con"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So2_Dx_exp_x.cpp",
"chars": 49,
"preview": "result[0] = -sin(theta);\nresult[1] = cos(theta);\n"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So2_Dx_log_exp_x_times_this_at_0.cpp",
"chars": 12,
"preview": "result = 1;\n"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So2_Dx_log_this.cpp",
"chars": 46,
"preview": "result = -c[1]/(pow(c[0], 2) + pow(c[1], 2));\n"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So2_Dx_this_mul_exp_x_at_0.cpp",
"chars": 37,
"preview": "result[0] = -c[1];\nresult[1] = c[0];\n"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So3_Dx_exp_x.cpp",
"chars": 867,
"preview": "Scalar const c0 = pow(omega[0], 2);\nScalar const c1 = pow(omega[1], 2);\nScalar const c2 = pow(omega[2], 2);\nScalar const"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So3_Dx_log_exp_x_times_this_at_0.cpp",
"chars": 1751,
"preview": "Scalar const c0 = 1.0*pow(q.x(), 4);\nScalar const c1 = 1.0*pow(q.y(), 4);\nScalar const c2 = 1.0*pow(q.z(), 4);\nScalar co"
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So3_Dx_log_this.cpp",
"chars": 751,
"preview": "Scalar const c0 = pow(q.x(), 2);\nScalar const c1 = pow(q.y(), 2);\nScalar const c2 = pow(q.z(), 2);\nScalar const c3 = c0 "
},
{
"path": "Thirdparty/Sophus/sympy/cpp_gencode/So3_Dx_this_mul_exp_x_at_0.cpp",
"chars": 379,
"preview": "Scalar const c0 = 0.5*q.w();\nScalar const c1 = 0.5*q.z();\nScalar const c2 = -c1;\nScalar const c3 = 0.5*q.y();\nScalar con"
},
{
"path": "Thirdparty/Sophus/sympy/run_tests.sh",
"chars": 287,
"preview": "#!/bin/bash\n\nEXIT=0\n\npython3 -m sophus.complex || EXIT=$?\npython3 -m sophus.quaternion || EXIT=$?\npython3 -m sophus.dual"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "Thirdparty/Sophus/sympy/sophus/complex.py",
"chars": 3260,
"preview": "import unittest\n\nimport sympy\n\n\nclass Complex:\n \"\"\" Complex class \"\"\"\n\n def __init__(self, real, imag):\n se"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/cse_codegen.py",
"chars": 474,
"preview": "import io\n\nimport sympy\n\n\ndef cse_codegen(symbols):\n cse_results = sympy.cse(symbols, sympy.numbered_symbols(\"c\"))\n "
},
{
"path": "Thirdparty/Sophus/sympy/sophus/dual_quaternion.py",
"chars": 3040,
"preview": "import unittest\n\nimport sympy\n\nfrom sophus.matrix import Vector3\nfrom sophus.quaternion import Quaternion\n\n\nclass DualQu"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/matrix.py",
"chars": 1277,
"preview": "import sys\n\nimport sympy\n\nassert sys.version_info >= (3, 5)\n\n\ndef dot(left, right):\n assert(isinstance(left, sympy.Ma"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/quaternion.py",
"chars": 4279,
"preview": "\"\"\" run with: python3 -m sophus.quaternion \"\"\"\n\nimport unittest\n\nimport sympy\n\nfrom sophus.matrix import Vector3\nfrom so"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/se2.py",
"chars": 8352,
"preview": "import functools\nimport unittest\n\nimport sympy\n\nfrom sophus.complex import Complex\nfrom sophus.cse_codegen import cse_co"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/se3.py",
"chars": 10086,
"preview": "import functools\nimport unittest\n\nimport sympy\n\nfrom sophus.cse_codegen import cse_codegen\nfrom sophus.matrix import Vec"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/so2.py",
"chars": 7289,
"preview": "import functools\nimport unittest\n\nimport sympy\n\nfrom sophus.complex import Complex\nfrom sophus.cse_codegen import cse_co"
},
{
"path": "Thirdparty/Sophus/sympy/sophus/so3.py",
"chars": 10633,
"preview": "import functools\nimport unittest\n\nimport sympy\n\nfrom sophus.cse_codegen import cse_codegen\nfrom sophus.matrix import Vec"
},
{
"path": "Thirdparty/Sophus/test/CMakeLists.txt",
"chars": 47,
"preview": "ADD_SUBDIRECTORY(core)\nADD_SUBDIRECTORY(ceres)\n"
},
{
"path": "Thirdparty/Sophus/test/ceres/CMakeLists.txt",
"chars": 968,
"preview": "# Make sure Ceres knows where to find Eigen\nlist(APPEND SEARCH_HEADERS ${EIGEN3_INCLUDE_DIR})\n\n# git clone https://ceres"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_rxso2.cpp",
"chars": 1750,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/rxso2.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nus"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_rxso3.cpp",
"chars": 1976,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/rxso3.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nus"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_se2.cpp",
"chars": 1649,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/se2.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nusin"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_se3.cpp",
"chars": 2158,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/se3.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nusin"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_sim2.cpp",
"chars": 2114,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/sim2.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nusi"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_sim3.cpp",
"chars": 2451,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/sim3.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nusi"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_so2.cpp",
"chars": 1406,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/so2.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nusin"
},
{
"path": "Thirdparty/Sophus/test/ceres/test_ceres_so3.cpp",
"chars": 3121,
"preview": "#include <ceres/ceres.h>\n#include <iostream>\n#include <sophus/se3.hpp>\n\n#include \"tests.hpp\"\n\ntemplate <typename T>\nusin"
},
{
"path": "Thirdparty/Sophus/test/ceres/tests.hpp",
"chars": 16490,
"preview": "#pragma once\n\n#include <ceres/ceres.h>\n\n#include <sophus/ceres_manifold.hpp>\n#include <sophus/test_macros.hpp>\n\ntemplate"
},
{
"path": "Thirdparty/Sophus/test/core/CMakeLists.txt",
"chars": 795,
"preview": "#Tests to run\nSET( TEST_SOURCES test_common\n test_cartesian2\n test_cartesian3\n "
},
{
"path": "Thirdparty/Sophus/test/core/test_cartesian2.cpp",
"chars": 2524,
"preview": "#include <iostream>\n\n#include <sophus/cartesian.hpp>\n#include \"tests.hpp\"\n\n// Explicit instantiate all class templates s"
},
{
"path": "Thirdparty/Sophus/test/core/test_cartesian3.cpp",
"chars": 2641,
"preview": "#include <iostream>\n\n#include <sophus/cartesian.hpp>\n#include \"tests.hpp\"\n\n// Explicit instantiate all class templates s"
},
{
"path": "Thirdparty/Sophus/test/core/test_common.cpp",
"chars": 3933,
"preview": "#include <iostream>\n\n#include <sophus/interpolate.hpp>\n#include <sophus/num_diff.hpp>\n#include <sophus/se3.hpp>\n#include"
},
{
"path": "Thirdparty/Sophus/test/core/test_geometry.cpp",
"chars": 4807,
"preview": "#include <iostream>\n\n#include <sophus/geometry.hpp>\n#include <sophus/test_macros.hpp>\n#include \"tests.hpp\"\n\nnamespace So"
},
{
"path": "Thirdparty/Sophus/test/core/test_rxso2.cpp",
"chars": 11401,
"preview": "#include <iostream>\n\n#include <sophus/rxso2.hpp>\n#include \"tests.hpp\"\n\n// Explicit instantiate all class templates so th"
},
{
"path": "Thirdparty/Sophus/test/core/test_rxso3.cpp",
"chars": 13138,
"preview": "#include <iostream>\n\n#include <sophus/rxso3.hpp>\n#include \"tests.hpp\"\n\n// Explicit instantiate all class templates so th"
},
{
"path": "Thirdparty/Sophus/test/core/test_se2.cpp",
"chars": 9737,
"preview": "#include <iostream>\n\n#include <sophus/se2.hpp>\n#include <unsupported/Eigen/MatrixFunctions>\n#include \"tests.hpp\"\n\n// Exp"
},
{
"path": "Thirdparty/Sophus/test/core/test_se3.cpp",
"chars": 10509,
"preview": "#include <iostream>\n\n#include <sophus/se3.hpp>\n#include \"tests.hpp\"\n\n// Explicit instantiate all class templates so that"
},
{
"path": "Thirdparty/Sophus/test/core/test_sim2.cpp",
"chars": 8650,
"preview": "#include <iostream>\n\n#include <unsupported/Eigen/MatrixFunctions>\n\n#include <sophus/sim2.hpp>\n#include \"tests.hpp\"\n\n// E"
},
{
"path": "Thirdparty/Sophus/test/core/test_sim3.cpp",
"chars": 11757,
"preview": "#include <iostream>\n\n#include <unsupported/Eigen/MatrixFunctions>\n\n#include <sophus/sim3.hpp>\n#include \"tests.hpp\"\n\n// E"
},
{
"path": "Thirdparty/Sophus/test/core/test_so2.cpp",
"chars": 5977,
"preview": "#include <iostream>\n\n#include <sophus/so2.hpp>\n#include \"tests.hpp\"\n\n// Explicit instantiate all class templates so that"
},
{
"path": "Thirdparty/Sophus/test/core/test_so3.cpp",
"chars": 10490,
"preview": "#include <iostream>\n\n#include <sophus/interpolate.hpp>\n#include <sophus/so3.hpp>\n#include \"tests.hpp\"\n\n// Explicit insta"
},
{
"path": "Thirdparty/Sophus/test/core/test_velocities.cpp",
"chars": 4588,
"preview": "#include <iostream>\n\n#include \"tests.hpp\"\n\n#include <sophus/velocities.hpp>\n\nnamespace Sophus {\nnamespace experimental {"
},
{
"path": "Thirdparty/Sophus/test/core/tests.hpp",
"chars": 29069,
"preview": "#ifndef SOPUHS_TESTS_HPP\n#define SOPUHS_TESTS_HPP\n\n#include <array>\n\n#include <Eigen/StdVector>\n#include <unsupported/Ei"
},
{
"path": "Thirdparty/tessil-src/CMakeLists.txt",
"chars": 3176,
"preview": "cmake_minimum_required(VERSION 3.1)\ninclude(GNUInstallDirs)\n\n\nproject(tsl-robin-map VERSION 0.6.3)\n\nadd_library(robin_ma"
},
{
"path": "Thirdparty/tessil-src/LICENSE",
"chars": 1102,
"preview": "MIT License\n\nCopyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n\nPermission is hereby granted, free of cha"
},
{
"path": "Thirdparty/tessil-src/README.md",
"chars": 14811,
"preview": "[](https://travis-ci.org/Tessil/robin-map) [![B"
},
{
"path": "Thirdparty/tessil-src/appveyor.yml",
"chars": 1268,
"preview": "environment:\n BOOST_ROOT: C:\\Libraries\\boost_1_67_0\n matrix:\n - APPVEYOR_BUILD_WORKER_IMAGE: Visual Studio "
},
{
"path": "Thirdparty/tessil-src/cmake/tsl-robin-mapConfig.cmake.in",
"chars": 407,
"preview": "# This module sets the following variables:\n# * tsl-robin-map_FOUND - true if tsl-robin-map found on the system\n# * tsl-"
},
{
"path": "Thirdparty/tessil-src/doxygen.conf",
"chars": 108206,
"preview": "# Doxyfile 1.8.11\n\n# This file describes the settings to be used by the documentation system\n# doxygen (www.doxygen.org)"
},
{
"path": "Thirdparty/tessil-src/include/tsl/robin_growth_policy.h",
"chars": 11764,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/include/tsl/robin_hash.h",
"chars": 52827,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/include/tsl/robin_map.h",
"chars": 27360,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/include/tsl/robin_set.h",
"chars": 22381,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tests/CMakeLists.txt",
"chars": 1111,
"preview": "cmake_minimum_required(VERSION 3.8)\n\nproject(tsl_robin_map_tests)\n\nadd_executable(tsl_robin_map_tests \"main.cpp\" \n "
},
{
"path": "Thirdparty/tessil-src/tests/custom_allocator_tests.cpp",
"chars": 4118,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tests/main.cpp",
"chars": 1281,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tests/policy_tests.cpp",
"chars": 3556,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tests/robin_map_tests.cpp",
"chars": 43982,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tests/robin_set_tests.cpp",
"chars": 5708,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tests/utils.h",
"chars": 9327,
"preview": "/**\n * MIT License\n * \n * Copyright (c) 2017 Thibaut Goetghebuer-Planchon <tessil@gmx.com>\n * \n * Permission is hereby g"
},
{
"path": "Thirdparty/tessil-src/tsl-robin-map.natvis",
"chars": 4623,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<AutoVisualizer xmlns=\"http://schemas.microsoft.com/vstudio/debugger/natvis/2010\""
},
{
"path": "config/avia.yaml",
"chars": 1067,
"preview": "common:\n lid_topic: \"/livox/lidar\"\n\npreprocess:\n lidar_type: 1 # Livox series LiDAR\n feature_ext"
},
{
"path": "config/horizon.yaml",
"chars": 1034,
"preview": "common:\n lid_topic: \"/livox/lidar\"\n\npreprocess:\n lidar_type: 1 # Livox series LiDAR\n feature_ex"
},
{
"path": "config/mid360.yaml",
"chars": 704,
"preview": "common:\n lid_topic: \"/livox/lidar\"\n\npreprocess:\n lidar_type: 1 # Livox\n feature_extract_en: fal"
},
{
"path": "config/ouster.yaml",
"chars": 1066,
"preview": "common:\n lid_topic: \"/os_cloud_node/points\"\n\npreprocess:\n lidar_type: 3 # Ouster LiDAR\n scan_li"
},
{
"path": "config/pandar.yaml",
"chars": 1059,
"preview": "common:\n lid_topic: \"/hesai/pandar\"\n\npreprocess:\n lidar_type: 5 # Hesai PandarXT\n scan_line: 32"
},
{
"path": "config/robosense.yaml",
"chars": 1066,
"preview": "common:\n lid_topic: \"/rslidar_points\"\n\npreprocess:\n lidar_type: 6 # Robosense LiDAR\n scan_line:"
},
{
"path": "config/velodyne.yaml",
"chars": 1063,
"preview": "common:\n lid_topic: \"/velodyne_points_0\"\n\npreprocess:\n lidar_type: 2 # Velodyne LiDAR\n scan_lin"
},
{
"path": "include/color.h",
"chars": 1114,
"preview": "#ifndef COLOR_H\n#define COLOR_H\n\n#define RESET \"\\033[0m\"\n#define BLACK \"\\033[30m\" /* Black */\n#d"
},
{
"path": "include/common_lib.h",
"chars": 10599,
"preview": "#ifndef COMMON_LIB_H\n#define COMMON_LIB_H\n\n#include <so3_math.h>\n#include <Eigen/Eigen>\n#include <pcl/point_types.h>\n#in"
},
{
"path": "include/ikd-Tree/README.md",
"chars": 73,
"preview": "# ikd-Tree\nikd-Tree is an incremental k-d tree for robotic applications.\n"
},
{
"path": "include/ikd-Tree/ikd_Tree.cpp",
"chars": 65683,
"preview": "#include \"ikd_Tree.h\"\n\n/*\nDescription: ikd-Tree: an incremental k-d tree for robotic applications \nAuthor: Yixi Cai\nemai"
},
{
"path": "include/ikd-Tree/ikd_Tree.h",
"chars": 6571,
"preview": "#pragma once\n#include <pcl/point_types.h>\n#include <Eigen/StdVector>\n#include <Eigen/Geometry>\n#include <stdio.h>\n#inclu"
},
{
"path": "include/matplotlibcpp.h",
"chars": 81742,
"preview": "#pragma once\n\n// Python headers must be included before any system headers, since\n// they define _POSIX_C_SOURCE\n#includ"
},
{
"path": "include/scope_timer.hpp",
"chars": 2906,
"preview": "//\n// Created by yunfan on 2021/3/19.\n// Version: 1.0.0\n//\n\n\n#ifndef SCROPE_TIMER_HPP//SRC_POLY_VISUAL_UTILS_HPP\n#define"
},
{
"path": "include/so3_math.h",
"chars": 3430,
"preview": "#ifndef SO3_MATH_H\n#define SO3_MATH_H\n\n#include <math.h>\n#include <Eigen/Core>\n\n\n#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1"
},
{
"path": "launch/hesai_pandarXT.launch",
"chars": 482,
"preview": "<launch>\n <!-- Launch file for Hesai PandarXT LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam comman"
},
{
"path": "launch/livox_avia.launch",
"chars": 449,
"preview": "<launch>\n<!-- Launch file for Livox AVIA LiDAR -->\n\n\t<arg name=\"rviz\" default=\"true\" />\n\n\t<rosparam command=\"load\" file="
},
{
"path": "launch/livox_horizon.launch",
"chars": 419,
"preview": "<launch>\n<!-- Launch file for Livox Horizon LiDAR -->\n\n\t<arg name=\"rviz\" default=\"true\" />\n\n\t<rosparam command=\"load\" fi"
},
{
"path": "launch/livox_mid360.launch",
"chars": 451,
"preview": "<launch>\n<!-- Launch file for Livox AVIA LiDAR -->\n\n\t<arg name=\"rviz\" default=\"true\" />\n\n\t<rosparam command=\"load\" file="
},
{
"path": "launch/ouster.launch",
"chars": 482,
"preview": "<launch>\n <!-- Launch file for ouster Ouster LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command"
},
{
"path": "launch/robosense.launch",
"chars": 473,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command=\"load\""
},
{
"path": "launch/velodyne.launch",
"chars": 472,
"preview": "<launch>\n <!-- Launch file for velodyne LiDAR -->\n\n <arg name=\"rviz\" default=\"true\" />\n\n <rosparam command=\"load\""
},
{
"path": "msg/Pose6D.msg",
"chars": 597,
"preview": "# the preintegrated Lidar states at the time of IMU measurements in a frame\nfloat64 offset_time # the offset time of IM"
},
{
"path": "msg/States.msg",
"chars": 569,
"preview": "Header header # timestamp of the first lidar in a frame\nfloat64[] rot_end # the estimated attitude (rotati"
},
{
"path": "package.xml",
"chars": 1465,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>i2ekf_lo</name>\n <version>0.0.0</version>\n\n <description>\n This is a modifi"
},
{
"path": "rviz_cfg/.gitignore",
"chars": 0,
"preview": ""
},
{
"path": "rviz_cfg/fast_lo.rviz",
"chars": 12080,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rviz_cfg/spinning.rviz",
"chars": 11224,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "src/IMU_Processing.hpp",
"chars": 15194,
"preview": "#ifndef _IMU_PROCESSING_HPP\n#define _IMU_PROCESSING_HPP\n\n#include <cmath>\n#include <math.h>\n#include <deque>\n#include <m"
},
{
"path": "src/laserMapping.cpp",
"chars": 53163,
"preview": "// This is an advanced implementation of the algorithm described in the\n// following paper:\n// J. Zhang and S. Singh. "
},
{
"path": "src/preprocess.cpp",
"chars": 56717,
"preview": "#include \"preprocess.h\"\n\n#define RETURN0 0x00\n#define RETURN0AND1 0x10\n\nconst bool time_list_cut_frame(PointType &x,"
},
{
"path": "src/preprocess.h",
"chars": 6295,
"preview": "#ifndef PREPROCESS_H\n#define PREPROCESS_H\n\n#include \"common_lib.h\"\n#include <ros/ros.h>\n#include <pcl_conversions/pcl_co"
}
]
About this extraction
This page contains the full source code of the YWL0720/I2EKF-LO GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 164 files (1.3 MB), approximately 368.3k tokens, and a symbol index with 1496 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.