Repository: zhuge2333/4DRadarSLAM Branch: main Commit: dd2ee8c23786 Files: 79 Total size: 508.8 KB Directory structure: gitextract_sqtxga8a/ ├── CMakeLists.txt ├── LICENSE ├── README.md ├── apps/ │ ├── preprocessing_nodelet.cpp │ ├── radar_graph_slam_nodelet.cpp │ └── scan_matching_odometry_nodelet.cpp ├── cmake/ │ └── FindG2O.cmake ├── config/ │ └── params.yaml ├── include/ │ ├── dbscan/ │ │ ├── DBSCAN_kdtree.h │ │ ├── DBSCAN_precomp.h │ │ └── DBSCAN_simple.h │ ├── g2o/ │ │ ├── edge_plane_identity.hpp │ │ ├── edge_plane_parallel.hpp │ │ ├── edge_plane_prior.hpp │ │ ├── edge_se3_gt_utm.hpp │ │ ├── edge_se3_plane.hpp │ │ ├── edge_se3_priorquat.hpp │ │ ├── edge_se3_priorvec.hpp │ │ ├── edge_se3_priorxy.hpp │ │ ├── edge_se3_priorxyz.hpp │ │ ├── edge_se3_priorz.hpp │ │ ├── edge_se3_se3.hpp │ │ ├── edge_se3_z.hpp │ │ └── robust_kernel_io.hpp │ ├── radar_ego_velocity_estimator.h │ ├── radar_graph_slam/ │ │ ├── graph_slam.hpp │ │ ├── information_matrix_calculator.hpp │ │ ├── keyframe.hpp │ │ ├── keyframe_updater.hpp │ │ ├── loop_detector.hpp │ │ ├── map_cloud_generator.hpp │ │ ├── nmea_sentence_parser.hpp │ │ ├── polynomial_interpolation.hpp │ │ ├── registrations.hpp │ │ ├── ros_time_hash.hpp │ │ └── ros_utils.hpp │ ├── rio_utils/ │ │ ├── data_types.h │ │ ├── math_helper.h │ │ ├── radar_point_cloud.h │ │ ├── ros_helper.h │ │ ├── simple_profiler.h │ │ └── strapdown.h │ ├── scan_context/ │ │ ├── KDTreeVectorOfVectorsAdaptor.h │ │ ├── Scancontext.h │ │ ├── nanoflann.hpp │ │ └── tictoc.h │ └── utility_radar.h ├── launch/ │ ├── radar_graph_slam.launch │ ├── rosbag_play_radar_carpark1.launch │ ├── rosbag_play_radar_carpark3.launch │ ├── rosbag_play_radar_garden.launch │ ├── rosbag_play_radar_nanyanglink.launch │ ├── rosbag_play_radar_ntuloop1.launch │ ├── rosbag_play_radar_ntuloop2.launch │ ├── rosbag_play_radar_ntuloop3.launch │ └── rosbag_play_radar_smoke.launch ├── msg/ │ ├── FloorCoeffs.msg │ └── ScanMatchingStatus.msg ├── nodelet_plugins.xml ├── package.xml ├── rviz/ │ └── radar_graph_slam.rviz ├── setup.py ├── src/ │ ├── g2o/ │ │ └── robust_kernel_io.cpp │ ├── gps_traj_align.cpp │ ├── gt_adjust.cpp │ ├── radar_ego_velocity_estimator.cpp │ └── radar_graph_slam/ │ ├── Scancontext.cpp │ ├── __init__.py │ ├── bag_player.py │ ├── ford2bag.py │ ├── graph_slam.cpp │ ├── information_matrix_calculator.cpp │ ├── keyframe.cpp │ ├── loop_detector.cpp │ ├── map2odom_publisher.py │ ├── map_cloud_generator.cpp │ └── registrations.cpp └── srv/ ├── DumpGraph.srv └── SaveMap.srv ================================================ FILE CONTENTS ================================================ ================================================ FILE: CMakeLists.txt ================================================ # SPDX-License-Identifier: BSD-2-Clause cmake_minimum_required(VERSION 2.8.3) project(radar_graph_slam) # Can we use C++17 in indigo? add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) # add_definitions("-Wall -g") set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") # pcl 1.7 causes a segfault when it is built with debug mode set(CMAKE_BUILD_TYPE "RELEASE") find_package(catkin REQUIRED COMPONENTS roscpp rospy pcl_ros geodesy nmea_msgs sensor_msgs geometry_msgs message_generation interactive_markers ndt_omp fast_gicp image_transport cv_bridge # rosopencv ) catkin_python_setup() find_package(OpenCV REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) message(STATUS "version: ${OpenCV_VERSION}") find_package(PCL REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS}) message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS}) message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS}) find_package(G2O REQUIRED) include_directories(SYSTEM ${G2O_INCLUDE_DIR} ${G2O_INCLUDE_DIRS}) link_directories(${G2O_LIBRARY_DIRS}) # link_libraries(${G2O_LIBRARIES}) find_package(OpenMP) if (OPENMP_FOUND) set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() find_package(GTSAM REQUIRED QUIET) link_directories(${GTSAM_LIBRARY_DIRS}) find_library(VGICP_CUDA_FOUND NAMES fast_vgicp_cuda) message(STATUS "VGICP_CUDA_FOUND:" ${VGICP_CUDA_FOUND}) if(VGICP_CUDA_FOUND) add_definitions(-DUSE_VGICP_CUDA) endif() ######################## ## message generation ## ######################## add_message_files(FILES FloorCoeffs.msg ScanMatchingStatus.msg ) add_service_files(FILES SaveMap.srv DumpGraph.srv ) generate_messages(DEPENDENCIES std_msgs geometry_msgs) ################################### ## catkin specific configuration ## ################################### catkin_package( INCLUDE_DIRS include LIBRARIES radar_graph_slam_nodelet # CATKIN_DEPENDS pcl_ros roscpp sensor_msgs DEPENDS system_lib GTSAM OpenCV ) ########### ## Build ## ########### include_directories(include) include_directories( ${PCL_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} "~/sensor_ws/devel/include" ) # nodelets add_library(preprocessing_nodelet apps/preprocessing_nodelet.cpp src/radar_ego_velocity_estimator.cpp #src/lib/radar_body_velocity_estimator.cpp #src/lib/radar_body_velocity_estimator_ros.cpp ) target_link_libraries(preprocessing_nodelet ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES} ) add_dependencies(preprocessing_nodelet ${PROJECT_NAME}_gencpp) add_library(scan_matching_odometry_nodelet apps/scan_matching_odometry_nodelet.cpp src/radar_graph_slam/registrations.cpp ) target_link_libraries(scan_matching_odometry_nodelet ${catkin_LIBRARIES} ${PCL_LIBRARIES} ) add_dependencies(scan_matching_odometry_nodelet ${PROJECT_NAME}_gencpp) add_library(radar_graph_slam_nodelet apps/radar_graph_slam_nodelet.cpp src/radar_graph_slam/graph_slam.cpp src/radar_graph_slam/loop_detector.cpp src/radar_graph_slam/Scancontext.cpp src/radar_graph_slam/keyframe.cpp src/radar_graph_slam/map_cloud_generator.cpp src/radar_graph_slam/registrations.cpp src/radar_graph_slam/information_matrix_calculator.cpp src/g2o/robust_kernel_io.cpp ) target_link_libraries(radar_graph_slam_nodelet ${catkin_LIBRARIES} ${OpenCV_LIBS} ${PCL_LIBRARIES} ${G2O_TYPES_DATA} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE} # be aware of that CSPARSE is released under LGPL ${G2O_SOLVER_CHOLMOD} # be aware of that cholmod is released under GPL ${G2O_TYPES_SLAM3D} ${G2O_TYPES_SLAM3D_ADDONS} ) add_dependencies(radar_graph_slam_nodelet ${PROJECT_NAME}_gencpp) # Adjust Groundtruth add_executable(gt_adjust src/gt_adjust.cpp ) target_link_libraries(gt_adjust radar_graph_slam_nodelet ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${G2O_TYPES_DATA} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE} # be aware of that CSPARSE is released under LGPL ${G2O_SOLVER_CHOLMOD} # be aware of that cholmod is released under GPL ${G2O_TYPES_SLAM3D} ${G2O_TYPES_SLAM3D_ADDONS} ) # Calculate UTM->world Transform add_executable(gps_traj_align src/gps_traj_align.cpp ) target_link_libraries(gps_traj_align radar_graph_slam_nodelet ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${G2O_TYPES_DATA} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE} # be aware of that CSPARSE is released under LGPL ${G2O_SOLVER_CHOLMOD} # be aware of that cholmod is released under GPL ${G2O_TYPES_SLAM3D} ${G2O_TYPES_SLAM3D_ADDONS} ) catkin_install_python( PROGRAMS src/${PROJECT_NAME}/bag_player.py src/${PROJECT_NAME}/ford2bag.py src/${PROJECT_NAME}/map2odom_publisher.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install(TARGETS preprocessing_nodelet scan_matching_odometry_nodelet radar_graph_slam_nodelet LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) ================================================ FILE: LICENSE ================================================ GNU GENERAL PUBLIC LICENSE Version 3, 29 June 2007 Copyright (C) 2007 Free Software Foundation, Inc. Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed. Preamble The GNU General Public License is a free, copyleft license for software and other kinds of works. The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. 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 them 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 prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others. For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. 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. Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it. For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions. Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users. Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free. The precise terms and conditions for copying, distribution and modification follow. TERMS AND CONDITIONS 0. Definitions. "This License" refers to version 3 of the GNU General Public License. "Copyright" also means copyright-like laws that apply to other kinds of works, such as semiconductor masks. "The Program" refers to any copyrightable work licensed under this License. Each licensee is addressed as "you". "Licensees" and "recipients" may be individuals or organizations. To "modify" a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a "modified version" of the earlier work or a work "based on" the earlier work. A "covered work" means either the unmodified Program or a work based on the Program. To "propagate" a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well. To "convey" a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying. An interactive user interface displays "Appropriate Legal Notices" to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion. 1. Source Code. The "source code" for a work means the preferred form of the work for making modifications to it. "Object code" means any non-source form of a work. A "Standard Interface" means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language. The "System Libraries" of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A "Major Component", in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it. The "Corresponding Source" for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work. The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source. The Corresponding Source for a work in source code form is that same work. 2. Basic Permissions. All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law. You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you. Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary. 3. Protecting Users' Legal Rights From Anti-Circumvention Law. No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures. When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures. 4. Conveying Verbatim Copies. You may convey 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; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program. You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee. 5. Conveying Modified Source Versions. You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions: a) The work must carry prominent notices stating that you modified it, and giving a relevant date. b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to "keep intact all notices". c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it. d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so. A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an "aggregate" if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate. 6. Conveying Non-Source Forms. You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways: a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange. b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge. c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b. d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements. e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d. A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work. A "User Product" is either (1) a "consumer product", which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, "normally used" refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product. "Installation Information" for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made. If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM). The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network. Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying. 7. Additional Terms. "Additional permissions" are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions. When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission. Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms: a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or d) Limiting the use for publicity purposes of names of licensors or authors of the material; or e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors. All other non-permissive additional terms are considered "further restrictions" within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying. If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms. Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way. 8. Termination. You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11). However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation. Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice. Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10. 9. Acceptance Not Required for Having Copies. You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so. 10. Automatic Licensing of Downstream Recipients. Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License. An "entity transaction" is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts. You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it. 11. Patents. A "contributor" is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's "contributor version". A contributor's "essential patent claims" are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, "control" includes the right to grant patent sublicenses in a manner consistent with the requirements of this License. Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version. In the following three paragraphs, a "patent license" is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To "grant" such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party. If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. "Knowingly relying" means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid. If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it. A patent license is "discriminatory" if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007. Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law. 12. No Surrender of Others' Freedom. If 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 convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program. 13. Use with the GNU Affero General Public License. Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such. 14. Revised Versions of this License. The Free Software Foundation may publish revised and/or new versions of the GNU 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 that a certain numbered version of the GNU General Public License "or any later version" applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation. If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program. Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version. 15. Disclaimer of Warranty. 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. 16. Limitation of Liability. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 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. 17. Interpretation of Sections 15 and 16. If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee. 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 state the exclusion of warranty; and each file should have at least the "copyright" line and a pointer to where the full notice is found. Copyright (C) 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 3 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, see . Also add information on how to contact you by electronic and paper mail. If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode: Copyright (C) This program 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, your program's commands might be different; for a GUI interface, you would use an "about box". You should also get your employer (if you work as a programmer) or school, if any, to sign a "copyright disclaimer" for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see . The GNU 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. But first, please read . ================================================ FILE: README.md ================================================ # 4DRadarSLAM ## A 4D Imaging Radar SLAM System for Large-scale Environments based on Pose Graph Optimization [Paper (ResearchGate)](https://www.researchgate.net/publication/371309896_4DRadarSLAM_A_4D_Imaging_Radar_SLAM_System_for_Large-scale_Environments_based_on_Pose_Graph_Optimization), [IEEEXplore](https://ieeexplore.ieee.org/document/10160670), [Video](https://www.youtube.com/watch?v=Qlvs7ywA5TI), [Dataset (NTU4DRadLM)](https://github.com/junzhang2016/NTU4DRadLM) ***4DRadarSLAM*** is an open source ROS package for real-time 6DOF SLAM using a 4D Radar. It is based on 3D Graph SLAM with Adaptive Probability Distribution GICP scan matching-based odometry estimation and Intensity Scan Context loop detection. It also supports several graph constraints, such as GPS. We have tested this package with ***Oculli Eagle*** in outdoor structured (buildings), unstructured (trees and grasses) and semi-structured environments. 4DRadarSLAM can operate in adverse wheather. We did a experiment in which sensors are covered by dense ***Smoke***. The Lidar SLAM (R2LIVE) failed, but our 4DRadarSLAM is not affected by it, thanks to the penetration of millimeter waves to small objects such as smoke and rain.

drawing

## 1. Dependency ### 1.1 **Ubuntu** and **ROS** Ubuntu 64-bit 18.04 or 20.04. ROS Melodic or Noetic. [ROS Installation](http://wiki.ros.org/ROS/Installation): ### 1.2 ***4DRadarSLAM*** requires the following libraries: - Eigen3 - OpenMP - PCL - g2o ### 1.3 The following ROS packages are required: - geodesy - nmea_msgs - pcl_ros - Our modified [fast_apdgicp](https://github.com/zhuge2333/fast_apdgicp), in which Adaptive Probability Distribution GICP algorithum module is added. The original is [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) - [barometer_bmp388](https://github.com/zhuge2333/barometer_bmp388.git) ``` sudo apt-get install ros-XXX-geodesy ros-XXX-pcl-ros ros-XXX-nmea-msgs ros-XXX-libg2o ``` **NOTICE:** remember to replace "XXX" on above command as your ROS distributions, for example, if your use ROS-noetic, the command should be: ``` sudo apt-get install ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs ros-noetic-libg2o ``` ## 2. System architecture ***4DRadarSLAM*** consists of three nodelets. - *preprocessing_nodelet* - *scan_matching_odometry_nodelet* - *radar_graph_slam_nodelet* The input point cloud is first downsampled by ***preprocessing_nodelet***; the radar pointcloud is transformed to Livox LiDAR frame; estimate its ego velocity and remove dynamic objects, and then passed to the next nodelets. While scan_matching_odometry_nodelet estimates the sensor pose by iteratively applying a scan matching between consecutive frames (i.e., odometry estimation). The estimated odometry are sent to ***radar_graph_slam***. To compensate the accumulated error of the scan matching, it performs loop detection and optimizes a pose graph which takes various constraints into account.
## 3. Parameter tuning guide The mapping quality largely depends on the parameter setting. In particular, scan matching parameters have a big impact on the result. Tune the parameters accoding to the following instructions: ### 3.1 Point cloud registration - ***registration_method*** This parameter allows to change the registration method to be used for odometry estimation and loop detection. Our code gives five options: ICP, NDT_OMP, FAST_GICP, FAST_APDGICP, FAST_VGICP. FAST_APDGICP is the implementation of our proposed Adaptive Probability Distribution GICP, it utilizes OpenMP for acceleration. Note that FAST_APDGICP requires extra parameters. Point uncertainty parameters: - ***dist_var*** - ***azimuth_var*** - ***elevation_var*** *dist_var* means the uncertainty of a point’s range measurement at 100m range, *azimuth_var* and *elevation_var* denote the azimuth and elevation angle accuracy (degree) ### 3.2 Loop detection - ***accum_distance_thresh***: Minimum distance beteen two edges of the loop - ***min_loop_interval_dist***: Minimum distance between a new loop edge and the last one - ***max_baro_difference***: Maximum altitude difference beteen two edges' odometry - ***max_yaw_difference***: Maximum yaw difference beteen two edges' odometry - ***odom_check_trans_thresh***: Translation threshold of Odometry Check - ***odom_check_rot_thresh***: Rotation threshold of Odometry Check - ***sc_dist_thresh***: Matching score threshold of Scan Context ### 3.3 Other parameters All the configurable parameters are available in the launch file. Many are similar to the project ***hdl_graph_slam***. ## 4. Run the package Download [our recorded rosbag](https://drive.google.com/drive/folders/14jVa_dzmckVMDdfELmY32fJlKrZG1Afv?usp=sharing) (**More datasets**: [NTU4DRadLM](https://github.com/junzhang2016/NTU4DRadLM)) and, then ``` roslaunch radar_graph_slam radar_graph_slam.launch ``` You'll see a point cloud like:
You can choose the dataset to play at end of the launch file. In our paper, we did evaluation on five datasets, mapping results are presented below:
## 5. Evaluate the results In our paper, we use [rpg_trajectory_evaluation](https://github.com/uzh-rpg/rpg_trajectory_evaluation.git), the performance indices used are RE (relative error) and ATE (absolute trajectory error). ## 6. Collect your own datasets You need a 4D Imaging radar (we use Oculii's Eagle). Also, a barometer (we use BMP388) and GPS/RTK-GPS (we use ZED-F9P) are optional. If you need to compare Lidar SLAM between the algorithum, or use its trajectory as ground truth, calibrating the transform between Radar and Lidar is a precondition. ## 7. Acknowlegement 1. 4DRadarSLAM is based on [koide3/hdl_graph_slam](https://github.com/koide3/hdl_graph_slam) 2. [irapkaist/scancontext](https://github.com/irapkaist/scancontext) scan context 3. [wh200720041/iscloam](https://github.com/wh200720041/iscloam) intensity scan context 4. [christopherdoer/reve](https://github.com/christopherdoer/reve) radar ego-velocity estimator 5. [NeBula-Autonomy/LAMP](https://github.com/NeBula-Autonomy/LAMP) odometry check for loop closure validation 6. [slambook-en](https://github.com/gaoxiang12/slambook-en) and [Dr. Gao Xiang (高翔)](https://github.com/gaoxiang12). His SLAM tutorial and blogs are the starting point of our SLAM journey. 7. [lvt2calib](https://github.com/Clothooo/lvt2calib) by LIUY Clothooo for sensor calibration. ## 8. Citation If you find this work is useful for your research, please consider citing: ``` @INPROCEEDINGS{ZhangZhuge2023ICRA, author={Zhang, Jun and Zhuge, Huayang and Wu, Zhenyu and Peng, Guohao and Wen, Mingxing and Liu, Yiyao and Wang, Danwei}, booktitle={2023 IEEE International Conference on Robotics and Automation (ICRA)}, title={4DRadarSLAM: A 4D Imaging Radar SLAM System for Large-scale Environments based on Pose Graph Optimization}, year={2023}, volume={}, number={}, pages={8333-8340}, doi={10.1109/ICRA48891.2023.10160670}} ``` ``` @INPROCEEDINGS{ZhangZhugeLiu2023ITSC, author={Jun Zhang∗, Huayang Zhuge∗, Yiyao Liu∗, Guohao Peng, Zhenyu Wu, Haoyuan Zhang, Qiyang Lyu, Heshan Li, Chunyang Zhao, Dogan Kircali, Sanat Mharolkar, Xun Yang, Su Yi, Yuanzhe Wang+ and Danwei Wang}, booktitle={2023 IEEE 26th International Conference on Intelligent Transportation Systems (ITSC)}, title={NTU4DRadLM: 4D Radar-centric Multi-Modal Dataset for Localization and Mapping}, year={2023}, volume={}, number={}, pages={}, doi={}} ``` ================================================ FILE: apps/preprocessing_nodelet.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "radar_ego_velocity_estimator.h" #include "rio_utils/radar_point_cloud.h" #include "utility_radar.h" using namespace std; namespace radar_graph_slam { class PreprocessingNodelet : public nodelet::Nodelet, public ParamServer { public: // typedef pcl::PointXYZI PointT; typedef pcl::PointXYZI PointT; PreprocessingNodelet() {} virtual ~PreprocessingNodelet() {} virtual void onInit() { nh = getNodeHandle(); private_nh = getPrivateNodeHandle(); initializeTransformation(); initializeParams(); points_sub = nh.subscribe(pointCloudTopic, 64, &PreprocessingNodelet::cloud_callback, this); imu_sub = nh.subscribe(imuTopic, 1, &PreprocessingNodelet::imu_callback, this); command_sub = nh.subscribe("/command", 10, &PreprocessingNodelet::command_callback, this); points_pub = nh.advertise("/filtered_points", 32); colored_pub = nh.advertise("/colored_points", 32); imu_pub = nh.advertise("/imu", 32); gt_pub = nh.advertise("/aftmapped_to_init", 16); std::string topic_twist = private_nh.param("topic_twist", "/eagle_data/twist"); std::string topic_inlier_pc2 = private_nh.param("topic_inlier_pc2", "/eagle_data/inlier_pc2"); std::string topic_outlier_pc2 = private_nh.param("topic_outlier_pc2", "/eagle_data/outlier_pc2"); pub_twist = nh.advertise(topic_twist, 5); pub_inlier_pc2 = nh.advertise(topic_inlier_pc2, 5); pub_outlier_pc2 = nh.advertise(topic_outlier_pc2, 5); pc2_raw_pub = nh.advertise("/eagle_data/pc2_raw",1); enable_dynamic_object_removal = private_nh.param("enable_dynamic_object_removal", false); power_threshold = private_nh.param("power_threshold", 0); } private: void initializeTransformation(){ livox_to_RGB = (cv::Mat_(4,4) << -0.006878330000, -0.999969000000, 0.003857230000, 0.029164500000, -7.737180000000E-05, -0.003856790000, -0.999993000000, 0.045695200000, 0.999976000000, -0.006878580000, -5.084110000000E-05, -0.19018000000, 0, 0, 0, 1); RGB_to_livox =livox_to_RGB.inv(); Thermal_to_RGB = (cv::Mat_(4,4) << 0.9999526089706319, 0.008963747151337641, -0.003798822163962599, 0.18106962419014, -0.008945181135788245, 0.9999481006917174, 0.004876439015823288, -0.04546324090016857, 0.00384233617405678, -0.004842226763999368, 0.999980894463835, 0.08046453079998771, 0,0,0,1); Radar_to_Thermal = (cv::Mat_(4,4) << 0.999665, 0.00925436, -0.0241851, -0.0248342, -0.00826999, 0.999146, 0.0404891, 0.0958317, 0.0245392, -0.0402755, 0.998887, 0.0268037, 0, 0, 0, 1); Change_Radarframe=(cv::Mat_(4,4) << 0,-1,0,0, 0,0,-1,0, 1,0,0,0, 0,0,0,1); Radar_to_livox=RGB_to_livox*Thermal_to_RGB*Radar_to_Thermal*Change_Radarframe; std::cout << "Radar_to_livox = "<< std::endl << " " << Radar_to_livox << std::endl << std::endl; } void initializeParams() { std::string downsample_method = private_nh.param("downsample_method", "VOXELGRID"); double downsample_resolution = private_nh.param("downsample_resolution", 0.1); if(downsample_method == "VOXELGRID") { std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; auto voxelgrid = new pcl::VoxelGrid(); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter.reset(voxelgrid); } else if(downsample_method == "APPROX_VOXELGRID") { std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid()); approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter = approx_voxelgrid; } else { if(downsample_method != "NONE") { std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl; std::cerr << " : use passthrough filter" << std::endl; } std::cout << "downsample: NONE" << std::endl; } std::string outlier_removal_method = private_nh.param("outlier_removal_method", "STATISTICAL"); if(outlier_removal_method == "STATISTICAL") { int mean_k = private_nh.param("statistical_mean_k", 20); double stddev_mul_thresh = private_nh.param("statistical_stddev", 1.0); std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl; pcl::StatisticalOutlierRemoval::Ptr sor(new pcl::StatisticalOutlierRemoval()); sor->setMeanK(mean_k); sor->setStddevMulThresh(stddev_mul_thresh); outlier_removal_filter = sor; } else if(outlier_removal_method == "RADIUS") { double radius = private_nh.param("radius_radius", 0.8); int min_neighbors = private_nh.param("radius_min_neighbors", 2); std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl; pcl::RadiusOutlierRemoval::Ptr rad(new pcl::RadiusOutlierRemoval()); rad->setRadiusSearch(radius); rad->setMinNeighborsInRadius(min_neighbors); outlier_removal_filter = rad; } // else if (outlier_removal_method == "BILATERAL") // { // double sigma_s = private_nh.param("bilateral_sigma_s", 5.0); // double sigma_r = private_nh.param("bilateral_sigma_r", 0.03); // std::cout << "outlier_removal: BILATERAL " << sigma_s << " - " << sigma_r << std::endl; // pcl::FastBilateralFilter::Ptr fbf(new pcl::FastBilateralFilter()); // fbf->setSigmaS (sigma_s); // fbf->setSigmaR (sigma_r); // outlier_removal_filter = fbf; // } else { std::cout << "outlier_removal: NONE" << std::endl; } use_distance_filter = private_nh.param("use_distance_filter", true); distance_near_thresh = private_nh.param("distance_near_thresh", 1.0); distance_far_thresh = private_nh.param("distance_far_thresh", 100.0); z_low_thresh = private_nh.param("z_low_thresh", -5.0); z_high_thresh = private_nh.param("z_high_thresh", 20.0); std::string file_name = private_nh.param("gt_file_location", ""); publish_tf = private_nh.param("publish_tf", false); ifstream file_in(file_name); if (!file_in.is_open()) { cout << "Can not open this gt file" << endl; } else{ std::vector vectorLines; std::string line; while (getline(file_in, line)) { vectorLines.push_back(line); } for (size_t i = 1; i < vectorLines.size(); i++) { std::string line_ = vectorLines.at(i); double stamp,tx,ty,tz,qx,qy,qz,qw; stringstream data(line_); data >> stamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw; nav_msgs::Odometry odom_msg; odom_msg.header.frame_id = mapFrame; odom_msg.child_frame_id = baselinkFrame; odom_msg.header.stamp = ros::Time().fromSec(stamp); odom_msg.pose.pose.orientation.w = qw; odom_msg.pose.pose.orientation.x = qx; odom_msg.pose.pose.orientation.y = qy; odom_msg.pose.pose.orientation.z = qz; odom_msg.pose.pose.position.x = tx; odom_msg.pose.pose.position.y = ty; odom_msg.pose.pose.position.z = tz; std::lock_guard lock(odom_queue_mutex); odom_msgs.push_back(odom_msg); } } file_in.close(); } void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) { sensor_msgs::Imu imu_data; imu_data.header.stamp = imu_msg->header.stamp; imu_data.header.seq = imu_msg->header.seq; imu_data.header.frame_id = "imu_frame"; Eigen::Quaterniond q_ahrs(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z); Eigen::Quaterniond q_r = Eigen::AngleAxisd( M_PI, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd( M_PI, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitX()); Eigen::Quaterniond q_rr = Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd( 0.00000, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd( M_PI, Eigen::Vector3d::UnitX()); Eigen::Quaterniond q_out = q_r * q_ahrs * q_rr; imu_data.orientation.w = q_out.w(); imu_data.orientation.x = q_out.x(); imu_data.orientation.y = q_out.y(); imu_data.orientation.z = q_out.z(); imu_data.angular_velocity.x = imu_msg->angular_velocity.x; imu_data.angular_velocity.y = -imu_msg->angular_velocity.y; imu_data.angular_velocity.z = -imu_msg->angular_velocity.z; imu_data.linear_acceleration.x = imu_msg->linear_acceleration.x; imu_data.linear_acceleration.y = -imu_msg->linear_acceleration.y; imu_data.linear_acceleration.z = -imu_msg->linear_acceleration.z; imu_pub.publish(imu_data); // imu_queue.push_back(imu_msg); double time_now = imu_msg->header.stamp.toSec(); bool updated = false; if (odom_msgs.size() != 0) { while (odom_msgs.front().header.stamp.toSec() + 0.001 < time_now) { std::lock_guard lock(odom_queue_mutex); odom_msgs.pop_front(); updated = true; if (odom_msgs.size() == 0) break; } } if (updated == true && odom_msgs.size() > 0){ if (publish_tf) { geometry_msgs::TransformStamped tf_msg; tf_msg.child_frame_id = baselinkFrame; tf_msg.header.frame_id = mapFrame; tf_msg.header.stamp = odom_msgs.front().header.stamp; // tf_msg.header.stamp = ros::Time().now(); tf_msg.transform.rotation = odom_msgs.front().pose.pose.orientation; tf_msg.transform.translation.x = odom_msgs.front().pose.pose.position.x; tf_msg.transform.translation.y = odom_msgs.front().pose.pose.position.y; tf_msg.transform.translation.z = odom_msgs.front().pose.pose.position.z; tf_broadcaster.sendTransform(tf_msg); } gt_pub.publish(odom_msgs.front()); } } void cloud_callback(const sensor_msgs::PointCloud::ConstPtr& eagle_msg) { // const pcl::PointCloud& src_cloud_r RadarPointCloudType radarpoint_raw; PointT radarpoint_xyzi; pcl::PointCloud::Ptr radarcloud_raw( new pcl::PointCloud ); pcl::PointCloud::Ptr radarcloud_xyzi( new pcl::PointCloud ); radarcloud_xyzi->header.frame_id = baselinkFrame; radarcloud_xyzi->header.seq = eagle_msg->header.seq; radarcloud_xyzi->header.stamp = eagle_msg->header.stamp.toSec() * 1e6; for(int i = 0; i < eagle_msg->points.size(); i++) { // cout << i << ": " <points[i].x<channels[2].values[i] > power_threshold) //"Power" { if (eagle_msg->points[i].x == NAN || eagle_msg->points[i].y == NAN || eagle_msg->points[i].z == NAN) continue; if (eagle_msg->points[i].x == INFINITY || eagle_msg->points[i].y == INFINITY || eagle_msg->points[i].z == INFINITY) continue; cv::Mat ptMat, dstMat; ptMat = (cv::Mat_(4, 1) << eagle_msg->points[i].x, eagle_msg->points[i].y, eagle_msg->points[i].z, 1); // Perform matrix multiplication and save as Mat_ for easy element access dstMat= Radar_to_livox * ptMat; radarpoint_raw.x = dstMat.at(0,0); radarpoint_raw.y = dstMat.at(1,0); radarpoint_raw.z = dstMat.at(2,0); radarpoint_raw.intensity = eagle_msg->channels[2].values[i]; radarpoint_raw.doppler = eagle_msg->channels[0].values[i]; radarpoint_xyzi.x = dstMat.at(0,0); radarpoint_xyzi.y = dstMat.at(1,0); radarpoint_xyzi.z = dstMat.at(2,0); radarpoint_xyzi.intensity = eagle_msg->channels[2].values[i]; radarcloud_raw->points.push_back(radarpoint_raw); radarcloud_xyzi->points.push_back(radarpoint_xyzi); } } //********** Publish PointCloud2 Format Raw Cloud ********** sensor_msgs::PointCloud2 pc2_raw_msg; pcl::toROSMsg(*radarcloud_raw, pc2_raw_msg); pc2_raw_msg.header.stamp = eagle_msg->header.stamp; pc2_raw_msg.header.frame_id = baselinkFrame; pc2_raw_pub.publish(pc2_raw_msg); //********** Ego Velocity Estimation ********** Eigen::Vector3d v_r, sigma_v_r; sensor_msgs::PointCloud2 inlier_radar_msg, outlier_radar_msg; clock_t start_ms = clock(); if (estimator.estimate(pc2_raw_msg, v_r, sigma_v_r, inlier_radar_msg, outlier_radar_msg)) { clock_t end_ms = clock(); double time_used = double(end_ms - start_ms) / CLOCKS_PER_SEC; egovel_time.push_back(time_used); geometry_msgs::TwistWithCovarianceStamped twist; twist.header.stamp = pc2_raw_msg.header.stamp; twist.twist.twist.linear.x = v_r.x(); twist.twist.twist.linear.y = v_r.y(); twist.twist.twist.linear.z = v_r.z(); twist.twist.covariance.at(0) = std::pow(sigma_v_r.x(), 2); twist.twist.covariance.at(7) = std::pow(sigma_v_r.y(), 2); twist.twist.covariance.at(14) = std::pow(sigma_v_r.z(), 2); pub_twist.publish(twist); pub_inlier_pc2.publish(inlier_radar_msg); pub_outlier_pc2.publish(outlier_radar_msg); } else{;} pcl::PointCloud::Ptr radarcloud_inlier( new pcl::PointCloud ); pcl::fromROSMsg (inlier_radar_msg, *radarcloud_inlier); pcl::PointCloud::ConstPtr src_cloud; if (enable_dynamic_object_removal) src_cloud = radarcloud_inlier; else src_cloud = radarcloud_xyzi; if(src_cloud->empty()) { return; } src_cloud = deskewing(src_cloud); // if baselinkFrame is defined, transform the input cloud to the frame if(!baselinkFrame.empty()) { if(!tf_listener.canTransform(baselinkFrame, src_cloud->header.frame_id, ros::Time(0))) { std::cerr << "failed to find transform between " << baselinkFrame << " and " << src_cloud->header.frame_id << std::endl; } tf::StampedTransform transform; tf_listener.waitForTransform(baselinkFrame, src_cloud->header.frame_id, ros::Time(0), ros::Duration(2.0)); tf_listener.lookupTransform(baselinkFrame, src_cloud->header.frame_id, ros::Time(0), transform); pcl::PointCloud::Ptr transformed(new pcl::PointCloud()); pcl_ros::transformPointCloud(*src_cloud, *transformed, transform); transformed->header.frame_id = baselinkFrame; transformed->header.stamp = src_cloud->header.stamp; src_cloud = transformed; } pcl::PointCloud::ConstPtr filtered = distance_filter(src_cloud); // filtered = passthrough(filtered); filtered = downsample(filtered); filtered = outlier_removal(filtered); // Distance Histogram static size_t num_frame = 0; if (num_frame % 10 == 0) { Eigen::VectorXi num_at_dist = Eigen::VectorXi::Zero(100); for (int i = 0; i < filtered->size(); i++){ int dist = floor(filtered->at(i).getVector3fMap().norm()); if (dist < 100) num_at_dist(dist) += 1; } num_at_dist_vec.push_back(num_at_dist); } points_pub.publish(*filtered); } pcl::PointCloud::ConstPtr passthrough(const pcl::PointCloud::ConstPtr& cloud) const { pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); PointT pt; for(int i = 0; i < cloud->size(); i++){ if (cloud->at(i).z < 10 && cloud->at(i).z > -2){ pt.x = (*cloud)[i].x; pt.y = (*cloud)[i].y; pt.z = (*cloud)[i].z; pt.intensity = (*cloud)[i].intensity; filtered->points.push_back(pt); } } filtered->header = cloud->header; return filtered; } pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { if(!downsample_filter) { // Remove NaN/Inf points pcl::PointCloud::Ptr cloudout(new pcl::PointCloud()); std::vector indices; pcl::removeNaNFromPointCloud(*cloud, *cloudout, indices); return cloudout; } pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); downsample_filter->setInputCloud(cloud); downsample_filter->filter(*filtered); filtered->header = cloud->header; return filtered; } pcl::PointCloud::ConstPtr outlier_removal(const pcl::PointCloud::ConstPtr& cloud) const { if(!outlier_removal_filter) { return cloud; } pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); outlier_removal_filter->setInputCloud(cloud); outlier_removal_filter->filter(*filtered); filtered->header = cloud->header; return filtered; } pcl::PointCloud::ConstPtr distance_filter(const pcl::PointCloud::ConstPtr& cloud) const { pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); filtered->reserve(cloud->size()); std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), [&](const PointT& p) { double d = p.getVector3fMap().norm(); double z = p.z; return d > distance_near_thresh && d < distance_far_thresh && z < z_high_thresh && z > z_low_thresh; }); // for (size_t i=0; isize(); i++){ // const PointT p = cloud->points.at(i); // double d = p.getVector3fMap().norm(); // double z = p.z; // if (d > distance_near_thresh && d < distance_far_thresh && z < z_high_thresh && z > z_low_thresh) // filtered->points.push_back(p); // } filtered->width = filtered->size(); filtered->height = 1; filtered->is_dense = false; filtered->header = cloud->header; return filtered; } pcl::PointCloud::ConstPtr deskewing(const pcl::PointCloud::ConstPtr& cloud) { ros::Time stamp = pcl_conversions::fromPCL(cloud->header.stamp); if(imu_queue.empty()) { return cloud; } // the color encodes the point number in the point sequence if(colored_pub.getNumSubscribers()) { pcl::PointCloud::Ptr colored(new pcl::PointCloud()); colored->header = cloud->header; colored->is_dense = cloud->is_dense; colored->width = cloud->width; colored->height = cloud->height; colored->resize(cloud->size()); for(int i = 0; i < cloud->size(); i++) { double t = static_cast(i) / cloud->size(); colored->at(i).getVector4fMap() = cloud->at(i).getVector4fMap(); colored->at(i).r = 255 * t; colored->at(i).g = 128; colored->at(i).b = 255 * (1 - t); } colored_pub.publish(*colored); } sensor_msgs::ImuConstPtr imu_msg = imu_queue.front(); auto loc = imu_queue.begin(); for(; loc != imu_queue.end(); loc++) { imu_msg = (*loc); if((*loc)->header.stamp > stamp) { break; } } imu_queue.erase(imu_queue.begin(), loc); Eigen::Vector3f ang_v(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); ang_v *= -1; pcl::PointCloud::Ptr deskewed(new pcl::PointCloud()); deskewed->header = cloud->header; deskewed->is_dense = cloud->is_dense; deskewed->width = cloud->width; deskewed->height = cloud->height; deskewed->resize(cloud->size()); double scan_period = private_nh.param("scan_period", 0.1); for(int i = 0; i < cloud->size(); i++) { const auto& pt = cloud->at(i); // TODO: transform IMU data into the LIDAR frame double delta_t = scan_period * static_cast(i) / cloud->size(); Eigen::Quaternionf delta_q(1, delta_t / 2.0 * ang_v[0], delta_t / 2.0 * ang_v[1], delta_t / 2.0 * ang_v[2]); Eigen::Vector3f pt_ = delta_q.inverse() * pt.getVector3fMap(); deskewed->at(i) = cloud->at(i); deskewed->at(i).getVector3fMap() = pt_; } return deskewed; } bool RadarRaw2PointCloudXYZ(const pcl::PointCloud::ConstPtr &raw, pcl::PointCloud::Ptr &cloudxyz) { pcl::PointXYZ point_xyz; for(int i = 0; i < raw->size(); i++) { point_xyz.x = (*raw)[i].x; point_xyz.y = (*raw)[i].y; point_xyz.z = (*raw)[i].z; cloudxyz->points.push_back(point_xyz); } return true; } bool RadarRaw2PointCloudXYZI(const pcl::PointCloud::ConstPtr &raw, pcl::PointCloud::Ptr &cloudxyzi) { pcl::PointXYZI radarpoint_xyzi; for(int i = 0; i < raw->size(); i++) { radarpoint_xyzi.x = (*raw)[i].x; radarpoint_xyzi.y = (*raw)[i].y; radarpoint_xyzi.z = (*raw)[i].z; radarpoint_xyzi.intensity = (*raw)[i].intensity; cloudxyzi->points.push_back(radarpoint_xyzi); } return true; } void command_callback(const std_msgs::String& str_msg) { if (str_msg.data == "time") { std::sort(egovel_time.begin(), egovel_time.end()); double median = egovel_time.at(size_t(egovel_time.size() / 2)); cout << "Ego velocity time cost (median): " << median << endl; } else if (str_msg.data == "point_distribution") { Eigen::VectorXi data(100); for (size_t i = 0; i < num_at_dist_vec.size(); i++){ // N Eigen::VectorXi& nad = num_at_dist_vec.at(i); for (int j = 0; j< 100; j++){ data(j) += nad(j); } } data /= num_at_dist_vec.size(); for (int i=0; i imu_queue; ros::Subscriber points_sub; ros::Publisher points_pub; ros::Publisher colored_pub; ros::Publisher imu_pub; ros::Publisher gt_pub; tf::TransformListener tf_listener; tf::TransformBroadcaster tf_broadcaster; bool use_distance_filter; double distance_near_thresh; double distance_far_thresh; double z_low_thresh; double z_high_thresh; pcl::Filter::Ptr downsample_filter; pcl::Filter::Ptr outlier_removal_filter; cv::Mat Radar_to_livox; // Transform Radar point cloud to LiDAR Frame cv::Mat Thermal_to_RGB,Radar_to_Thermal,RGB_to_livox,livox_to_RGB,Change_Radarframe; rio::RadarEgoVelocityEstimator estimator; ros::Publisher pub_twist, pub_inlier_pc2, pub_outlier_pc2, pc2_raw_pub; float power_threshold; bool enable_dynamic_object_removal = false; std::mutex odom_queue_mutex; std::deque odom_msgs; bool publish_tf; ros::Subscriber command_sub; std::vector egovel_time; std::vector num_at_dist_vec; }; } // namespace radar_graph_slam PLUGINLIB_EXPORT_CLASS(radar_graph_slam::PreprocessingNodelet, nodelet::Nodelet) ================================================ FILE: apps/radar_graph_slam_nodelet.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "radar_graph_slam/polynomial_interpolation.hpp" #include #include "scan_context/Scancontext.h" #include #include #include #include #include #include #include #include #include #include "utility_radar.h" using namespace std; namespace radar_graph_slam { class RadarGraphSlamNodelet : public nodelet::Nodelet, public ParamServer { public: typedef pcl::PointXYZI PointT; typedef PointXYZIRPYT PointTypePose; typedef message_filters::sync_policies::ApproximateTime ApproxSyncPolicy; RadarGraphSlamNodelet() {} virtual ~RadarGraphSlamNodelet() {} virtual void onInit() { nh = getNodeHandle(); mt_nh = getMTNodeHandle(); private_nh = getPrivateNodeHandle(); // init parameters map_cloud_resolution = private_nh.param("map_cloud_resolution", 0.05); trans_odom2map.setIdentity(); trans_aftmapped.setIdentity(); trans_aftmapped_incremental.setIdentity(); initial_pose.setIdentity(); max_keyframes_per_update = private_nh.param("max_keyframes_per_update", 10); anchor_node = nullptr; anchor_edge = nullptr; floor_plane_node = nullptr; graph_slam.reset(new GraphSLAM(private_nh.param("g2o_solver_type", "lm_var"))); keyframe_updater.reset(new KeyframeUpdater(private_nh)); loop_detector.reset(new LoopDetector(private_nh)); map_cloud_generator.reset(new MapCloudGenerator()); inf_calclator.reset(new InformationMatrixCalculator(private_nh)); nmea_parser.reset(new NmeaSentenceParser()); gps_edge_intervals = private_nh.param("gps_edge_intervals", 10); gps_time_offset = private_nh.param("gps_time_offset", 0.0); gps_edge_stddev_xy = private_nh.param("gps_edge_stddev_xy", 10000.0); gps_edge_stddev_z = private_nh.param("gps_edge_stddev_z", 10.0); max_gps_edge_stddev_xy = private_nh.param("max_gps_edge_stddev_xy", 1.0); max_gps_edge_stddev_z = private_nh.param("max_gps_edge_stddev_z", 2.0); // Preintegration Parameters enable_preintegration = private_nh.param("enable_preintegration", false); use_egovel_preinteg_trans = private_nh.param("use_egovel_preinteg_trans", false); preinteg_trans_stddev = private_nh.param("preinteg_trans_stddev", 1.0); preinteg_orient_stddev = private_nh.param("preinteg_orient_stddev", 2.0); enable_barometer = private_nh.param("enable_barometer", false); barometer_edge_type = private_nh.param("barometer_edge_type", 2); barometer_edge_stddev = private_nh.param("barometer_edge_stddev", 0.5); points_topic = private_nh.param("points_topic", "/radar_enhanced_pcl"); show_sphere = private_nh.param("show_sphere", false); dataset_name = private_nh.param("dataset_name", ""); registration = select_registration_method(private_nh); // subscribers odom_sub.reset(new message_filters::Subscriber(mt_nh, odomTopic, 256)); cloud_sub.reset(new message_filters::Subscriber(mt_nh, "/filtered_points", 32)); sync.reset(new message_filters::Synchronizer(ApproxSyncPolicy(32), *odom_sub, *cloud_sub)); sync->registerCallback(boost::bind(&RadarGraphSlamNodelet::cloud_callback, this, _1, _2)); if(private_nh.param("enable_gps", true)) { gps_sub = mt_nh.subscribe("/gps/geopoint", 1024, &RadarGraphSlamNodelet::gps_callback, this); nmea_sub = mt_nh.subscribe("/gpsimu_driver/nmea_sentence", 1024, &RadarGraphSlamNodelet::nmea_callback, this); navsat_sub = mt_nh.subscribe(gpsTopic, 1024, &RadarGraphSlamNodelet::navsat_callback, this); } if(private_nh.param("enable_barometer", false)) { barometer_sub = mt_nh.subscribe("/barometer/filtered", 16, &RadarGraphSlamNodelet::barometer_callback, this); } if (enable_preintegration) imu_odom_sub = nh.subscribe("/imu_pre_integ/imu_odom_incre", 1024, &RadarGraphSlamNodelet::imu_odom_callback, this); imu_sub = nh.subscribe("/imu", 1024, &RadarGraphSlamNodelet::imu_callback, this); command_sub = nh.subscribe("/command", 10, &RadarGraphSlamNodelet::command_callback, this); //***** publishers ****** markers_pub = mt_nh.advertise("/radar_graph_slam/markers", 16); // Transform RadarOdom_to_base odom2base_pub = mt_nh.advertise("/radar_graph_slam/odom2base", 16); aftmapped_odom_pub = mt_nh.advertise("/radar_graph_slam/aftmapped_odom", 16); aftmapped_odom_incremenral_pub = mt_nh.advertise("/radar_graph_slam/aftmapped_odom_incremental", 16); map_points_pub = mt_nh.advertise("/radar_graph_slam/map_points", 1, true); read_until_pub = mt_nh.advertise("/radar_graph_slam/read_until", 32); odom_frame2frame_pub = mt_nh.advertise("/radar_graph_slam/odom_frame2frame", 16); dump_service_server = mt_nh.advertiseService("/radar_graph_slam/dump", &RadarGraphSlamNodelet::dump_service, this); save_map_service_server = mt_nh.advertiseService("/radar_graph_slam/save_map", &RadarGraphSlamNodelet::save_map_service, this); graph_updated = false; double graph_update_interval = private_nh.param("graph_update_interval", 3.0); double map_cloud_update_interval = private_nh.param("map_cloud_update_interval", 10.0); optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), &RadarGraphSlamNodelet::optimization_timer_callback, this); map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &RadarGraphSlamNodelet::map_points_publish_timer_callback, this); if (dataset_name == "loop3") utm_to_world << -0.057621, 0.996222, -0.064972, -128453.624105, -0.998281, -0.058194, -0.006954, 361869.958099, -0.010708, 0.064459, 0.997863, -5882.237973, 0.000000, 0.000000, 0.000000, 1.000000; else if (dataset_name == "loop2") utm_to_world << -0.085585, 0.995774, -0.033303, -117561.214476, -0.996323, -0.085401, 0.006904, 364927.287181, 0.004031, 0.033772, 0.999421, -6478.377842, 0.000000, 0.000000, 0.000000, 1.000000; } private: /** * @brief received point clouds are pushed to #keyframe_queue * @param odom_msg * @param cloud_msg */ void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) { const ros::Time& stamp = cloud_msg->header.stamp; Eigen::Isometry3d odom_now = odom2isometry(odom_msg); Eigen::Matrix4d matrix_map2base; // Publish TF between /map and /base_link if(keyframes.size() > 0) { const KeyFrame::Ptr& keyframe_last = keyframes.back(); Eigen::Isometry3d lastkeyframe_odom_incre = keyframe_last->odom_scan2scan.inverse() * odom_now; Eigen::Isometry3d keyframe_map2base_matrix = keyframe_last->node->estimate(); // map2base = odom^(-1) * base matrix_map2base = (keyframe_map2base_matrix * lastkeyframe_odom_incre).matrix(); } geometry_msgs::TransformStamped map2base_trans = matrix2transform(cloud_msg->header.stamp, matrix_map2base, mapFrame, baselinkFrame); if (pow(map2base_trans.transform.rotation.w,2)+pow(map2base_trans.transform.rotation.x,2)+ pow(map2base_trans.transform.rotation.y,2)+pow(map2base_trans.transform.rotation.z,2) < pow(0.9,2)) {map2base_trans.transform.rotation.w=1; map2base_trans.transform.rotation.x=0; map2base_trans.transform.rotation.y=0; map2base_trans.transform.rotation.z=0;} map2base_broadcaster.sendTransform(map2base_trans); pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); pcl::fromROSMsg(*cloud_msg, *cloud); if(baselinkFrame.empty()) { baselinkFrame = cloud_msg->header.frame_id; } // Push ego velocity to queue geometry_msgs::TwistStamped::Ptr twist_(new geometry_msgs::TwistStamped); twist_->header.stamp = cloud_msg->header.stamp; twist_->twist.linear = odom_msg->twist.twist.linear; { std::lock_guard lock(keyframe_queue_mutex); twist_queue.push_back(twist_); } //********** Decided whether to accept the frame as a key frame or not ********** if(!keyframe_updater->decide(odom_now, stamp)) { std::lock_guard lock(keyframe_queue_mutex); if(keyframe_queue.empty()) { std_msgs::Header read_until; read_until.stamp = stamp + ros::Duration(10, 0); read_until.frame_id = points_topic; read_until_pub.publish(read_until); read_until.frame_id = "/filtered_points"; read_until_pub.publish(read_until); } return; } // Get time of this key frame for Intergeration, to integerate between two key frames thisKeyframeTime = cloud_msg->header.stamp.toSec(); double accum_d = keyframe_updater->get_accum_distance(); // Construct keyframe KeyFrame::Ptr keyframe(new KeyFrame(keyframe_index, stamp, odom_now, accum_d, cloud)); keyframe_index ++; if (enable_preintegration){ // Intergerate translation of ego velocity, add rotation geometry_msgs::Transform transf_integ = preIntegrationTransform(); static uint32_t sequ = 0; nav_msgs::Odometry odom_frame2frame; odom_frame2frame.pose.pose.orientation = transf_integ.rotation; odom_frame2frame.pose.pose.position.x = transf_integ.translation.x; odom_frame2frame.pose.pose.position.y = transf_integ.translation.y; odom_frame2frame.pose.pose.position.z = transf_integ.translation.z; odom_frame2frame.header.frame_id = "map"; odom_frame2frame.header.stamp = cloud_msg->header.stamp; odom_frame2frame.header.seq = sequ; sequ ++; odom_frame2frame_pub.publish(odom_frame2frame); keyframe->trans_integrated = transf_integ; } std::lock_guard lock(keyframe_queue_mutex); keyframe_queue.push_back(keyframe); // Scan Context loop detector - giseop // - SINGLE_SCAN_FULL: using downsampled original point cloud (/full_cloud_projected + downsampling) // - SINGLE_SCAN_FEAT: using surface feature as an input point cloud for scan context (2020.04.01: checked it works.) // - MULTI_SCAN_FEAT: using NearKeyframes (because a MulRan scan does not have beyond region, so to solve this issue ... ) const SCInputType sc_input_type = SCInputType::SINGLE_SCAN_FULL; // change this if( sc_input_type == SCInputType::SINGLE_SCAN_FULL ) { loop_detector->scManager->makeAndSaveScancontextAndKeys(*cloud); } // else if (sc_input_type == SCInputType::SINGLE_SCAN_FEAT) { // scManager.makeAndSaveScancontextAndKeys(*thisSurfKeyFrame); // } // else if (sc_input_type == SCInputType::MULTI_SCAN_FEAT) { // pcl::PointCloud::Ptr multiKeyFrameFeatureCloud(new pcl::PointCloud()); // loopFindNearKeyframes(multiKeyFrameFeatureCloud, cloudKeyPoses6D->size() - 1, historyKeyframeSearchNum); // scManager.makeAndSaveScancontextAndKeys(*multiKeyFrameFeatureCloud); // } lastKeyframeTime = thisKeyframeTime; } void imu_callback(const sensor_msgs::ImuConstPtr& imu_odom_msg) { // Transform to Radar's Frame geometry_msgs::QuaternionStamped::Ptr imu_quat(new geometry_msgs::QuaternionStamped); imu_quat->quaternion = imu_odom_msg->orientation; Eigen::Quaterniond imu_quat_from(imu_quat->quaternion.w, imu_quat->quaternion.x, imu_quat->quaternion.y, imu_quat->quaternion.z); Eigen::Quaterniond imu_quat_deskew = imu_quat_from * extQRPY; imu_quat_deskew.normalize(); static int cnt = 0; if(cnt == 0) { double roll, pitch, yaw; tf::Quaternion orientation = tf::Quaternion(imu_quat_deskew.x(),imu_quat_deskew.y(),imu_quat_deskew.z(),imu_quat_deskew.w()); tf::quaternionMsgToTF(imu_odom_msg->orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); // Eigen::Matrix3d imu_mat_deskew = imu_quat_deskew.toRotationMatrix(); // Eigen::Vector3d eulerAngle = imu_mat_deskew.eulerAngles(0,1,2); // roll pitch yaw Eigen::AngleAxisd rollAngle(AngleAxisd(roll,Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(pitch,Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(0.0,Vector3d::UnitZ())); Eigen::Matrix3d imu_mat_final; imu_mat_final = yawAngle * pitchAngle * rollAngle; Eigen::Isometry3d isom_initial_pose; isom_initial_pose.setIdentity(); isom_initial_pose.rotate(imu_mat_final); // Set rotation initial_pose = isom_initial_pose.matrix(); ROS_INFO("Initial Position Matrix = "); std::cout << initial_pose(0,0) << ", " << initial_pose(0,1) << ", " << initial_pose(0,2) << ", " << initial_pose(0,3) << ", " << std::endl << initial_pose(1,0) << ", " << initial_pose(1,1) << ", " << initial_pose(1,2) << ", " << initial_pose(1,3) << ", " << std::endl << initial_pose(2,0) << ", " << initial_pose(2,1) << ", " << initial_pose(2,2) << ", " << initial_pose(2,3) << ", " << std::endl << initial_pose(3,0) << ", " << initial_pose(3,1) << ", " << initial_pose(3,2) << ", " << initial_pose(3,3) << ", " << std::endl << std::endl; cnt = 1; } } void imu_odom_callback(const nav_msgs::OdometryConstPtr& imu_odom_msg) { { std::lock_guard lock(keyframe_queue_mutex); imu_odom_queue.push_back(imu_odom_msg); } } geometry_msgs::Transform preIntegrationTransform(){ double lastImuOdomQT = -1; // double lastTwistQT = -1; double delta_t = 0; size_t imu_odom_end_index = 0; // The index of the last used IMU odom geometry_msgs::Transform trans_; // Transform to return // pop old IMU orientation message while (!imu_odom_queue.empty() && imu_odom_queue.front()->header.stamp.toSec() < lastKeyframeTime - delta_t){ lastImuOdomQT = imu_odom_queue.front()->header.stamp.toSec(); imu_odom_queue.pop_front(); } // pop old Twist message while (!twist_queue.empty() && twist_queue.front()->header.stamp.toSec() < lastKeyframeTime - delta_t){ // lastTwistQT = twist_queue.front()->header.stamp.toSec(); twist_queue.pop_front(); } // repropogate Eigen::Isometry3d isom_frame2frame; // Translation increment between last key frame and this IMU msg Eigen::Isometry3d isometry_rotation; // Rotation of IMU odom Eigen::Isometry3d isometry_translation; // Translation of IMU odom isom_frame2frame.setIdentity(); isometry_rotation.setIdentity(); geometry_msgs::Vector3 translation_frame2frame; // Translation between two key frames if (!imu_odom_queue.empty()) { // (Doing this because some imu_odom later than thisKeyframeTime will be recieved) for (size_t i = 0; i < imu_odom_queue.size(); ++i){ if (imu_odom_queue.at(i)->header.stamp.toSec() < thisKeyframeTime) imu_odom_end_index = i; } std::unique_ptr egovelIntegratorX; std::unique_ptr egovelIntegratorY; std::unique_ptr egovelIntegratorZ; if (use_egovel_preinteg_trans) { // integrate imu message from the beginning of this optimization size_t twist_length = twist_queue.size(); Eigen::MatrixXd q_via_x(twist_length,3),q_via_y(twist_length,3),q_via_z(twist_length,3); Eigen::VectorXd t_via_x(twist_length),t_via_y(twist_length),t_via_z(twist_length); // double dt = 1.0/12; for (size_t i=0; i < twist_length; i++){ q_via_x(i, 0) = twist_queue.at(i)->twist.linear.x; q_via_x(i, 1) = 0; q_via_x(i, 2) = 0; // acc: q(i,1)=(twist_queue.at(i+1).twist.linear.x - twist_queue.at(i).twist.linear.x) / dt; t_via_x(i) = twist_queue.at(i)->header.stamp.toSec(); q_via_y(i, 0) = twist_queue.at(i)->twist.linear.y; q_via_y(i, 1) = 0; q_via_y(i, 2) = 0; t_via_y(i) = t_via_x(i); q_via_z(i, 0) = twist_queue.at(i)->twist.linear.z; q_via_z(i, 1) = 0; q_via_z(i, 2) = 0; t_via_z(i) = t_via_x(i); } egovelIntegratorX.reset(new CubicInterpolation(q_via_x, t_via_x)); // Integrator of ego velocity at X axis egovelIntegratorY.reset(new CubicInterpolation(q_via_y, t_via_y)); // Integrator of ego velocity at Y axis egovelIntegratorZ.reset(new CubicInterpolation(q_via_z, t_via_z)); // Integrator of ego velocity at Z axis // ROS_INFO("Twist queue - Start time: %f Stop time: %f", t_via_x(0), t_via_x(t_via_x.size()-1)); } Eigen::Isometry3d isom_imu_odom_last(Eigen::Isometry3d::Identity()); Eigen::Quaterniond q_imu_odom(imu_odom_queue.front()->pose.pose.orientation.w, imu_odom_queue.front()->pose.pose.orientation.x, imu_odom_queue.front()->pose.pose.orientation.y, imu_odom_queue.front()->pose.pose.orientation.z); isom_imu_odom_last.rotate(q_imu_odom); isom_imu_odom_last.pretranslate(Eigen::Vector3d(imu_odom_queue.front()->pose.pose.position.x, imu_odom_queue.front()->pose.pose.position.y, imu_odom_queue.front()->pose.pose.position.z)); Eigen::Isometry3d isom_imu_odom_this(Eigen::Isometry3d::Identity()); q_imu_odom = Eigen::Quaterniond(imu_odom_queue.at(imu_odom_end_index)->pose.pose.orientation.w, imu_odom_queue.at(imu_odom_end_index)->pose.pose.orientation.x, imu_odom_queue.at(imu_odom_end_index)->pose.pose.orientation.y, imu_odom_queue.at(imu_odom_end_index)->pose.pose.orientation.z); isom_imu_odom_this.rotate(q_imu_odom); isom_imu_odom_this.pretranslate(Eigen::Vector3d(imu_odom_queue.at(imu_odom_end_index)->pose.pose.position.x, imu_odom_queue.at(imu_odom_end_index)->pose.pose.position.y, imu_odom_queue.at(imu_odom_end_index)->pose.pose.position.z)); Eigen::Isometry3d isom_imu_odom_btn = isom_imu_odom_last.inverse() * isom_imu_odom_this; // cout << isom_imu_odom_btn.matrix() << endl; // ROS_INFO("IMU Odom queue - Start time: %f Stop time: %f", imu_odom_queue.at(0)->header.stamp.toSec(), imu_odom_queue.at(imu_odom_end_index)->header.stamp.toSec()); if (use_egovel_preinteg_trans){ // Use Ego vel as translation integration for (size_t i = 0; i < imu_odom_end_index + 1; i++) { nav_msgs::Odometry::ConstPtr thisImu = imu_odom_queue.at(i); double imuOdomTime = thisImu->header.stamp.toSec(); isometry_translation.setIdentity(); isometry_rotation.setIdentity(); double dt = (lastImuOdomQT < 0) ? (1.0 / 200.0) :(imuOdomTime - lastImuOdomQT); Eigen::Vector3d translation_ = Eigen::Vector3d( egovelIntegratorX->getPosition(imuOdomTime), egovelIntegratorY->getPosition(imuOdomTime), egovelIntegratorZ->getPosition(imuOdomTime))* dt; isometry_translation.pretranslate(translation_); // Set translation isom_frame2frame = isom_frame2frame.pretranslate(isometry_translation.translation()); lastImuOdomQT = imuOdomTime; } } else { // Use IMU as translation integration isom_frame2frame = isom_imu_odom_btn; } translation_frame2frame.x = isom_frame2frame.translation().x(); translation_frame2frame.y = isom_frame2frame.translation().y(); translation_frame2frame.z = isom_frame2frame.translation().z(); const auto& last_imu_orien = imu_odom_queue.at(imu_odom_end_index); trans_.rotation = last_imu_orien->pose.pose.orientation; trans_.translation = translation_frame2frame; } return trans_; } void barometer_callback(const barometer_bmp388::BarometerPtr& baro_msg) { if(!enable_barometer) { return; } std::lock_guard lock(barometer_queue_mutex); barometer_queue.push_back(baro_msg); } bool flush_barometer_queue() { std::lock_guard lock(barometer_queue_mutex); if(keyframes.empty() || barometer_queue.empty()) { return false; } bool updated = false; auto barometer_cursor = barometer_queue.begin(); for(size_t i=0; i < keyframes.size(); i++) { auto keyframe = keyframes.at(i); if(keyframe->stamp > barometer_queue.back()->header.stamp) { break; } if(keyframe->stamp < (*barometer_cursor)->header.stamp || keyframe->altitude) { continue; } // find the barometer data which is closest to the keyframe_ auto closest_barometer = barometer_cursor; for(auto baro = barometer_cursor; baro != barometer_queue.end(); baro++) { auto dt = ((*closest_barometer)->header.stamp - keyframe->stamp).toSec(); auto dt2 = ((*baro)->header.stamp - keyframe->stamp).toSec(); if(std::abs(dt) < std::abs(dt2)) { break; } closest_barometer = baro; } // if the time residual between the barometer and keyframe_ is too large, skip it barometer_cursor = closest_barometer; if(0.2 < std::abs(((*closest_barometer)->header.stamp - keyframe->stamp).toSec())) { continue; } Eigen::Vector4d aftmapped_pos(keyframe->odom_scan2scan.translation().x(), keyframe->odom_scan2scan.translation().y(), (*closest_barometer)->altitude, 1.0); aftmapped_pos = initial_pose * aftmapped_pos; // std::cout << "baro position: " << aftmapped_pos(0) << ", " << aftmapped_pos(1) << ", " << aftmapped_pos(2) << std::endl; nav_msgs::Odometry initial_odom; // Initial posture initial_odom = matrix2odom(keyframe->stamp, initial_pose, mapFrame, baselinkFrame); Eigen::Vector1d z(aftmapped_pos(2)); // the first barometer data position will be the origin of the map if(!zero_alt) { zero_alt = z; } z -= (*zero_alt); keyframe->altitude = z; if (enable_barometer){ //********** G2O Edge *********** if (barometer_edge_type == 1){ g2o::OptimizableGraph::Edge* edge; Eigen::Vector1d information_matrix = Eigen::Vector1d::Identity() / barometer_edge_stddev; edge = graph_slam->add_se3_prior_z_edge(keyframe->node, z, information_matrix); graph_slam->add_robust_kernel(edge, private_nh.param("barometer_edge_robust_kernel", "NONE"), private_nh.param("barometer_edge_robust_kernel_size", 1.0)); } else if (barometer_edge_type == 2 && i != 0 && keyframes.at(i-1)->altitude.is_initialized()){ g2o::OptimizableGraph::Edge* edge; Eigen::Vector1d information_matrix = Eigen::Vector1d::Identity() / barometer_edge_stddev; Eigen::Vector1d relative_z(keyframe->altitude.value() - keyframes.at(i-1)->altitude.value()); edge = graph_slam->add_se3_z_edge(keyframe->node, keyframes.at(i-1)->node, relative_z, information_matrix); graph_slam->add_robust_kernel(edge, private_nh.param("barometer_edge_robust_kernel", "NONE"), private_nh.param("barometer_edge_robust_kernel_size", 1.0)); } } updated = true; } auto remove_loc = std::upper_bound(barometer_queue.begin(), barometer_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const barometer_bmp388::BarometerConstPtr& baropoint) { return stamp < baropoint->header.stamp; }); barometer_queue.erase(barometer_queue.begin(), remove_loc); return updated; } /** * @brief this method adds all the keyframes_ in #keyframe_queue to the pose graph (odometry edges) * @return if true, at least one keyframe_ was added to the pose graph */ bool flush_keyframe_queue() { std::lock_guard lock(keyframe_queue_mutex); if(keyframe_queue.empty()) { return false; } trans_odom2map_mutex.lock(); Eigen::Isometry3d odom2map(trans_odom2map.cast()); trans_odom2map_mutex.unlock(); int num_processed = 0; // ********** Select number of keyframess to be optimized ********** for(int i = 0; i < std::min(keyframe_queue.size(), max_keyframes_per_update); i++) { num_processed = i; const auto& keyframe = keyframe_queue[i]; // new_keyframess will be tested later for loop closure new_keyframes.push_back(keyframe); // add pose node Eigen::Isometry3d odom = odom2map * keyframe->odom_scan2scan; // ********** Vertex of keyframess is contructed here *********** keyframe->node = graph_slam->add_se3_node(odom); keyframe_hash[keyframe->stamp] = keyframe; // fix the first node if(keyframes.empty() && new_keyframes.size() == 1) { if(private_nh.param("fix_first_node", false)) { Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); std::stringstream sst(private_nh.param("fix_first_node_stddev", "1 1 1 1 1 1")); for(int i = 0; i < 6; i++) { double stddev = 1.0; sst >> stddev; inf(i, i) = 1.0 / stddev; } anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity()); anchor_node->setFixed(true); anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf); } } if(i == 0 && keyframes.empty()) { continue; } /***** Scan-to-Scan Add edge to between consecutive keyframes *****/ const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1]; // relative pose between odom of previous frame and this frame R2=R12*R1 => R12 = inv(R2) * R1 Eigen::Isometry3d relative_pose = keyframe->odom_scan2scan.inverse() * prev_keyframe->odom_scan2scan; // calculate fitness score as information Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose); auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information); // cout << information << endl; graph_slam->add_robust_kernel(edge, private_nh.param("odometry_edge_robust_kernel", "NONE"), private_nh.param("odometry_edge_robust_kernel_size", 1.0)); if (enable_preintegration){ // Add Preintegration edge geometry_msgs::Transform relative_trans = keyframe->trans_integrated; g2o::SE3Quat relative_se3quat ( Eigen::Quaterniond(relative_trans.rotation.w, relative_trans.rotation.x, relative_trans.rotation.y, relative_trans.rotation.z), Eigen::Vector3d(relative_trans.translation.x, relative_trans.translation.y, relative_trans.translation.z)); Eigen::Isometry3d relative_isometry = transform2isometry(relative_trans); Eigen::MatrixXd information_integ = Eigen::MatrixXd::Identity(6, 6); information_integ << 1.0 / preinteg_trans_stddev, 0, 0, 0, 0, 0, 0, 1.0 / preinteg_trans_stddev, 0, 0, 0, 0, 0, 0, 1.0 / preinteg_trans_stddev, 0, 0, 0, 0, 0, 0, 1.0 / preinteg_orient_stddev, 0, 0, 0, 0, 0, 0, 1.0 / preinteg_orient_stddev, 0, 0, 0, 0, 0, 0, 1.0 / preinteg_orient_stddev; auto edge_integ = graph_slam->add_se3_edge(prev_keyframe->node, keyframe->node, relative_isometry, information_integ); graph_slam->add_robust_kernel(edge_integ, private_nh.param("integ_edge_robust_kernel", "NONE"), private_nh.param("integ_edge_robust_kernel_size", 1.0)); } } std_msgs::Header read_until; read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0); read_until.frame_id = points_topic; read_until_pub.publish(read_until); read_until.frame_id = "/filtered_points"; read_until_pub.publish(read_until); keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1); return true; } /** * @brief Back-end Optimization. This methods adds all the data in the queues to the pose graph, and then optimizes the pose graph * @param event */ void optimization_timer_callback(const ros::WallTimerEvent& event) { std::lock_guard lock(main_thread_mutex); // add keyframes_ and floor coeffs in the queues to the pose graph bool keyframe_updated = flush_keyframe_queue(); if(!keyframe_updated) { std_msgs::Header read_until; read_until.stamp = ros::Time::now() + ros::Duration(30, 0); read_until.frame_id = points_topic; read_until_pub.publish(read_until); read_until.frame_id = "/filtered_points"; read_until_pub.publish(read_until); } if(!keyframe_updated & !flush_gps_queue() & !flush_barometer_queue()) { return; } // loop detection if(private_nh.param("enable_loop_closure", false)){ std::vector loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam); } // Copy "new_keyframes_" to vector "keyframes_", "new_keyframes_" was used for loop detaction std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes)); new_keyframes.clear(); if(private_nh.param("enable_loop_closure", false)) addLoopFactor(); // move the first node / position to the current estimate of the first node pose // so the first node moves freely while trying to stay around the origin if(anchor_node && private_nh.param("fix_first_node_adaptive", true)) { Eigen::Isometry3d anchor_target = static_cast(anchor_edge->vertices()[1])->estimate(); anchor_node->setEstimate(anchor_target); } // optimize the pose graph int num_iterations = private_nh.param("g2o_solver_num_iterations", 1024); clock_t start_ms = clock(); graph_slam->optimize(num_iterations); clock_t end_ms = clock(); double time_used = double(end_ms - start_ms) / CLOCKS_PER_SEC; opt_time.push_back(time_used); //********** publish tf ********** const auto& keyframe = keyframes.back(); // RadarOdom_to_base = map_to_base * map_to_RadarOdom^(-1) Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom_scan2scan.inverse(); Eigen::Isometry3d map2base_trans = keyframe->node->estimate(); trans_odom2map_mutex.lock(); trans_odom2map = trans.matrix(); // map2base_incremental = map2base_last^(-1) * map2base_this trans_aftmapped_incremental = trans_aftmapped.inverse() * map2base_trans; trans_aftmapped = map2base_trans; trans_odom2map_mutex.unlock(); std::vector snapshot(keyframes.size()); std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const KeyFrame::Ptr& k) { return std::make_shared(k); }); keyframes_snapshot_mutex.lock(); keyframes_snapshot.swap(snapshot); keyframes_snapshot_mutex.unlock(); graph_updated = true; // Publish After-Mapped Odometry nav_msgs::Odometry aft = isometry2odom(keyframe->stamp, trans_aftmapped, mapFrame, odometryFrame); aftmapped_odom_pub.publish(aft); // Publish After-Mapped Odometry Incrementation nav_msgs::Odometry aft_incre = isometry2odom(keyframe->stamp, trans_aftmapped_incremental, mapFrame, odometryFrame); aftmapped_odom_incremenral_pub.publish(aft_incre); // Publish /odom to /base_link if(odom2base_pub.getNumSubscribers()) { // Returns the number of subscribers that are currently connected to this Publisher geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix(), mapFrame, odometryFrame); odom2base_pub.publish(ts); } if(markers_pub.getNumSubscribers()) { auto markers = create_marker_array(ros::Time::now()); markers_pub.publish(markers); } } void addLoopFactor() { if (loop_detector->loopIndexQueue.empty()) return; for (int i = 0; i < (int)loop_detector->loopIndexQueue.size(); ++i){ int indexFrom = loop_detector->loopIndexQueue[i].first; int indexTo = loop_detector->loopIndexQueue[i].second; Eigen::Isometry3d poseBetween = loop_detector->loopPoseQueue[i]; Eigen::MatrixXd information_matrix = loop_detector->loopInfoQueue[i]; auto edge = graph_slam->add_se3_edge(keyframes[indexFrom]->node, keyframes[indexTo]->node, poseBetween, information_matrix); graph_slam->add_robust_kernel(edge, private_nh.param("loop_closure_edge_robust_kernel", "NONE"), private_nh.param("loop_closure_edge_robust_kernel_size", 1.0)); } // loopIndexQueue.clear(); // loopPoseQueue.clear(); // loopInfoQueue.clear(); // aLoopIsClosed = true; } /** * @brief generate map point cloud and publish it * @param event */ void map_points_publish_timer_callback(const ros::WallTimerEvent& event) { if(!map_points_pub.getNumSubscribers() || !graph_updated) { return; } std::vector snapshot; keyframes_snapshot_mutex.lock(); snapshot = keyframes_snapshot; keyframes_snapshot_mutex.unlock(); auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution); if(!cloud) { return; } cloud->header.frame_id = mapFrame; cloud->header.stamp = snapshot.back()->cloud->header.stamp; sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2()); pcl::toROSMsg(*cloud, *cloud_msg); map_points_pub.publish(cloud_msg); } /** * @brief create visualization marker * @param stamp * @return */ visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const { visualization_msgs::MarkerArray markers; if (show_sphere) markers.markers.resize(5); else markers.markers.resize(4); // loop edges visualization_msgs::Marker& loop_marker = markers.markers[0]; loop_marker.header.frame_id = "map"; loop_marker.header.stamp = stamp; loop_marker.action = visualization_msgs::Marker::ADD; loop_marker.type = visualization_msgs::Marker::LINE_LIST; loop_marker.ns = "loop_edges"; loop_marker.id = 1; loop_marker.pose.orientation.w = 1; loop_marker.scale.x = 0.1; loop_marker.scale.y = 0.1; loop_marker.scale.z = 0.1; loop_marker.color.r = 0.9; loop_marker.color.g = 0.9; loop_marker.color.b = 0; loop_marker.color.a = 1; for (auto it = loop_detector->loopIndexContainer.begin(); it != loop_detector->loopIndexContainer.end(); ++it) { int key_cur = it->first; int key_pre = it->second; geometry_msgs::Point p; Eigen::Vector3d pos = keyframes[key_cur]->node->estimate().translation(); p.x = pos.x(); p.y = pos.y(); p.z = pos.z(); loop_marker.points.push_back(p); pos = keyframes[key_pre]->node->estimate().translation(); p.x = pos.x(); p.y = pos.y(); p.z = pos.z(); loop_marker.points.push_back(p); } // node markers visualization_msgs::Marker& traj_marker = markers.markers[1]; traj_marker.header.frame_id = "map"; traj_marker.header.stamp = stamp; traj_marker.ns = "nodes"; traj_marker.id = 0; traj_marker.type = visualization_msgs::Marker::SPHERE_LIST; traj_marker.pose.orientation.w = 1.0; traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 0.3; visualization_msgs::Marker& imu_marker = markers.markers[2]; imu_marker.header = traj_marker.header; imu_marker.ns = "imu"; imu_marker.id = 1; imu_marker.type = visualization_msgs::Marker::SPHERE_LIST; imu_marker.pose.orientation.w = 1.0; imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75; traj_marker.points.resize(keyframes.size()); traj_marker.colors.resize(keyframes.size()); for(size_t i = 0; i < keyframes.size(); i++) { Eigen::Vector3d pos = keyframes[i]->node->estimate().translation(); traj_marker.points[i].x = pos.x(); traj_marker.points[i].y = pos.y(); traj_marker.points[i].z = pos.z(); double p = static_cast(i) / keyframes.size(); traj_marker.colors[i].r = 0.0;//1.0 - p; traj_marker.colors[i].g = 1.0;//p; traj_marker.colors[i].b = 0.0; traj_marker.colors[i].a = 1.0; if(keyframes[i]->acceleration) { Eigen::Vector3d pos = keyframes[i]->node->estimate().translation(); geometry_msgs::Point point; point.x = pos.x(); point.y = pos.y(); point.z = pos.z(); std_msgs::ColorRGBA color; color.r = 0.0; color.g = 0.0; color.b = 1.0; color.a = 0.1; imu_marker.points.push_back(point); imu_marker.colors.push_back(color); } } // edge markers visualization_msgs::Marker& edge_marker = markers.markers[3]; edge_marker.header.frame_id = "map"; edge_marker.header.stamp = stamp; edge_marker.ns = "edges"; edge_marker.id = 2; edge_marker.type = visualization_msgs::Marker::LINE_LIST; edge_marker.pose.orientation.w = 1.0; edge_marker.scale.x = 0.05; edge_marker.points.resize(graph_slam->graph->edges().size() * 2); edge_marker.colors.resize(graph_slam->graph->edges().size() * 2); auto edge_itr = graph_slam->graph->edges().begin(); for(int i = 0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) { g2o::HyperGraph::Edge* edge = *edge_itr; g2o::EdgeSE3* edge_se3 = dynamic_cast(edge); if(edge_se3) { g2o::VertexSE3* v1 = dynamic_cast(edge_se3->vertices()[0]); g2o::VertexSE3* v2 = dynamic_cast(edge_se3->vertices()[1]); Eigen::Vector3d pt1 = v1->estimate().translation(); Eigen::Vector3d pt2 = v2->estimate().translation(); edge_marker.points[i * 2].x = pt1.x(); edge_marker.points[i * 2].y = pt1.y(); edge_marker.points[i * 2].z = pt1.z(); edge_marker.points[i * 2 + 1].x = pt2.x(); edge_marker.points[i * 2 + 1].y = pt2.y(); edge_marker.points[i * 2 + 1].z = pt2.z(); double p1 = static_cast(v1->id()) / graph_slam->graph->vertices().size(); double p2 = static_cast(v2->id()) / graph_slam->graph->vertices().size(); edge_marker.colors[i * 2].r = 0.0;//1.0 - p1; edge_marker.colors[i * 2].g = 1.0;//p1; edge_marker.colors[i * 2].a = 1.0; edge_marker.colors[i * 2 + 1].r = 0.0;//1.0 - p2; edge_marker.colors[i * 2 + 1].g = 1.0;//p2; edge_marker.colors[i * 2 + 1].a = 1.0; if(std::abs(v1->id() - v2->id()) > 2) { // edge_marker.points[i * 2].z += 0.5; // edge_marker.points[i * 2 + 1].z += 0.5; edge_marker.colors[i * 2].r = 0.9; edge_marker.colors[i * 2].g = 0.9; edge_marker.colors[i * 2].b = 0.0; edge_marker.colors[i * 2 + 1].r = 0.9; edge_marker.colors[i * 2 + 1].g = 0.9; edge_marker.colors[i * 2 + 1].b = 0.0; edge_marker.colors[i * 2].a = 0.0; edge_marker.colors[i * 2 + 1].a += 0.0; } continue; } g2o::EdgeSE3Plane* edge_plane = dynamic_cast(edge); if(edge_plane) { g2o::VertexSE3* v1 = dynamic_cast(edge_plane->vertices()[0]); Eigen::Vector3d pt1 = v1->estimate().translation(); Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0); edge_marker.points[i * 2].x = pt1.x(); edge_marker.points[i * 2].y = pt1.y(); edge_marker.points[i * 2].z = pt1.z(); edge_marker.points[i * 2 + 1].x = pt2.x(); edge_marker.points[i * 2 + 1].y = pt2.y(); edge_marker.points[i * 2 + 1].z = pt2.z(); edge_marker.colors[i * 2].b = 1.0; edge_marker.colors[i * 2].a = 1.0; edge_marker.colors[i * 2 + 1].b = 1.0; edge_marker.colors[i * 2 + 1].a = 1.0; continue; } g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast(edge); if(edge_priori_xy) { g2o::VertexSE3* v1 = dynamic_cast(edge_priori_xy->vertices()[0]); Eigen::Vector3d pt1 = v1->estimate().translation(); Eigen::Vector3d pt2 = Eigen::Vector3d::Zero(); pt2.head<2>() = edge_priori_xy->measurement(); edge_marker.points[i * 2].x = pt1.x(); edge_marker.points[i * 2].y = pt1.y(); edge_marker.points[i * 2].z = pt1.z() + 0.5; edge_marker.points[i * 2 + 1].x = pt2.x(); edge_marker.points[i * 2 + 1].y = pt2.y(); edge_marker.points[i * 2 + 1].z = pt2.z() + 0.5; edge_marker.colors[i * 2].r = 1.0; edge_marker.colors[i * 2].a = 1.0; edge_marker.colors[i * 2 + 1].r = 1.0; edge_marker.colors[i * 2 + 1].a = 1.0; continue; } g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast(edge); if(edge_priori_xyz) { g2o::VertexSE3* v1 = dynamic_cast(edge_priori_xyz->vertices()[0]); Eigen::Vector3d pt1 = v1->estimate().translation(); Eigen::Vector3d pt2 = edge_priori_xyz->measurement(); edge_marker.points[i * 2].x = pt1.x(); edge_marker.points[i * 2].y = pt1.y(); edge_marker.points[i * 2].z = pt1.z() + 0.5; edge_marker.points[i * 2 + 1].x = pt2.x(); edge_marker.points[i * 2 + 1].y = pt2.y(); edge_marker.points[i * 2 + 1].z = pt2.z(); edge_marker.colors[i * 2].r = 1.0; edge_marker.colors[i * 2].a = 1.0; edge_marker.colors[i * 2 + 1].r = 1.0; edge_marker.colors[i * 2 + 1].a = 1.0; continue; } } if (show_sphere) { // sphere visualization_msgs::Marker& sphere_marker = markers.markers[4]; sphere_marker.header.frame_id = "map"; sphere_marker.header.stamp = stamp; sphere_marker.ns = "loop_close_radius"; sphere_marker.id = 3; sphere_marker.type = visualization_msgs::Marker::SPHERE; if(!keyframes.empty()) { Eigen::Vector3d pos = keyframes.back()->node->estimate().translation(); sphere_marker.pose.position.x = pos.x(); sphere_marker.pose.position.y = pos.y(); sphere_marker.pose.position.z = pos.z(); } sphere_marker.pose.orientation.w = 1.0; sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0; sphere_marker.color.r = 1.0; sphere_marker.color.a = 0.3; } return markers; } /** * @brief dump all data to the current directory * @param req * @param res * @return */ bool dump_service(radar_graph_slam::DumpGraphRequest& req, radar_graph_slam::DumpGraphResponse& res) { std::lock_guard lock(main_thread_mutex); std::string directory = req.destination; if(directory.empty()) { std::array buffer; buffer.fill(0); time_t rawtime; time(&rawtime); const auto timeinfo = localtime(&rawtime); strftime(buffer.data(), sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo); } if(!boost::filesystem::is_directory(directory)) { boost::filesystem::create_directory(directory); } std::cout << "all data dumped to:" << directory << std::endl; graph_slam->save(directory + "/graph.g2o"); for(size_t i = 0; i < keyframes.size(); i++) { std::stringstream sst; sst << boost::format("%s/%06d") % directory % i; keyframes[i]->save(sst.str()); } if(zero_utm) { std::ofstream zero_utm_ofs(directory + "/zero_utm"); zero_utm_ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl; } std::ofstream ofs(directory + "/special_nodes.csv"); ofs << "anchor_node " << (anchor_node == nullptr ? -1 : anchor_node->id()) << std::endl; ofs << "anchor_edge " << (anchor_edge == nullptr ? -1 : anchor_edge->id()) << std::endl; ofs << "floor_node " << (floor_plane_node == nullptr ? -1 : floor_plane_node->id()) << std::endl; res.success = true; return true; } /** * @brief save map data as pcd * @param req * @param res * @return */ bool save_map_service(radar_graph_slam::SaveMapRequest& req, radar_graph_slam::SaveMapResponse& res) { std::vector snapshot; keyframes_snapshot_mutex.lock(); snapshot = keyframes_snapshot; keyframes_snapshot_mutex.unlock(); auto cloud = map_cloud_generator->generate(snapshot, req.resolution); if(!cloud) { res.success = false; return true; } if(zero_utm && req.utm) { for(auto& pt : cloud->points) { pt.getVector3fMap() += (*zero_utm).cast(); } } cloud->header.frame_id = mapFrame; cloud->header.stamp = snapshot.back()->cloud->header.stamp; if(zero_utm) { std::ofstream ofs(req.destination + ".utm"); ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl; } int ret = pcl::io::savePCDFileBinary(req.destination, *cloud); res.success = ret == 0; return true; } void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) { GPRMC grmc = nmea_parser->parse(nmea_msg->sentence); if(grmc.status != 'A') return; geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped()); gps_msg->header = nmea_msg->header; gps_msg->position.latitude = grmc.latitude; gps_msg->position.longitude = grmc.longitude; gps_msg->position.altitude = NAN; gps_callback(gps_msg); } void navsat_callback(const sensor_msgs::NavSatFixConstPtr& navsat_msg) { sensor_msgs::NavSatFix gps_msg; gps_msg.header = navsat_msg->header; gps_msg.latitude = navsat_msg->latitude; gps_msg.longitude = navsat_msg->longitude; gps_msg.altitude = navsat_msg->altitude; gps_msg.position_covariance = navsat_msg->position_covariance; gps_msg.position_covariance_type = navsat_msg->position_covariance_type; gps_msg.status = navsat_msg->status; gps_navsat_queue.push_back(gps_msg); } /** * @brief received gps data is added to #gps_queue_ * @param gps_msg */ void gps_callback(const geographic_msgs::GeoPointStampedPtr& gps_msg) { std::lock_guard lock(gps_queue_mutex); gps_msg->header.stamp += ros::Duration(gps_time_offset); gps_geopoint_queue.push_back(gps_msg); } /** * @brief * @return */ bool flush_gps_queue() { std::lock_guard lock(gps_queue_mutex); if(keyframes.empty() || gps_navsat_queue.empty()) { return false; } bool updated = false; auto gps_cursor = gps_navsat_queue.begin(); for(auto& keyframe : keyframes) { if (keyframe->index - last_gps_edge_index < gps_edge_intervals) continue; if (keyframe->stamp > gps_navsat_queue.back().header.stamp) { break; } if (keyframe->stamp < (*gps_cursor).header.stamp || keyframe->utm_coord) { continue; } // find the gps data which is closest to the keyframe_ auto closest_gps = gps_cursor; for(auto gps = gps_cursor; gps != gps_navsat_queue.end(); gps++) { auto dt = ((*closest_gps).header.stamp - keyframe->stamp).toSec(); auto dt2 = ((*gps).header.stamp - keyframe->stamp).toSec(); if(std::abs(dt) < std::abs(dt2)) { break; } closest_gps = gps; } // if the time residual between the gps and keyframe_ is too large, skip it gps_cursor = closest_gps; if(0.2 < std::abs(((*closest_gps).header.stamp - keyframe->stamp).toSec())) { continue; } // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate geographic_msgs::GeoPoint gps_geopoint; gps_geopoint.altitude = (*closest_gps).altitude; gps_geopoint.latitude = (*closest_gps).latitude; gps_geopoint.longitude = (*closest_gps).longitude; geodesy::UTMPoint utm; geodesy::fromMsg(gps_geopoint, utm); Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude); double cov_x = (*closest_gps).position_covariance.at(0); double cov_y = (*closest_gps).position_covariance.at(4); double cov_z = (*closest_gps).position_covariance.at(8); if (cov_x > max_gps_edge_stddev_xy || cov_y > max_gps_edge_stddev_xy || cov_z > max_gps_edge_stddev_z) continue; // the first gps data position will be the origin of the map // if(!zero_utm) { // zero_utm = xyz; // } // xyz -= (*zero_utm); keyframe->utm_coord = xyz; Eigen::Vector3d world_coordinate = (utm_to_world * Eigen::Vector4d(utm.easting, utm.northing, utm.altitude, 1)).head(3); Eigen::Vector3d trans_err = keyframe->node->estimate().translation() - world_coordinate; if (trans_err.norm() < 5.0) continue; //********** G2O Edge *********** g2o::OptimizableGraph::Edge* edge; if(std::isnan(world_coordinate.z())) { Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / cov_x; edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, world_coordinate.head<2>(), information_matrix); } else { Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity(); information_matrix(0, 0) /= cov_x; information_matrix(1, 1) /= cov_y; information_matrix(2, 2) /= cov_z; edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, world_coordinate, information_matrix); } graph_slam->add_robust_kernel(edge, private_nh.param("gps_edge_robust_kernel", "NONE"), private_nh.param("gps_edge_robust_kernel_size", 1.0)); last_gps_edge_index = keyframe->index; updated = true; } auto remove_loc = std::upper_bound(gps_navsat_queue.begin(), gps_navsat_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const sensor_msgs::NavSatFix& geopoint) { return stamp < geopoint.header.stamp; }); gps_navsat_queue.erase(gps_navsat_queue.begin(), remove_loc); return updated; } void command_callback(const std_msgs::String& str_msg) { if (str_msg.data == "output_aftmapped") { ofstream fout; fout.open("/home/zhuge/stamped_pose_graph_estimate.txt", ios::out); fout << "# timestamp tx ty tz qx qy qz qw" << endl; fout.setf(ios::fixed, ios::floatfield); // fixed mode，float fout.precision(8); // Set precision 8 for(size_t i = 0; i < keyframes.size(); i++) { Eigen::Vector3d pos_ = keyframes[i]->node->estimate().translation(); Eigen::Matrix3d rot_ = keyframes[i]->node->estimate().rotation(); Eigen::Quaterniond quat_(rot_); double timestamp = keyframes[i]->stamp.toSec(); double tx = pos_(0), ty = pos_(1), tz = pos_(2); double qx = quat_.x(), qy = quat_.y(), qz = quat_.z(), qw = quat_.w(); fout << timestamp << " " << tx << " " << ty << " " << tz << " " << qx << " " << qy << " " << qz << " " << qw << endl; } fout.close(); ROS_INFO("Optimized edges have been output!"); } else if (str_msg.data == "time") { if (loop_detector->pf_time.size() > 0) { std::sort(loop_detector->pf_time.begin(), loop_detector->pf_time.end()); double median = loop_detector->pf_time.at(floor((double)loop_detector->pf_time.size() / 2)); cout << "Pre-filtering Matching time cost (median): " << median << endl; } if (loop_detector->sc_time.size() > 0) { std::sort(loop_detector->sc_time.begin(), loop_detector->sc_time.end()); double median = loop_detector->sc_time.at(floor((double)loop_detector->sc_time.size() / 2)); cout << "Scan Context time cost (median): " << median << endl; } if (loop_detector->oc_time.size() > 0) { std::sort(loop_detector->oc_time.begin(), loop_detector->oc_time.end()); double median = loop_detector->oc_time.at(floor((double)loop_detector->oc_time.size() / 2)); cout << "Odometry Check time cost (median): " << median << endl; } if (opt_time.size() > 0) { std::sort(opt_time.begin(), opt_time.end()); double median = opt_time.at(floor((double)opt_time.size() / 2)); cout << "Optimization time cost (median): " << median << endl; } } } private: // ROS ros::NodeHandle nh; ros::NodeHandle mt_nh; ros::NodeHandle private_nh; ros::WallTimer optimization_timer; ros::WallTimer map_publish_timer; std::unique_ptr> odom_sub; std::unique_ptr> cloud_sub; std::unique_ptr> sync; ros::Subscriber barometer_sub; ros::Subscriber gps_sub; ros::Subscriber nmea_sub; ros::Subscriber navsat_sub; ros::Subscriber imu_odom_sub; ros::Subscriber imu_sub; ros::Subscriber command_sub; ros::Publisher imu_odom_pub; ros::Publisher markers_pub; std::mutex trans_odom2map_mutex; Eigen::Matrix4d trans_odom2map; // keyframe->node->estimate() * keyframe->odom.inverse(); Eigen::Isometry3d trans_aftmapped; // Odometry from /map to /base_link Eigen::Isometry3d trans_aftmapped_incremental; ros::Publisher odom2base_pub; ros::Publisher aftmapped_odom_pub; ros::Publisher aftmapped_odom_incremenral_pub; ros::Publisher odom_frame2frame_pub; std::string points_topic; ros::Publisher read_until_pub; ros::Publisher map_points_pub; tf::TransformListener tf_listener; tf::TransformBroadcaster map2base_broadcaster; // odom_frame => base_frame ros::ServiceServer dump_service_server; ros::ServiceServer save_map_service_server; // keyframe queue std::mutex keyframe_queue_mutex; std::deque keyframe_queue; std::deque twist_queue; std::deque imu_odom_queue; double thisKeyframeTime; double lastKeyframeTime; size_t keyframe_index = 0; // IMU / Ego Velocity Integration bool enable_preintegration; double preinteg_orient_stddev; double preinteg_trans_stddev; bool enable_imu_orientation; bool use_egovel_preinteg_trans; Eigen::Matrix4d initial_pose; // barometer queue bool enable_barometer; int barometer_edge_type; double barometer_edge_stddev; boost::optional zero_alt; std::mutex barometer_queue_mutex; std::deque barometer_queue; // gps queue int gps_edge_intervals; int last_gps_edge_index; double gps_time_offset; double gps_edge_stddev_xy; double gps_edge_stddev_z; double max_gps_edge_stddev_xy; double max_gps_edge_stddev_z; boost::optional zero_utm; std::mutex gps_queue_mutex; std::deque gps_geopoint_queue; std::deque gps_navsat_queue; Eigen::Matrix4d utm_to_world; std::string dataset_name; // Marker coefficients bool show_sphere; // for map cloud generation std::atomic_bool graph_updated; double map_cloud_resolution; std::mutex keyframes_snapshot_mutex; std::vector keyframes_snapshot; std::unique_ptr map_cloud_generator; // graph slam // all the below members must be accessed after locking main_thread_mutex std::mutex main_thread_mutex; int max_keyframes_per_update; // Used for Loop Closure detection source, // pushed form keyframe_queue at "flush_keyframe_queue()", // inserted to "keyframes_" before optimization std::deque new_keyframes; // Previous keyframes_ std::vector keyframes; std::unordered_map keyframe_hash; g2o::VertexSE3* anchor_node; g2o::EdgeSE3* anchor_edge; g2o::VertexPlane* floor_plane_node; std::unique_ptr graph_slam; std::unique_ptr loop_detector; std::unique_ptr keyframe_updater; std::unique_ptr nmea_parser; std::unique_ptr inf_calclator; // Registration Method pcl::Registration::Ptr registration; pcl::KdTreeFLANN::Ptr kdtreeHistoryKeyPoses; std::vector opt_time; }; } // namespace radar_graph_slam PLUGINLIB_EXPORT_CLASS(radar_graph_slam::RadarGraphSlamNodelet, nodelet::Nodelet) ================================================ FILE: apps/scan_matching_odometry_nodelet.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "utility_radar.h" using namespace std; namespace radar_graph_slam { class ScanMatchingOdometryNodelet : public nodelet::Nodelet, public ParamServer { public: typedef pcl::PointXYZI PointT; typedef message_filters::sync_policies::ApproximateTime ApproxSyncPolicy; // typedef message_filters::sync_policies::ApproximateTime ApproxSyncPolicy2; EIGEN_MAKE_ALIGNED_OPERATOR_NEW ScanMatchingOdometryNodelet() {} virtual ~ScanMatchingOdometryNodelet() {} virtual void onInit() { NODELET_DEBUG("initializing scan_matching_odometry_nodelet..."); nh = getNodeHandle(); mt_nh = getMTNodeHandle(); private_nh = getPrivateNodeHandle(); initialize_params(); // this if(private_nh.param("enable_imu_frontend", false)) { msf_pose_sub = nh.subscribe("/msf_core/pose", 1, boost::bind(&ScanMatchingOdometryNodelet::msf_pose_callback, this, _1, false)); msf_pose_after_update_sub = nh.subscribe("/msf_core/pose_after_update", 1, boost::bind(&ScanMatchingOdometryNodelet::msf_pose_callback, this, _1, true)); } //******** Subscribers ********** ego_vel_sub.reset(new message_filters::Subscriber(mt_nh, "/eagle_data/twist", 256)); points_sub.reset(new message_filters::Subscriber(mt_nh, "/filtered_points", 32)); sync.reset(new message_filters::Synchronizer(ApproxSyncPolicy(32), *ego_vel_sub, *points_sub)); sync->registerCallback(boost::bind(&ScanMatchingOdometryNodelet::pointcloud_callback, this, _1, _2)); imu_sub = nh.subscribe("/imu", 1024, &ScanMatchingOdometryNodelet::imu_callback, this); command_sub = nh.subscribe("/command", 10, &ScanMatchingOdometryNodelet::command_callback, this); //******** Publishers ********** read_until_pub = nh.advertise("/scan_matching_odometry/read_until", 32); // Odometry of Radar scan-matching_ odom_pub = nh.advertise(odomTopic, 32); // Transformation of Radar scan-matching_ trans_pub = nh.advertise("/scan_matching_odometry/transform", 32); status_pub = private_nh.advertise("/scan_matching_odometry/status", 8); aligned_points_pub = nh.advertise("/aligned_points", 32); submap_pub = nh.advertise("/radar_graph_slam/submap", 2); } private: /** * @brief initialize parameters */ void initialize_params() { auto& pnh = private_nh; points_topic = pnh.param("points_topic", "/radar_enhanced_pcl"); use_ego_vel = pnh.param("use_ego_vel", false); // The minimum tranlational distance and rotation angle between keyframes_. // If this value is zero, frames are always compared with the previous frame keyframe_delta_trans = pnh.param("keyframe_delta_trans", 0.25); keyframe_delta_angle = pnh.param("keyframe_delta_angle", 0.15); keyframe_delta_time = pnh.param("keyframe_delta_time", 1.0); // Registration validation by thresholding enable_transform_thresholding = pnh.param("enable_transform_thresholding", false); enable_imu_thresholding = pnh.param("enable_imu_thresholding", false); max_acceptable_trans = pnh.param("max_acceptable_trans", 1.0); max_acceptable_angle = pnh.param("max_acceptable_angle", 1.0); max_diff_trans = pnh.param("max_diff_trans", 1.0); max_diff_angle = pnh.param("max_diff_angle", 1.0); max_egovel_cum = pnh.param("max_egovel_cum", 1.0); map_cloud_resolution = pnh.param("map_cloud_resolution", 0.05); keyframe_updater.reset(new KeyframeUpdater(pnh)); enable_scan_to_map = pnh.param("enable_scan_to_map", false); max_submap_frames = pnh.param("max_submap_frames", 5); enable_imu_fusion = private_nh.param("enable_imu_fusion", false); imu_debug_out = private_nh.param("imu_debug_out", false); cout << "enable_imu_fusion = " << enable_imu_fusion << endl; imu_fusion_ratio = private_nh.param("imu_fusion_ratio", 0.1); // graph_slam.reset(new GraphSLAM(pnh.param("g2o_solver_type", "lm_var"))); // select a downsample method (VOXELGRID, APPROX_VOXELGRID, NONE) std::string downsample_method = pnh.param("downsample_method", "VOXELGRID"); double downsample_resolution = pnh.param("downsample_resolution", 0.1); if(downsample_method == "VOXELGRID") { std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; auto voxelgrid = new pcl::VoxelGrid(); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter.reset(voxelgrid); } else if(downsample_method == "APPROX_VOXELGRID") { std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid()); approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter = approx_voxelgrid; } else { if(downsample_method != "NONE") { std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl; std::cerr << " : use passthrough filter" << std::endl; } std::cout << "downsample: NONE" << std::endl; pcl::PassThrough::Ptr passthrough(new pcl::PassThrough()); downsample_filter = passthrough; } registration_s2s = select_registration_method(pnh); registration_s2m = select_registration_method(pnh); } void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) { Eigen::Quaterniond imu_quat_from(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z); Eigen::Quaterniond imu_quat_deskew = imu_quat_from * extQRPY; imu_quat_deskew.normalize(); double roll, pitch, yaw; // tf::quaternionMsgToTF(imu_odom_msg->orientation, orientation); tf::Quaternion orientation = tf::Quaternion(imu_quat_deskew.x(),imu_quat_deskew.y(),imu_quat_deskew.z(),imu_quat_deskew.w()); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); imuPointerLast = (imuPointerLast + 1) % imuQueLength; imuTime[imuPointerLast] = imu_msg->header.stamp.toSec(); imuRoll[imuPointerLast] = roll; imuPitch[imuPointerLast] = pitch; // cout << "get imu rp: " << roll << " " << pitch << endl; sensor_msgs::ImuPtr imu(new sensor_msgs::Imu); imu->header = imu_msg->header; imu->angular_velocity = imu_msg->angular_velocity; imu->linear_acceleration = imu_msg->linear_acceleration; imu->angular_velocity_covariance = imu_msg->angular_velocity_covariance; imu->linear_acceleration_covariance, imu_msg->linear_acceleration_covariance; imu->orientation_covariance = imu_msg->orientation_covariance; imu->orientation.w=imu_quat_deskew.w(); imu->orientation.x = imu_quat_deskew.x(); imu->orientation.y = imu_quat_deskew.y(); imu->orientation.z = imu_quat_deskew.z(); { std::lock_guard lock(imu_queue_mutex); imu_queue.push_back(imu); } static int cnt = 0; if(cnt == 0) { geometry_msgs::Quaternion imuQuat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, 0); global_orient_matrix = Eigen::Quaterniond(imuQuat.w, imuQuat.x, imuQuat.y, imuQuat.z).toRotationMatrix(); ROS_INFO_STREAM("Initial IMU euler angles (RPY): " << RAD2DEG(roll) << ", " << RAD2DEG(pitch) << ", " << RAD2DEG(yaw)); cnt = 1; } } bool flush_imu_queue() { std::lock_guard lock(imu_queue_mutex); if(keyframes.empty() || imu_queue.empty()) { return false; } bool updated = false; auto imu_cursor = imu_queue.begin(); for(size_t i=0; i < keyframes.size(); i++) { auto keyframe = keyframes.at(i); if(keyframe->stamp < (*imu_cursor)->header.stamp) { continue; } if(keyframe->stamp > imu_queue.back()->header.stamp) { break; } // find the imu data which is closest to the keyframe_ auto closest_imu = imu_cursor; for(auto imu = imu_cursor; imu != imu_queue.end(); imu++) { auto dt = ((*closest_imu)->header.stamp - keyframe->stamp).toSec(); auto dt2 = ((*imu)->header.stamp - keyframe->stamp).toSec(); if(std::abs(dt) < std::abs(dt2)) { break; } closest_imu = imu; } // if the time residual between the imu and keyframe_ is too large, skip it imu_cursor = closest_imu; if(0.2 < std::abs(((*closest_imu)->header.stamp - keyframe->stamp).toSec())) { continue; } sensor_msgs::Imu imu_; imu_.header = (*closest_imu)->header; imu_.orientation = (*closest_imu)->orientation; imu_.angular_velocity = (*closest_imu)->angular_velocity; imu_.linear_acceleration = (*closest_imu)->linear_acceleration; imu_.angular_velocity_covariance = (*closest_imu)->angular_velocity_covariance; imu_.linear_acceleration_covariance = (*closest_imu)->linear_acceleration_covariance; imu_.orientation_covariance = (*closest_imu)->orientation_covariance; keyframe->imu = imu_; updated = true; } auto remove_loc = std::upper_bound(imu_queue.begin(), imu_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const sensor_msgs::ImuConstPtr& imupoint) { return stamp < imupoint->header.stamp; }); imu_queue.erase(imu_queue.begin(), remove_loc); return updated; } std::pair get_closest_imu(ros::Time frame_stamp) { sensor_msgs::Imu imu_; std::pair false_result {false, imu_}; if(keyframes.empty() || imu_queue.empty()) return false_result; bool updated = false; auto imu_cursor = imu_queue.begin(); // find the imu data which is closest to the keyframe_ auto closest_imu = imu_cursor; for(auto imu = imu_cursor; imu != imu_queue.end(); imu++) { auto dt = ((*closest_imu)->header.stamp - frame_stamp).toSec(); auto dt2 = ((*imu)->header.stamp - frame_stamp).toSec(); if(std::abs(dt) < std::abs(dt2)) { break; } closest_imu = imu; } // if the time residual between the imu and keyframe_ is too large, skip it imu_cursor = closest_imu; if(0.2 < std::abs(((*closest_imu)->header.stamp - frame_stamp).toSec())) return false_result; imu_.header = (*closest_imu)->header; imu_.orientation = (*closest_imu)->orientation; imu_.angular_velocity = (*closest_imu)->angular_velocity; imu_.linear_acceleration = (*closest_imu)->linear_acceleration; imu_.angular_velocity_covariance = (*closest_imu)->angular_velocity_covariance; imu_.linear_acceleration_covariance = (*closest_imu)->linear_acceleration_covariance; imu_.orientation_covariance = (*closest_imu)->orientation_covariance; updated = true; // cout << (*closest_imu)->orientation < result {updated, imu_}; return result; } void transformUpdate(Eigen::Matrix4d& odom_to_update) // IMU { if (imuPointerLast >= 0) { // cout << " "; float imuRollLast = 0, imuPitchLast = 0; while (imuPointerFront != imuPointerLast) { if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) { break; } imuPointerFront = (imuPointerFront + 1) % imuQueLength; } cout << " "; if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) { imuRollLast = imuRoll[imuPointerFront]; imuPitchLast = imuPitch[imuPointerFront]; cout << " "; } else { cout << " "; int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; } Eigen::Matrix3d matr = odom_to_update.block<3, 3>(0, 0); // Eigen::Vector3d xyz = odom_to_update.block<3, 1>(0, 3); Eigen::Vector3d ypr_odom = R2ypr(matr.block<3,3>(0,0)); geometry_msgs::Quaternion imuQuat = tf::createQuaternionMsgFromRollPitchYaw(imuRollLast, imuPitchLast, ypr_odom(0)); Eigen::Matrix3d imu_rot = Eigen::Matrix3d(Eigen::Quaterniond(imuQuat.w, imuQuat.x, imuQuat.y, imuQuat.z)); Eigen::Vector3d ypr_imu = R2ypr(imu_rot); // IMU orientation transformed from world coordinate to map coordinate Eigen::Matrix3d imu_rot_transed = global_orient_matrix.inverse() * imu_rot; Eigen::Vector3d ypr_imu_trans = R2ypr(imu_rot_transed); double& yaw_ = ypr_odom(0); double pitch_fused = (1 - imu_fusion_ratio) * ypr_odom(1) + imu_fusion_ratio * ypr_imu_trans(1); double roll_fused = (1 - imu_fusion_ratio) * ypr_odom(2) + imu_fusion_ratio * ypr_imu_trans(2); geometry_msgs::Quaternion rosQuat = tf::createQuaternionMsgFromRollPitchYaw(roll_fused, pitch_fused, yaw_); Eigen::Quaterniond quat_updated = Eigen::Quaterniond(rosQuat.w, rosQuat.x, rosQuat.y, rosQuat.z); odom_to_update.block<3, 3>(0, 0) = quat_updated.toRotationMatrix(); if (imu_debug_out) cout << "IMU rp: " << RAD2DEG(ypr_imu(2)) << " " << RAD2DEG(ypr_imu(1)) << ". IMU transed rp: " << RAD2DEG(ypr_imu_trans(2)) << " " << RAD2DEG(ypr_imu_trans(1)) // << ". Odom rp: " << RAD2DEG(ypr_odom(2)) << " " << RAD2DEG(ypr_odom(1)) // << ". Updated rp: " << RAD2DEG(roll_fused) << " " << RAD2DEG(pitch_fused) // << ". Roll Pitch increment: " << RAD2DEG(roll_fused - ypr_odom(2)) << " " << RAD2DEG(pitch_fused - ypr_odom(1)) << endl; } } /** * @brief callback for point clouds * @param cloud_msg point cloud msg */ void pointcloud_callback(const geometry_msgs::TwistWithCovarianceStampedConstPtr& twistMsg, const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { if(!ros::ok()) { return; } timeLaserOdometry = cloud_msg->header.stamp.toSec(); double this_cloud_time = cloud_msg->header.stamp.toSec(); static double last_cloud_time = this_cloud_time; double dt = this_cloud_time - last_cloud_time; double egovel_cum_x = twistMsg->twist.twist.linear.x * dt; double egovel_cum_y = twistMsg->twist.twist.linear.y * dt; double egovel_cum_z = twistMsg->twist.twist.linear.z * dt; // If too large, set 0 if (pow(egovel_cum_x,2)+pow(egovel_cum_y,2)+pow(egovel_cum_z,2) > pow(max_egovel_cum, 2)); else egovel_cum.block<3, 1>(0, 3) = Eigen::Vector3d(egovel_cum_x, egovel_cum_y, egovel_cum_z); last_cloud_time = this_cloud_time; pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); pcl::fromROSMsg(*cloud_msg, *cloud); // Matching Eigen::Matrix4d pose = matching(cloud_msg->header.stamp, cloud); geometry_msgs::TwistWithCovariance twist = twistMsg->twist; // publish map to odom frame publish_odometry(cloud_msg->header.stamp, mapFrame, odometryFrame, pose, twist); // In offline estimation, point clouds will be supplied until the published time std_msgs::HeaderPtr read_until(new std_msgs::Header()); read_until->frame_id = points_topic; read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0); read_until_pub.publish(read_until); read_until->frame_id = "/filtered_points"; read_until_pub.publish(read_until); } void msf_pose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg, bool after_update) { if(after_update) { msf_pose_after_update = pose_msg; } else { msf_pose = pose_msg; } } /** * @brief downsample a point cloud * @param cloud input cloud * @return downsampled point cloud */ pcl::PointCloud::ConstPtr downsample(const pcl::PointCloud::ConstPtr& cloud) const { if(!downsample_filter) { return cloud; } pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); downsample_filter->setInputCloud(cloud); downsample_filter->filter(*filtered); return filtered; } /** * @brief estimate the relative pose between an input cloud and a keyframe_ cloud * @param stamp the timestamp of the input cloud * @param cloud the input cloud * @return the relative pose between the input cloud and the keyframe_ cloud */ Eigen::Matrix4d matching(const ros::Time& stamp, const pcl::PointCloud::ConstPtr& cloud) { if(!keyframe_cloud_s2s) { prev_time = ros::Time(); prev_trans_s2s.setIdentity(); keyframe_pose_s2s.setIdentity(); keyframe_stamp = stamp; keyframe_cloud_s2s = cloud;//downsample(cloud); registration_s2s->setInputTarget(keyframe_cloud_s2s); // Scan-to-scan if (enable_scan_to_map){ prev_trans_s2m.setIdentity(); keyframe_pose_s2m.setIdentity(); keyframe_cloud_s2m = cloud; registration_s2m->setInputTarget(keyframe_cloud_s2m); } return Eigen::Matrix4d::Identity(); } // auto filtered = downsample(cloud); auto filtered = cloud; // Set Source Cloud registration_s2s->setInputSource(filtered); if (enable_scan_to_map) registration_s2m->setInputSource(filtered); std::string msf_source; Eigen::Isometry3d msf_delta = Eigen::Isometry3d::Identity(); pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); Eigen::Matrix4d odom_s2s_now; Eigen::Matrix4d odom_s2m_now; // ********** Matching ********** Eigen::Matrix4d guess; if (use_ego_vel) guess = prev_trans_s2s * egovel_cum * msf_delta.matrix(); else guess = prev_trans_s2s * msf_delta.matrix(); chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); registration_s2s->align(*aligned, guess.cast()); chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); double time_used = chrono::duration_cast>(t2 - t1).count(); s2s_matching_time.push_back(time_used); publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta); // If not converged, use last transformation if(!registration_s2s->hasConverged()) { NODELET_INFO_STREAM("scan matching_ has not converged!!"); NODELET_INFO_STREAM("ignore this frame(" << stamp << ")"); if (enable_scan_to_map) return keyframe_pose_s2m * prev_trans_s2m; else return keyframe_pose_s2s * prev_trans_s2s; } Eigen::Matrix4d trans_s2s = registration_s2s->getFinalTransformation().cast(); odom_s2s_now = keyframe_pose_s2s * trans_s2s; Eigen::Matrix4d trans_s2m; if (enable_scan_to_map){ registration_s2m->align(*aligned, guess.cast()); if(!registration_s2m->hasConverged()) { NODELET_INFO_STREAM("scan matching_ has not converged!!"); NODELET_INFO_STREAM("ignore this frame(" << stamp << ")"); return keyframe_pose_s2m * prev_trans_s2m; } trans_s2m = registration_s2m->getFinalTransformation().cast(); odom_s2m_now = keyframe_pose_s2m * trans_s2m; } // Add abnormal judgment, that is, if the difference between the two frames matching point cloud // transition matrix is too large, it will be discarded bool thresholded = false; if(enable_transform_thresholding) { Eigen::Matrix4d radar_delta; if(enable_scan_to_map) radar_delta = prev_trans_s2m.inverse() * trans_s2m; else radar_delta = prev_trans_s2s.inverse() * trans_s2s; double dx_rd = radar_delta.block<3, 1>(0, 3).norm(); // double da_rd = std::acos(Eigen::Quaterniond(radar_delta.block<3, 3>(0, 0)).w())*180/M_PI; Eigen::AngleAxisd rotation_vector; rotation_vector.fromRotationMatrix(radar_delta.block<3, 3>(0, 0)); double da_rd = rotation_vector.angle(); Eigen::Matrix3d rot_rd = radar_delta.block<3, 3>(0, 0).cast(); bool too_large_trans = dx_rd > max_acceptable_trans || da_rd > max_acceptable_angle; double da, dx, delta_rot_imu = 0; Eigen::Matrix3d matrix_rot; Eigen::Vector3d delta_trans_egovel; if (enable_imu_thresholding) { // Use IMU orientation to determine whether the matching result is good or not sensor_msgs::Imu frame_imu; Eigen::Matrix3d rot_imu = Eigen::Matrix3d::Identity(); auto result = get_closest_imu(stamp); if (result.first) { frame_imu = result.second; Eigen::Quaterniond imu_quat(frame_imu.orientation.w, frame_imu.orientation.x, frame_imu.orientation.y, frame_imu.orientation.z); Eigen::Quaterniond prev_imu_quat(last_frame_imu.orientation.w, last_frame_imu.orientation.x, last_frame_imu.orientation.y, last_frame_imu.orientation.z); rot_imu = (prev_imu_quat.inverse() * imu_quat).toRotationMatrix(); Eigen::Vector3d eulerAngle_imu = rot_imu.eulerAngles(0,1,2); // roll pitch yaw Eigen::Vector3d eulerAngle_rd = last_radar_delta.block<3,3>(0,0).eulerAngles(0,1,2); Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(restrict_rad(eulerAngle_imu(0)),Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(restrict_rad(eulerAngle_imu(1)),Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(restrict_rad(eulerAngle_rd(2)),Eigen::Vector3d::UnitZ())); matrix_rot = yawAngle * pitchAngle * rollAngle; da = fabs(std::acos(Eigen::Quaterniond(rot_rd.inverse() * rot_imu).w()))*180/M_PI; delta_rot_imu = fabs(std::acos(Eigen::Quaterniond(rot_imu).w()))*180/M_PI; last_frame_imu = frame_imu; } delta_trans_egovel = egovel_cum.block<3,1>(0,3).cast(); Eigen::Vector3d delta_trans_radar = radar_delta.block<3,1>(0,3).cast(); dx = (delta_trans_egovel - delta_trans_radar).norm(); if (dx > max_diff_trans || da > max_diff_angle || too_large_trans) { Eigen::Matrix4d mat_est(Eigen::Matrix4d::Identity()); mat_est.block<3, 3>(0, 0) = matrix_rot; mat_est.block<3, 1>(0, 3) = delta_trans_egovel; if (too_large_trans) cout << "Too large transform! " << dx_rd << "[m] " << da_rd << "[deg] "; cout << "Difference of Odom and IMU/EgoVel too large " << dx << "[m] " << da << "[deg] (" << stamp << ")" << endl; prev_trans_s2s = prev_trans_s2s * mat_est; thresholded = true; if (enable_scan_to_map){ prev_trans_s2m = prev_trans_s2m * mat_est; odom_s2m_now = keyframe_pose_s2m * prev_trans_s2m; } else odom_s2s_now = keyframe_pose_s2s * prev_trans_s2s; } } else { if (too_large_trans) { cout << "Too large transform!! " << dx_rd << "[m] " << da_rd << "[degree]"<< " Ignore this frame (" << stamp << ")" << endl; prev_trans_s2s = trans_s2s; thresholded = true; if (enable_scan_to_map){ prev_trans_s2m = trans_s2m; odom_s2m_now = keyframe_pose_s2m * prev_trans_s2m * radar_delta; } else odom_s2s_now = keyframe_pose_s2s * prev_trans_s2s * radar_delta; } } last_radar_delta = radar_delta; if(0){ cout << "radar trans:" << dx_rd << " m rot:" << da_rd << " degree. EgoVel " << delta_trans_egovel.norm() << " m. " << "IMU rot " << delta_rot_imu << " degree." << endl; cout << "dx " << dx << " m. da " << da << " degree." << endl; } } prev_time = stamp; if (!thresholded) { prev_trans_s2s = trans_s2s; prev_trans_s2m = trans_s2m; } //********** Decided whether to accept the frame as a key frame or not ********** if(keyframe_updater->decide(Eigen::Isometry3d(odom_s2s_now), stamp)) { // Loose Coupling the IMU roll & pitch if (enable_imu_fusion){ if(enable_scan_to_map) transformUpdate(odom_s2m_now); else transformUpdate(odom_s2s_now); } keyframe_cloud_s2s = filtered; registration_s2s->setInputTarget(keyframe_cloud_s2s); keyframe_pose_s2s = odom_s2s_now; keyframe_stamp = stamp; prev_time = stamp; prev_trans_s2s.setIdentity(); double accum_d = keyframe_updater->get_accum_distance(); KeyFrame::Ptr keyframe(new KeyFrame(keyframe_index, stamp, Eigen::Isometry3d(odom_s2s_now.cast()), accum_d, cloud)); keyframe_index ++; keyframes.push_back(keyframe); // record keyframe's imu flush_imu_queue(); if (enable_scan_to_map){ pcl::PointCloud::Ptr submap_cloud(new pcl::PointCloud()); pcl::PointCloud::ConstPtr submap_cloud_downsampled; for(size_t i=std::max(0, (int)keyframes.size()-max_submap_frames); i < keyframes.size()-1; i++){ Eigen::Matrix4d rel_pose = keyframes.at(i)->odom_scan2scan.matrix().inverse() * keyframes.back()->odom_scan2scan.matrix(); pcl::PointCloud::Ptr cloud_transformed(new pcl::PointCloud()); pcl::transformPointCloud(*keyframes.at(i)->cloud, *cloud_transformed, rel_pose); *submap_cloud += *cloud_transformed; } submap_cloud_downsampled = downsample(submap_cloud); keyframe_cloud_s2m = submap_cloud_downsampled; registration_s2m->setInputTarget(keyframe_cloud_s2m); keyframes.back()->odom_scan2map = Eigen::Isometry3d(odom_s2m_now); keyframe_pose_s2m = odom_s2m_now; prev_trans_s2m.setIdentity(); } } if (aligned_points_pub.getNumSubscribers() > 0) { pcl::transformPointCloud (*cloud, *aligned, odom_s2s_now); aligned->header.frame_id = odometryFrame; aligned_points_pub.publish(*aligned); } if (enable_scan_to_map) return odom_s2m_now; else return odom_s2s_now; } /** * @brief publish odometry * @param stamp timestamp * @param pose odometry pose to be published */ void publish_odometry(const ros::Time& stamp, const std::string& father_frame_id, const std::string& child_frame_id, const Eigen::Matrix4d& pose_in, const geometry_msgs::TwistWithCovariance twist_in) { // publish transform stamped for IMU integration geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose_in, father_frame_id, child_frame_id); //"map" trans_pub.publish(odom_trans); // broadcast the transform over TF map2odom_broadcaster.sendTransform(odom_trans); // publish the transform nav_msgs::Odometry odom; odom.header.stamp = stamp; odom.header.frame_id = father_frame_id; // frame: /odom odom.child_frame_id = child_frame_id; odom.pose.pose.position.x = pose_in(0, 3); odom.pose.pose.position.y = pose_in(1, 3); odom.pose.pose.position.z = pose_in(2, 3); odom.pose.pose.orientation = odom_trans.transform.rotation; odom.twist = twist_in; odom_pub.publish(odom); } /** * @brief publish scan matching_ status */ void publish_scan_matching_status(const ros::Time& stamp, const std::string& frame_id, pcl::PointCloud::ConstPtr aligned, const std::string& msf_source, const Eigen::Isometry3d& msf_delta) { if(!status_pub.getNumSubscribers()) { return; } ScanMatchingStatus status; status.header.frame_id = frame_id; status.header.stamp = stamp; status.has_converged = registration_s2s->hasConverged(); status.matching_error = registration_s2s->getFitnessScore(); const double max_correspondence_dist = 0.5; int num_inliers = 0; std::vector k_indices; std::vector k_sq_dists; for(int i=0; isize(); i++) { const auto& pt = aligned->at(i); registration_s2s->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists); if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) { num_inliers++; } } status.inlier_fraction = static_cast(num_inliers) / aligned->size(); status.relative_pose = isometry2pose(Eigen::Isometry3d(registration_s2s->getFinalTransformation().cast())); if(!msf_source.empty()) { status.prediction_labels.resize(1); status.prediction_labels[0].data = msf_source; status.prediction_errors.resize(1); Eigen::Isometry3d error = Eigen::Isometry3d(registration_s2s->getFinalTransformation().cast()).inverse() * msf_delta; status.prediction_errors[0] = isometry2pose(error.cast()); } status_pub.publish(status); } void command_callback(const std_msgs::String& str_msg) { if (str_msg.data == "time") { std::sort(s2s_matching_time.begin(), s2s_matching_time.end()); double median = s2s_matching_time.at(size_t(s2s_matching_time.size() / 2)); cout << "Scan Matching time cost (median): " << median << endl; } } private: // ROS topics ros::NodeHandle nh; ros::NodeHandle mt_nh; ros::NodeHandle private_nh; // ros::Subscriber points_sub; ros::Subscriber msf_pose_sub; ros::Subscriber msf_pose_after_update_sub; ros::Subscriber imu_sub; std::mutex imu_queue_mutex; std::deque imu_queue; sensor_msgs::Imu last_frame_imu; bool enable_imu_fusion; bool imu_debug_out; Eigen::Matrix3d global_orient_matrix; // The rotation matrix with initial IMU roll & pitch measurement (yaw = 0) double timeLaserOdometry = 0; int imuPointerFront; int imuPointerLast; double imuTime[imuQueLength]; float imuRoll[imuQueLength]; float imuPitch[imuQueLength]; double imu_fusion_ratio; std::unique_ptr> ego_vel_sub; std::unique_ptr> points_sub; std::unique_ptr> sync; // Submap ros::Publisher submap_pub; std::unique_ptr keyframe_updater; std::vector keyframes; size_t keyframe_index = 0; double map_cloud_resolution; int max_submap_frames; bool enable_scan_to_map; // std::unique_ptr graph_slam; // std::unique_ptr inf_calclator; ros::Publisher odom_pub; ros::Publisher trans_pub; // ros::Publisher keyframe_trans_pub; ros::Publisher aligned_points_pub; ros::Publisher status_pub; ros::Publisher read_until_pub; tf::TransformListener tf_listener; tf::TransformBroadcaster map2odom_broadcaster; // map => odom_frame std::string points_topic; // keyframe_ parameters double keyframe_delta_trans; // minimum distance between keyframes_ double keyframe_delta_angle; // double keyframe_delta_time; // // registration validation by thresholding bool enable_transform_thresholding; // bool enable_imu_thresholding; double max_acceptable_trans; // double max_acceptable_angle; double max_diff_trans; double max_diff_angle; double max_egovel_cum; Eigen::Matrix4d last_radar_delta = Eigen::Matrix4d::Identity(); // odometry calculation geometry_msgs::PoseWithCovarianceStampedConstPtr msf_pose; geometry_msgs::PoseWithCovarianceStampedConstPtr msf_pose_after_update; Eigen::Matrix4d egovel_cum = Eigen::Matrix4d::Identity(); bool use_ego_vel; ros::Time prev_time; Eigen::Matrix4d prev_trans_s2s; // previous relative transform from keyframe_ Eigen::Matrix4d keyframe_pose_s2s; // keyframe_ pose Eigen::Matrix4d prev_trans_s2m; Eigen::Matrix4d keyframe_pose_s2m; // keyframe_ pose ros::Time keyframe_stamp; // keyframe_ time pcl::PointCloud::ConstPtr keyframe_cloud_s2s; // keyframe_ point cloud pcl::PointCloud::ConstPtr keyframe_cloud_s2m; // keyframe_ point cloud // Registration pcl::Filter::Ptr downsample_filter; pcl::Registration::Ptr registration_s2s; // Scan-to-Scan Registration pcl::Registration::Ptr registration_s2m; // Scan-to-Submap Registration // Time evaluation std::vector s2s_matching_time; ros::Subscriber command_sub; }; } // namespace radar_graph_slam PLUGINLIB_EXPORT_CLASS(radar_graph_slam::ScanMatchingOdometryNodelet, nodelet::Nodelet) ================================================ FILE: cmake/FindG2O.cmake ================================================ # Find the header files FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h ${G2O_ROOT}/include $ENV{G2O_ROOT}/include $ENV{G2O_ROOT} /usr/local/include /usr/include /opt/local/include /sw/local/include /sw/include /opt/ros/$ENV{ROS_DISTRO}/include NO_DEFAULT_PATH ) # Macro to unify finding both the debug and release versions of the # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY # macro. MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) FIND_LIBRARY("${MYLIBRARY}_DEBUG" NAMES "g2o_${MYLIBRARYNAME}_d" PATHS ${G2O_ROOT}/lib/Debug ${G2O_ROOT}/lib $ENV{G2O_ROOT}/lib/Debug $ENV{G2O_ROOT}/lib /opt/ros/$ENV{ROS_DISTRO}/lib NO_DEFAULT_PATH ) FIND_LIBRARY("${MYLIBRARY}_DEBUG" NAMES "g2o_${MYLIBRARYNAME}_d" PATHS ~/Library/Frameworks /Library/Frameworks /usr/local/lib /usr/local/lib64 /usr/lib /usr/lib64 /opt/local/lib /sw/local/lib /sw/lib ) FIND_LIBRARY(${MYLIBRARY} NAMES "g2o_${MYLIBRARYNAME}" PATHS ${G2O_ROOT}/lib/Release ${G2O_ROOT}/lib $ENV{G2O_ROOT}/lib/Release $ENV{G2O_ROOT}/lib /opt/ros/$ENV{ROS_DISTRO}/lib NO_DEFAULT_PATH ) FIND_LIBRARY(${MYLIBRARY} NAMES "g2o_${MYLIBRARYNAME}" PATHS ~/Library/Frameworks /Library/Frameworks /usr/local/lib /usr/local/lib64 /usr/lib /usr/lib64 /opt/local/lib /sw/local/lib /sw/lib ) IF(NOT ${MYLIBRARY}_DEBUG) IF(MYLIBRARY) SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) ENDIF(MYLIBRARY) ENDIF( NOT ${MYLIBRARY}_DEBUG) ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) # Find the core elements FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) # Find the CLI library FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) # Find the pluggable solvers FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) # Find the predefined types FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons) # G2O solvers declared found if we found at least one solver SET(G2O_SOLVERS_FOUND "NO") IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) SET(G2O_SOLVERS_FOUND "YES") ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) # G2O itself declared found if we found the core libraries and at least one solver SET(G2O_FOUND "NO") IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) SET(G2O_FOUND "YES") ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) ================================================ FILE: config/params.yaml ================================================ radar_slam: # Topics pointCloudTopic: "/radar_enhanced_pcl" # Point cloud data imuTopic: "/vectornav/imu" # IMU data odomTopic: "/odom" # IMU pre-preintegration odometry, same frequency as IMU gpsTopic: "/ublox/fix" # GPS odometry topic from navsat, see module_navsat.launch file # Frames lidarFrame: "" baselinkFrame: "base_link" odometryFrame: "odom" mapFrame: "map" # GPS Settings # useGpsElevation: false # if GPS elevation is bad, set to "false" # gpsCovThreshold: 2.0 # m^2, threshold for using GPS data # poseCovThreshold: 25.0 # m^2, threshold for using GPS data # Export settings # savePCD: false # # savePCDDirectory: "/Downloads/4DRadarSLAM/" # # Sensor Settings # downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 # lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used # lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used # IMU Settings imuAccNoise: 0.0022281160035059417 # 3.9939570888238808e-03 imuGyrNoise: 0.00011667951042710442 # 1.5636343949698187e-03 imuAccBiasN: 0.00011782392708033614 # 6.4356659353532566e-05 imuGyrBiasN: 2.616129872371749e-06 # 3.5640318696367613e-05 imuGravity: 9.80511 imuRPYWeight: 0.01 # Extrinsics (Lidar -> IMU) # extrinsicTrans: [0, 0, 0] extrinsicTrans: [-0.3176955976234, -0.13761019052125, 0.05898352725152] # extrinsicRot: [1, 0, 0, # 0, -1, 0, # 0, 0, -1] # extrinsicRPY: [1, 0, 0, # 0, -1, 0, # 0, 0, -1] extrinsicRot: [0.999735807578, -0.0215215701795, -0.0081643477385, -0.02148120581797, -0.9997581134183, 0.00502853428037, -0.00826995351904, -0.0048509797951, -0.99995400578406] extrinsicRPY: [0.999735807578, -0.0215215701795, -0.0081643477385, -0.02148120581797, -0.9997581134183, 0.00502853428037, -0.00826995351904, -0.0048509797951, -0.99995400578406] # extrinsicRot: [1, 0, 0, # 0, 1, 0, # 0, 0, 1] # extrinsicRPY: [1, 0, 0, # 0, 1, 0, # 0, 0, 1] ================================================ FILE: include/dbscan/DBSCAN_kdtree.h ================================================ #ifndef DBSCAN_KDTREE_H #define DBSCAN_KDTREE_H #include #include "DBSCAN_simple.h" template class DBSCANKdtreeCluster: public DBSCANSimpleCluster { protected: virtual int radiusSearch ( int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { return this->search_method_->radiusSearch(index, radius, k_indices, k_sqr_distances); } }; // class DBSCANCluster #endif // DBSCAN_KDTREE_H ================================================ FILE: include/dbscan/DBSCAN_precomp.h ================================================ #ifndef DBSCAN_PRECOMP_H #define DBSCAN_PRECOMP_H #include #include "DBSCAN_simple.h" template class DBSCANPrecompCluster : public DBSCANSimpleCluster { public: virtual void setInputCloud(pcl::PointCloud::Ptr cloud) { this->input_cloud_ = cloud; int size = this->input_cloud_->points.size(); adjoint_indexes_ = std::vector>(size, std::vector()); distances_ = std::vector>(size, std::vector()); precomp(); } protected: std::vector> distances_; std::vector> adjoint_indexes_; void precomp() { int size = this->input_cloud_->points.size(); for (int i = 0; i < size; i++) { adjoint_indexes_[i].push_back(i); distances_[i].push_back(0.0); } double radius_square = this->eps_ * this->eps_; for (int i = 0; i < size; i++) { for (int j = i+1; j < size; j++) { double distance_x = this->input_cloud_->points[i].x - this->input_cloud_->points[j].x; double distance_y = this->input_cloud_->points[i].y - this->input_cloud_->points[j].y; double distance_z = this->input_cloud_->points[i].z - this->input_cloud_->points[j].z; double distance_square = distance_x * distance_x + distance_y * distance_y + distance_z * distance_z; if (distance_square <= radius_square) { adjoint_indexes_[i].push_back(j); adjoint_indexes_[j].push_back(i); double distance = std::sqrt(distance_square); distances_[i].push_back(distance); distances_[j].push_back(distance); } } } } virtual int radiusSearch( int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { // radius = eps_ k_indices = adjoint_indexes_[index]; k_sqr_distances = distances_[index]; return k_indices.size(); } }; // class DBSCANCluster #endif // DBSCAN_PRECOMP_H ================================================ FILE: include/dbscan/DBSCAN_simple.h ================================================ #ifndef DBSCAN_H #define DBSCAN_H #include #define UN_PROCESSED 0 #define PROCESSING 1 #define PROCESSED 2 inline bool comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b) { return (a.indices.size () < b.indices.size ()); } template class DBSCANSimpleCluster { public: typedef typename pcl::PointCloud::Ptr PointCloudPtr; typedef typename pcl::search::KdTree::Ptr KdTreePtr; virtual void setInputCloud(PointCloudPtr cloud) { input_cloud_ = cloud; } void setSearchMethod(KdTreePtr tree) { search_method_ = tree; } void extract(std::vector& cluster_indices) { std::vector nn_indices; std::vector nn_distances; std::vector is_noise(input_cloud_->points.size(), false); std::vector types(input_cloud_->points.size(), UN_PROCESSED); for (int i = 0; i < input_cloud_->points.size(); i++) { if (types[i] == PROCESSED) { continue; } int nn_size = radiusSearch(i, eps_, nn_indices, nn_distances); if (nn_size < minPts_) { is_noise[i] = true; continue; } std::vector seed_queue; seed_queue.push_back(i); types[i] = PROCESSED; for (int j = 0; j < nn_size; j++) { if (nn_indices[j] != i) { seed_queue.push_back(nn_indices[j]); types[nn_indices[j]] = PROCESSING; } } // for every point near the chosen core point. int sq_idx = 1; while (sq_idx < seed_queue.size()) { int cloud_index = seed_queue[sq_idx]; if (is_noise[cloud_index] || types[cloud_index] == PROCESSED) { // seed_queue.push_back(cloud_index); types[cloud_index] = PROCESSED; sq_idx++; continue; // no need to check neighbors. } nn_size = radiusSearch(cloud_index, eps_, nn_indices, nn_distances); if (nn_size >= minPts_) { for (int j = 0; j < nn_size; j++) { if (types[nn_indices[j]] == UN_PROCESSED) { seed_queue.push_back(nn_indices[j]); types[nn_indices[j]] = PROCESSING; } } } types[cloud_index] = PROCESSED; sq_idx++; } if (seed_queue.size() >= min_pts_per_cluster_ && seed_queue.size () <= max_pts_per_cluster_) { pcl::PointIndices r; r.indices.resize(seed_queue.size()); for (int j = 0; j < seed_queue.size(); ++j) { r.indices[j] = seed_queue[j]; } // These two lines should not be needed: (can anyone confirm?) -FF std::sort (r.indices.begin (), r.indices.end ()); r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); r.header = input_cloud_->header; cluster_indices.push_back (r); // We could avoid a copy by working directly in the vector } } // for every point in input cloud std::sort (cluster_indices.rbegin (), cluster_indices.rend (), comparePointClusters); } void setClusterTolerance(double tolerance) { eps_ = tolerance; } void setMinClusterSize (int min_cluster_size) { min_pts_per_cluster_ = min_cluster_size; } void setMaxClusterSize (int max_cluster_size) { max_pts_per_cluster_ = max_cluster_size; } void setCorePointMinPts(int core_point_min_pts) { minPts_ = core_point_min_pts; } protected: PointCloudPtr input_cloud_; double eps_ {0.0}; int minPts_ {1}; // not including the point itself. int min_pts_per_cluster_ {1}; int max_pts_per_cluster_ {std::numeric_limits::max()}; KdTreePtr search_method_; virtual int radiusSearch( int index, double radius, std::vector &k_indices, std::vector &k_sqr_distances) const { k_indices.clear(); k_sqr_distances.clear(); k_indices.push_back(index); k_sqr_distances.push_back(0); int size = input_cloud_->points.size(); double radius_square = radius * radius; for (int i = 0; i < size; i++) { if (i == index) { continue; } double distance_x = input_cloud_->points[i].x - input_cloud_->points[index].x; double distance_y = input_cloud_->points[i].y - input_cloud_->points[index].y; double distance_z = input_cloud_->points[i].z - input_cloud_->points[index].z; double distance_square = distance_x * distance_x + distance_y * distance_y + distance_z * distance_z; if (distance_square <= radius_square) { k_indices.push_back(i); k_sqr_distances.push_back(std::sqrt(distance_square)); } } return k_indices.size(); } }; // class DBSCANCluster #endif // DBSCAN_H ================================================ FILE: include/g2o/edge_plane_identity.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef EDGE_PLANE_IDENTITY_HPP #define EDGE_PLANE_IDENTITY_HPP #include #include #include namespace g2o { /** * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane normals. * */ class EdgePlaneIdentity : public BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePlaneIdentity() : BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane>() { _information.setIdentity(); _error.setZero(); } void computeError() { const VertexPlane* v1 = static_cast(_vertices[0]); const VertexPlane* v2 = static_cast(_vertices[1]); Eigen::Vector4d p1 = v1->estimate().toVector(); Eigen::Vector4d p2 = v2->estimate().toVector(); if(p1.dot(p2) < 0.0) { p2 = -p2; } _error = (p2 - p1) - _measurement; } virtual bool read(std::istream& is) override { Eigen::Vector4d v; for(int i = 0; i < 4; ++i) { is >> v[i]; } setMeasurement(v); for(int i = 0; i < information().rows(); ++i) { for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) { information()(j, i) = information()(i, j); } } } return true; } virtual bool write(std::ostream& os) const override { for(int i = 0; i < 4; ++i) { os << _measurement[i] << " "; } for(int i = 0; i < information().rows(); ++i) { for(int j = i; j < information().cols(); ++j) { os << " " << information()(i, j); }; } return os.good(); } virtual void setMeasurement(const Eigen::Vector4d& m) override { _measurement = m; } virtual int measurementDimension() const override { return 4; } }; } // namespace g2o #endif // EDGE_PLANE_PARALLEL_HPP ================================================ FILE: include/g2o/edge_plane_parallel.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef EDGE_PLANE_PARALLEL_HPP #define EDGE_PLANE_PARALLEL_HPP #include #include #include namespace g2o { class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePlaneParallel() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() { _information.setIdentity(); _error.setZero(); } void computeError() override { const VertexPlane* v1 = static_cast(_vertices[0]); const VertexPlane* v2 = static_cast(_vertices[1]); Eigen::Vector3d normal1 = v1->estimate().normal(); Eigen::Vector3d normal2 = v2->estimate().normal(); if(normal1.dot(normal2) < 0.0) { normal2 = -normal2; } _error = (normal2 - normal1) - _measurement; } virtual bool read(std::istream& is) override { Eigen::Vector3d v; for(int i = 0; i < 3; ++i) { is >> v[i]; } setMeasurement(v); for(int i = 0; i < information().rows(); ++i) { for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) { information()(j, i) = information()(i, j); } } } return true; } virtual bool write(std::ostream& os) const override { for(int i = 0; i < 3; ++i) { os << _measurement[i] << " "; } for(int i = 0; i < information().rows(); ++i) { for(int j = i; j < information().cols(); ++j) { os << " " << information()(i, j); }; } return os.good(); } virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } virtual int measurementDimension() const override { return 3; } }; class EdgePlanePerpendicular : public BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePlanePerpendicular() : BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane>() { _information.setIdentity(); _error.setZero(); } void computeError() override { const VertexPlane* v1 = static_cast(_vertices[0]); const VertexPlane* v2 = static_cast(_vertices[1]); Eigen::Vector3d normal1 = v1->estimate().normal().normalized(); Eigen::Vector3d normal2 = v2->estimate().normal().normalized(); _error[0] = normal1.dot(normal2); } virtual bool read(std::istream& is) override { Eigen::Vector3d v; for(int i = 0; i < 3; ++i) { is >> v[i]; } setMeasurement(v); for(int i = 0; i < information().rows(); ++i) { for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) { information()(j, i) = information()(i, j); } } } return true; } virtual bool write(std::ostream& os) const override { for(int i = 0; i < 3; ++i) { os << _measurement[i] << " "; } for(int i = 0; i < information().rows(); ++i) { for(int j = i; j < information().cols(); ++j) { os << " " << information()(i, j); }; } return os.good(); } virtual void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } virtual int measurementDimension() const override { return 3; } }; } // namespace g2o #endif // EDGE_PLANE_PARALLEL_HPP ================================================ FILE: include/g2o/edge_plane_prior.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef EDGE_PLANE_PRIOR_HPP #define EDGE_PLANE_PRIOR_HPP #include #include #include namespace g2o { class EdgePlanePriorNormal : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePlanePriorNormal() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane>() {} void computeError() override { const g2o::VertexPlane* v1 = static_cast(_vertices[0]); Eigen::Vector3d normal = v1->estimate().normal(); if(normal.dot(_measurement) < 0.0) { normal = -normal; } _error = normal - _measurement; } void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } virtual bool read(std::istream& is) override { Eigen::Vector3d v; is >> v(0) >> v(1) >> v(2); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector3d v = _measurement; os << v(0) << " " << v(1) << " " << v(2) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; class EdgePlanePriorDistance : public g2o::BaseUnaryEdge<1, double, g2o::VertexPlane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgePlanePriorDistance() : g2o::BaseUnaryEdge<1, double, g2o::VertexPlane>() {} void computeError() override { const g2o::VertexPlane* v1 = static_cast(_vertices[0]); _error[0] = _measurement - v1->estimate().distance(); } void setMeasurement(const double& m) override { _measurement = m; } virtual bool read(std::istream& is) override { is >> _measurement; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { os << _measurement; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif // EDGE_SE3_PRIORXY_HPP ================================================ FILE: include/g2o/edge_se3_gt_utm.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef KKL_G2O_EDGE_SE3_GTUTM_HPP #define KKL_G2O_EDGE_SE3_GTUTM_HPP #include #include namespace g2o { class EdgeSE3GtUTM : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3GtUTM() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); Eigen::Vector4d utm_homogeneous = Eigen::Vector4d(_measurement_utm(0), _measurement_utm(1), _measurement_utm(2), 1); Eigen::Vector3d estimate = (v1->estimate() * utm_homogeneous).head(3); _error = estimate - _measurement; } void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } void setMeasurement_utm(const Eigen::Vector3d& m){ _measurement_utm = m; } virtual bool read(std::istream& is) override { Eigen::Vector3d v; is >> v(0) >> v(1) >> v(2); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector3d v = _measurement; os << v(0) << " " << v(1) << " " << v(2) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } private: Eigen::Vector3d _measurement_utm; }; } // namespace g2o #endif ================================================ FILE: include/g2o/edge_se3_plane.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef KKL_G2O_EDGE_SE3_PLANE_HPP #define KKL_G2O_EDGE_SE3_PLANE_HPP #include #include #include namespace g2o { class EdgeSE3Plane : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3Plane() : BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); const g2o::VertexPlane* v2 = static_cast(_vertices[1]); Eigen::Isometry3d w2n = v1->estimate().inverse(); Plane3D local_plane = w2n * v2->estimate(); _error = local_plane.ominus(_measurement); } void setMeasurement(const g2o::Plane3D& m) override { _measurement = m; } virtual bool read(std::istream& is) override { Eigen::Vector4d v; is >> v(0) >> v(1) >> v(2) >> v(3); setMeasurement(Plane3D(v)); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector4d v = _measurement.toVector(); os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif ================================================ FILE: include/g2o/edge_se3_priorquat.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef KKL_G2O_EDGE_SE3_PRIORQUAT_HPP #define KKL_G2O_EDGE_SE3_PRIORQUAT_HPP #include #include namespace g2o { class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3PriorQuat() : g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear()); if(_measurement.coeffs().dot(estimate.coeffs()) < 0.0) { estimate.coeffs() = -estimate.coeffs(); } _error = estimate.vec() - _measurement.vec(); } void setMeasurement(const Eigen::Quaterniond& m) override { _measurement = m; if(m.w() < 0.0) { _measurement.coeffs() = -m.coeffs(); } } virtual bool read(std::istream& is) override { Eigen::Quaterniond q; is >> q.w() >> q.x() >> q.y() >> q.z(); setMeasurement(q); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Quaterniond q = _measurement; os << q.w() << " " << q.x() << " " << q.y() << " " << q.z(); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif ================================================ FILE: include/g2o/edge_se3_priorvec.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef KKL_G2O_EDGE_SE3_PRIORVEC_HPP #define KKL_G2O_EDGE_SE3_PRIORVEC_HPP #include #include namespace g2o { class EdgeSE3PriorVec : public g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3PriorVec() : g2o::BaseUnaryEdge<3, Eigen::Matrix, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); Eigen::Vector3d direction = _measurement.head<3>(); Eigen::Vector3d measurement = _measurement.tail<3>(); Eigen::Vector3d estimate = (v1->estimate().linear().inverse() * direction); _error = estimate - measurement; } void setMeasurement(const Eigen::Matrix& m) override { _measurement.head<3>() = m.head<3>().normalized(); _measurement.tail<3>() = m.tail<3>().normalized(); } virtual bool read(std::istream& is) override { Eigen::Matrix v; is >> v[0] >> v[1] >> v[2] >> v[3] >> v[4] >> v[5]; setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Matrix v = _measurement; os << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5]; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif ================================================ FILE: include/g2o/edge_se3_priorxy.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef EDGE_SE3_PRIORXY_HPP #define EDGE_SE3_PRIORXY_HPP #include #include namespace g2o { class EdgeSE3PriorXY : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3PriorXY() : g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); Eigen::Vector2d estimate = v1->estimate().translation().head<2>(); _error = estimate - _measurement; } void setMeasurement(const Eigen::Vector2d& m) override { _measurement = m; } virtual bool read(std::istream& is) override { Eigen::Vector2d v; is >> v(0) >> v(1); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector2d v = _measurement; os << v(0) << " " << v(1) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif // EDGE_SE3_PRIORXY_HPP ================================================ FILE: include/g2o/edge_se3_priorxyz.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef KKL_G2O_EDGE_SE3_PRIORXYZ_HPP #define KKL_G2O_EDGE_SE3_PRIORXYZ_HPP #include #include namespace g2o { class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3PriorXYZ() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); Eigen::Vector3d estimate = v1->estimate().translation(); _error = estimate - _measurement; } void setMeasurement(const Eigen::Vector3d& m) override { _measurement = m; } virtual bool read(std::istream& is) override { Eigen::Vector3d v; is >> v(0) >> v(1) >> v(2); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector3d v = _measurement; os << v(0) << " " << v(1) << " " << v(2) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif ================================================ FILE: include/g2o/edge_se3_priorz.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef EDGE_SE3_PRIORZ_HPP #define EDGE_SE3_PRIORZ_HPP #include #include namespace Eigen { typedef Eigen::Matrix Vector1d; } namespace g2o { class EdgeSE3PriorZ : public g2o::BaseUnaryEdge<1, Eigen::Vector1d, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3PriorZ() : g2o::BaseUnaryEdge<1, Eigen::Vector1d, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); Eigen::Vector1d estimate = v1->estimate().translation().tail<1>(); _error = estimate - _measurement; } void setMeasurement(const Eigen::Vector1d& m) override { _measurement = m; } virtual bool read(std::istream& is) override { Eigen::Vector1d v; is >> v(0); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector1d v = _measurement; os << v(0) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif // EDGE_SE3_PRIORZ_HPP ================================================ FILE: include/g2o/edge_se3_se3.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef KKL_G2O_EDGE_SE3_SE3_HPP #define KKL_G2O_EDGE_SE3_SE3_HPP #include #include // #include // #include #include #include namespace g2o { class EdgeSE3SE3 : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3SE3() : BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); const g2o::VertexSE3* v2 = static_cast(_vertices[1]); g2o::SE3Quat estimate1 (v1->estimate().rotation(), v1->estimate().translation()); g2o::SE3Quat estimate2 (v2->estimate().rotation(), v2->estimate().translation()); g2o::SE3Quat C(_measurement); SE3Quat error= estimate2.inverse() * C * estimate1; _error = error.log(); // const VertexSE3* v1 = dynamic_cast(_vertices[0]); // const VertexSE3* v2 = dynamic_cast(_vertices[1]); // SE3Quat delta = _measurement.inverse(). * (v1->estimate().inverse()*v2->estimate()); // _error.head<3>() = delta.translation(); // // The analytic Jacobians assume the error in this special form (w beeing positive) // if (delta.rotation().w() < 0.) // _error.tail<3>() = - delta.rotation().vec(); // else // _error.tail<3>() = delta.rotation().vec(); } // void linearizeOplus() override { // const g2o::VertexSE3* v1 = static_cast(_vertices[0]); // const g2o::VertexSE3* v2 = static_cast(_vertices[1]); // SE3Quat Ti (v1->estimate().rotation(), v1->estimate().translation()); // SE3Quat Tj (v2->estimate().rotation(), v2->estimate().translation()); // //注意这里把测量标记为Tij应该是标记错误了，应该是Tji，不然整个误差公式说不通了 // //这个可以看orbslam EdgeSim3里添加测量时就是用的Sji // const SE3Quat & Tij = _measurement; // shoulb be Tji // SE3Quat invTij = Tij.inverse(); // SE3Quat invTj_Tij = Tj.inverse()*Tij; // SE3Quat infTi_invTij = Ti.inverse()*invTij; // _jacobianOplusXi = invTj_Tij.adj(); // _jacobianOplusXj = - infTi_invTij.adj(); // } void setMeasurement(const g2o::SE3Quat& m) override { _measurement = m; } virtual bool read(std::istream& is) override { g2o::Vector6 vec6; is >> vec6(0) >> vec6(1) >> vec6(2) >> vec6(3) >> vec6(4) >> vec6(5); double q = sqrt(1.0-vec6(0)*vec6(0)-vec6(1)*vec6(1)-vec6(2)*vec6(2)); g2o::SE3Quat v(Eigen::Quaterniond(q,vec6(0),vec6(1),vec6(2)), Eigen::Vector3d(vec6(3),vec6(4),vec6(5))); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { g2o::SE3Quat v = _measurement; g2o::Vector6 vec6 = v.log(); os << vec6(0) << " " << vec6(1) << " " << vec6(2) << " " << vec6(3) << " " << vec6(4) << " " << vec6(5) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif ================================================ FILE: include/g2o/edge_se3_z.hpp ================================================ // g2o - General Graph Optimization // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard // All rights reserved. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are // met: // // * Redistributions of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // * Redistributions in binary form must reproduce the above copyright // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef EDGE_SE3_Z_HPP #define EDGE_SE3_Z_HPP #include #include namespace Eigen { typedef Eigen::Matrix Vector1d; } namespace g2o { class EdgeSE3Z : public g2o::BaseBinaryEdge<1, Eigen::Vector1d, g2o::VertexSE3, g2o::VertexSE3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3Z() : g2o::BaseBinaryEdge<1, Eigen::Vector1d, g2o::VertexSE3, g2o::VertexSE3>() {} void computeError() override { const g2o::VertexSE3* v1 = static_cast(_vertices[0]); const g2o::VertexSE3* v2 = static_cast(_vertices[1]); Eigen::Vector1d estimate = v2->estimate().translation().tail<1>() - v1->estimate().translation().tail<1>(); _error = estimate - _measurement; } void setMeasurement(const Eigen::Vector1d& m) override { _measurement = m; } virtual bool read(std::istream& is) override { Eigen::Vector1d v; is >> v(0); setMeasurement(v); for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) { is >> information()(i, j); if(i != j) information()(j, i) = information()(i, j); } return true; } virtual bool write(std::ostream& os) const override { Eigen::Vector1d v = _measurement; os << v(0) << " "; for(int i = 0; i < information().rows(); ++i) for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j); return os.good(); } }; } // namespace g2o #endif // EDGE_SE3_PRIORZ_HPP ================================================ FILE: include/g2o/robust_kernel_io.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef ROBUST_KERNEL_IO_HPP #define ROBUST_KERNEL_IO_HPP #include #include namespace g2o { std::string kernel_type(g2o::RobustKernel* kernel); bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph); bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph); } // namespace g2o #endif // ROBUST_KERNEL_IO_HPP ================================================ FILE: include/radar_ego_velocity_estimator.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // (Institute of Control Systems, Karlsruhe Institute of Technology) // RIO 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 3 of the License, or //(at your option) any later version. // RIO 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 RIO. If not, see . #pragma once #include #include #include // #include namespace rio { struct RadarEgoVelocityEstimatorConfig { float min_dist = 1; float max_dist = 400; float min_db = 10; float elevation_thresh_deg = 22.5; float azimuth_thresh_deg = 56.5; float doppler_velocity_correction_factor = 1; float thresh_zero_velocity = 0.05; //Below this is recognized as inlier (m/s) float allowed_outlier_percentage = 0.30; float sigma_zero_velocity_x = 1.0e-03; // default Standard Deviation float sigma_zero_velocity_y = 3.2e-03; float sigma_zero_velocity_z = 1.0e-02; float sigma_offset_radar_x = 0; float sigma_offset_radar_y = 0; float sigma_offset_radar_z = 0; float max_sigma_x = 0.2; float max_sigma_y = 0.2; float max_sigma_z = 0.2; float max_r_cond; bool use_cholesky_instead_of_bdcsvd = true; bool use_ransac = true; float outlier_prob = 0.05; // Outlier Probability, to calculate ransac_iter_ float success_prob = 0.995; float N_ransac_points = 5; float inlier_thresh = 0.5; // err(j) threshold, 0.1 too small, 1.0 too large }; struct RadarEgoVelocityEstimatorIndices { uint x_r = 0; uint y_r = 1; uint z_r = 2; uint snr_db = 3; uint doppler = 4; uint range = 5; uint azimuth = 6; uint elevation = 7; uint normalized_x = 8; uint normalized_y = 9; uint normalized_z = 10; }; class RadarEgoVelocityEstimator { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** * @brief RadarEgoVelocityEstimator constructor */ RadarEgoVelocityEstimator() { setRansacIter(); } /** * @brief Reconfigure callback * @param config has to contain RadarEgoVelocityEstimatorConfig * @return */ template bool configure(ConfigContainingRadarEgoVelocityEstimatorConfig& config); /** * @brief Estimates the radar ego velocity based on a single radar scan * @param[in] radar_scan_msg radar scan * @param[out] v_r estimated radar ego velocity * @param[out] sigma_v_r estimated sigmas of ego velocity * @param[out] inlier_radar_scan inlier point cloud * @returns true if estimation successful */ bool estimate(const sensor_msgs::PointCloud2& radar_scan_msg, Vector3& v_r, Vector3& sigma_v_r); bool estimate(const sensor_msgs::PointCloud2& radar_scan_msg, Vector3& v_r, Vector3& sigma_v_r, sensor_msgs::PointCloud2& inlier_radar_msg, sensor_msgs::PointCloud2& outlier_radar_msg); private: /** * @brief Implementation of the ransac based estimation * @param[in] radar_data matrix of parsed radar scan --> see RadarEgoVelocityEstimatorIndices * @param[out] v_r estimated radar ego velocity * @param[out] sigma_v_r estimated sigmas of ego velocity * @param[out] inlier_idx_best idices of inlier * @returns true if estimation successful */ bool solve3DFullRansac(const Matrix& radar_data, Vector3& v_r, Vector3& sigma_v_r, std::vector& inlier_idx_best, std::vector& outlier_idx_best); /** * @brief Estimates the radar ego velocity using all mesurements provided in radar_data * @param[in] radar_data matrix of parsed radar scan --> see RadarEgoVelocityEstimatorIndices * @param[out] v_r estimated radar ego velocity * @param[out] sigma_v_r estimated sigmas of ego velocity * @param estimate_sigma if true sigma will be estimated as well * @returns true if estimation successful */ bool solve3DFull(const Matrix& radar_data, Vector3& v_r, Vector3& sigma_v_r, bool estimate_sigma = true); /** * @brief Helper function which estiamtes the number of RANSAC iterations */ void setRansacIter() { ransac_iter_ = uint((std::log(1.0 - config_.success_prob)) / std::log(1.0 - std::pow(1.0 - config_.outlier_prob, config_.N_ransac_points))); ROS_INFO_STREAM(kPrefix << "Number of Ransac iterations: " << ransac_iter_); } const std::string kPrefix = "[RadarEgoVelocityEstimator]: "; const RadarEgoVelocityEstimatorIndices idx_; RadarEgoVelocityEstimatorConfig config_; uint ransac_iter_ = 0; }; template bool RadarEgoVelocityEstimator::configure(ConfigContainingRadarEgoVelocityEstimatorConfig& config) { config_.min_dist = config.min_dist; config_.max_dist = config.max_dist; config_.min_db = config.min_db; config_.elevation_thresh_deg = config.elevation_thresh_deg; config_.azimuth_thresh_deg = config.azimuth_thresh_deg; config_.doppler_velocity_correction_factor = config.doppler_velocity_correction_factor; config_.thresh_zero_velocity = config.thresh_zero_velocity; config_.allowed_outlier_percentage = config.allowed_outlier_percentage; config_.sigma_zero_velocity_x = config.sigma_zero_velocity_x; config_.sigma_zero_velocity_y = config.sigma_zero_velocity_y; config_.sigma_zero_velocity_z = config.sigma_zero_velocity_z; config_.sigma_offset_radar_x = config.sigma_offset_radar_x; config_.sigma_offset_radar_y = config.sigma_offset_radar_y; config_.sigma_offset_radar_z = config.sigma_offset_radar_z; config_.max_sigma_x = config.max_sigma_x; config_.max_sigma_y = config.max_sigma_y; config_.max_sigma_z = config.max_sigma_z; config_.max_r_cond = config.max_r_cond; config_.use_cholesky_instead_of_bdcsvd = config.use_cholesky_instead_of_bdcsvd; config_.use_ransac = config.use_ransac; config_.outlier_prob = config.outlier_prob; config_.success_prob = config.success_prob; config_.N_ransac_points = config.N_ransac_points; config_.inlier_thresh = config.inlier_thresh; setRansacIter(); ROS_INFO_STREAM(kPrefix << "Number of Ransac iterations: " << ransac_iter_); return true; } } // namespace rio ================================================ FILE: include/radar_graph_slam/graph_slam.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef GRAPH_SLAM_HPP #define GRAPH_SLAM_HPP #include #include #include #include #include #include #include namespace g2o { class VertexSE3; class VertexPlane; class VertexPointXYZ; class EdgeSE3; class EdgeSE3Plane; class EdgeSE3PointXYZ; class EdgeSE3PriorXY; class EdgeSE3PriorXYZ; class EdgeSE3PriorVec; class EdgeSE3PriorQuat; class EdgePlane; class EdgePlaneIdentity; class EdgePlaneParallel; class EdgePlanePerpendicular; class EdgePlanePriorNormal; class EdgePlanePriorDistance; class RobustKernelFactory; } // namespace g2o namespace radar_graph_slam { class GraphSLAM { public: GraphSLAM(const std::string& solver_type = "lm_var"); virtual ~GraphSLAM(); int num_vertices() const; int num_edges() const; void set_solver(const std::string& solver_type); /** * @brief add a SE3 node to the graph * @param pose * @return registered node */ g2o::VertexSE3* add_se3_node(const Eigen::Isometry3d& pose); /** * @brief add a plane node to the graph * @param plane_coeffs * @return registered node */ g2o::VertexPlane* add_plane_node(const Eigen::Vector4d& plane_coeffs); /** * @brief add a point_xyz node to the graph * @param xyz * @return registered node */ g2o::VertexPointXYZ* add_point_xyz_node(const Eigen::Vector3d& xyz); /** * @brief add an edge between SE3 nodes * @param v1 node1 * @param v2 node2 * @param relative_pose relative pose between node1 and node2 * @param information_matrix information matrix (it must be 6x6) * @return registered edge */ g2o::EdgeSE3* add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3Prior* add_se3_prior_edge(g2o::VertexSE3* v_se3, const Eigen::Isometry3d& pose, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3SE3* add_se3_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const g2o::SE3Quat& relative_pose, const Eigen::MatrixXd& information_matrix); /** * @brief add an edge between an SE3 node and a plane node * @param v_se3 SE3 node * @param v_plane plane node * @param plane_coeffs plane coefficients w.r.t. v_se3 * @param information_matrix information matrix (it must be 3x3) * @return registered edge */ g2o::EdgeSE3Plane* add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix); /** * @brief add an edge between an SE3 node and a point_xyz node * @param v_se3 SE3 node * @param v_xyz point_xyz node * @param xyz xyz coordinate * @param information information_matrix (it must be 3x3) * @return registered edge */ g2o::EdgeSE3PointXYZ* add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); /** * @brief add a prior edge to an SE3 node * @param v_se3 * @param xy * @param information_matrix * @return */ g2o::EdgeSE3GtUTM* add_se3_gt_utm_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& gt_xyz, const Eigen::Vector3d& utm_xyz, const Eigen::MatrixXd& information_matrix); g2o::EdgePlanePriorNormal* add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix); g2o::EdgePlanePriorDistance* add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3PriorZ* add_se3_prior_z_edge(g2o::VertexSE3* v_se3, const Eigen::Vector1d& z, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3Z* add_se3_z_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Vector1d& z, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3PriorQuat* add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix); g2o::EdgeSE3PriorVec* add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix); g2o::EdgePlane* add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information); g2o::EdgePlaneIdentity* add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information); g2o::EdgePlaneParallel* add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information); g2o::EdgePlanePerpendicular* add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information); void add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size); /** * @brief perform graph optimization */ int optimize(int num_iterations); /** * @brief save the pose graph to a file * @param filename output filename */ void save(const std::string& filename); /** * @brief load the pose graph from file * @param filename output filename */ bool load(const std::string& filename); public: g2o::RobustKernelFactory* robust_kernel_factory; std::unique_ptr graph; // g2o graph }; } // namespace radar_graph_slam #endif // GRAPH_SLAM_HPP ================================================ FILE: include/radar_graph_slam/information_matrix_calculator.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef INFORMATION_MATRIX_CALCULATOR_HPP #define INFORMATION_MATRIX_CALCULATOR_HPP #include #include #include namespace radar_graph_slam { class InformationMatrixCalculator { public: using PointT = pcl::PointXYZI; InformationMatrixCalculator() {} InformationMatrixCalculator(ros::NodeHandle& nh); ~InformationMatrixCalculator(); template void load(ParamServer& params) { use_const_inf_matrix = params.template param("use_const_inf_matrix", false); const_stddev_x = params.template param("const_stddev_x", 0.5); const_stddev_q = params.template param("const_stddev_q", 0.1); var_gain_a = params.template param("var_gain_a", 20.0); min_stddev_x = params.template param("min_stddev_x", 0.1); max_stddev_x = params.template param("max_stddev_x", 5.0); min_stddev_q = params.template param("min_stddev_q", 0.05); max_stddev_q = params.template param("max_stddev_q", 0.2); fitness_score_thresh = params.template param("fitness_score_thresh", 2.5); } static double calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range = std::numeric_limits::max()); Eigen::MatrixXd calc_information_matrix(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const; private: double weight(double a, double max_x, double min_y, double max_y, double x) const { double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x)); return min_y + (max_y - min_y) * y; } private: bool use_const_inf_matrix; double const_stddev_x; double const_stddev_q; double var_gain_a; double min_stddev_x; double max_stddev_x; double min_stddev_q; double max_stddev_q; double fitness_score_thresh; }; } // namespace radar_graph_slam #endif // INFORMATION_MATRIX_CALCULATOR_HPP ================================================ FILE: include/radar_graph_slam/keyframe.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef KEYFRAME_HPP #define KEYFRAME_HPP #include #include #include #include #include #include #include namespace g2o { class VertexSE3; class HyperGraph; class SparseOptimizer; } // namespace g2o namespace radar_graph_slam { /** * @brief KeyFrame (pose node) */ struct KeyFrame { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using PointT = pcl::PointXYZI; using Ptr = std::shared_ptr; KeyFrame(const size_t index, const ros::Time& stamp, const Eigen::Isometry3d& odom_scan2scan, double accum_distance, const pcl::PointCloud::ConstPtr& cloud); KeyFrame(const std::string& directory, g2o::HyperGraph* graph); virtual ~KeyFrame(); void save(const std::string& directory); bool load(const std::string& directory, g2o::HyperGraph* graph); long id() const; Eigen::Isometry3d estimate() const; public: size_t index; ros::Time stamp; // timestamp Eigen::Isometry3d odom_scan2scan; // odometry (estimated by scan_matching_odometry) Eigen::Isometry3d odom_scan2map; double accum_distance; // accumulated distance from the first node (by scan_matching_odometry) pcl::PointCloud::ConstPtr cloud; // point cloud boost::optional floor_coeffs; // detected floor's coefficients boost::optional utm_coord; // UTM coord obtained by GPS boost::optional altitude; // Altitude (Filtered) obtained by Barometer boost::optional acceleration; // boost::optional orientation; // geometry_msgs::Transform trans_integrated; // relative transform obtained by imu preintegration boost::optional imu; // the IMU message close to keyframe_ g2o::VertexSE3* node; // node instance }; /** * @brief KeyFramesnapshot for map cloud generation */ struct KeyFrameSnapshot { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using PointT = KeyFrame::PointT; using Ptr = std::shared_ptr; KeyFrameSnapshot(const KeyFrame::Ptr& key); KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud::ConstPtr& cloud); ~KeyFrameSnapshot(); public: Eigen::Isometry3d pose; // pose estimated by graph optimization pcl::PointCloud::ConstPtr cloud; // point cloud }; } // namespace radar_graph_slam #endif // KEYFRAME_HPP ================================================ FILE: include/radar_graph_slam/keyframe_updater.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef KEYFRAME_UPDATER_HPP #define KEYFRAME_UPDATER_HPP #include #include #include namespace radar_graph_slam { /** * @brief this class decides if a new frame should be registered to the pose graph as a keyframe */ class KeyframeUpdater { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** * @brief constructor * @param pnh */ KeyframeUpdater(ros::NodeHandle& pnh) : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()) { keyframe_delta_trans = pnh.param("keyframe_delta_trans", 2.0); keyframe_delta_angle = pnh.param("keyframe_delta_angle", 2.0); // keyframe_delta_time = pnh.param("keyframe_delta_time", 2.0); keyframe_min_size = pnh.param("keyframe_min_size", 1000); accum_distance = 0.0; } /** * @brief decide if a new frame should be registered to the graph * @param pose pose of the frame * @return if true, the frame should be registered */ bool decide(const Eigen::Isometry3d& pose, const ros::Time& stamp) { // first frame is always registered to the graph if(is_first) { is_first = false; prev_keypose = pose; prev_keytime = stamp.toSec(); return true; } // calculate the delta transformation from the previous keyframe Eigen::Isometry3d delta = prev_keypose.inverse() * pose; double dx = delta.translation().norm(); double da = Eigen::AngleAxisd(delta.linear()).angle(); // double dt = stamp.toSec() - prev_keytime; // too close to the previous frame if((dx < keyframe_delta_trans && da < keyframe_delta_angle)) { return false; } accum_distance += dx; prev_keypose = pose; prev_keytime = stamp.toSec(); return true; } /** * @brief the last keyframe's accumulated distance from the first keyframe * @return accumulated distance */ double get_accum_distance() const { return accum_distance; } private: // parameters double keyframe_delta_trans; // double keyframe_delta_angle; // std::size_t keyframe_min_size; // bool is_first; double accum_distance; Eigen::Isometry3d prev_keypose; double prev_keytime; }; } // namespace radar_graph_slam #endif // KEYFRAME_UPDATOR_HPP ================================================ FILE: include/radar_graph_slam/loop_detector.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef LOOP_DETECTOR_HPP #define LOOP_DETECTOR_HPP #include #include #include #include #include #include #include #include #include #include #include using namespace std; namespace radar_graph_slam { struct Loop { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using Ptr = std::shared_ptr; Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose) : key1(key1), key2(key2), relative_pose(relpose) {} public: KeyFrame::Ptr key1; KeyFrame::Ptr key2; Eigen::Matrix4f relative_pose; }; /** * @brief this class finds loops by scam matching and adds them to the pose graph */ class LoopDetector { public: typedef pcl::PointXYZI PointT; /** * @brief constructor * @param pnh */ LoopDetector() {} LoopDetector(ros::NodeHandle& pnh); ~LoopDetector(); std::vector detect(const std::vector& keyframes, const std::deque& new_keyframes, radar_graph_slam::GraphSLAM& graph_slam); double get_distance_thresh() const; Loop::Ptr performScanContextLoopClosure(const std::vector& keyframes, const std::vector& candidate_keyframes, const KeyFrame::Ptr& new_keyframe); private: std::vector find_candidates(const std::vector& keyframes, const KeyFrame::Ptr& new_keyframe) const; cv::Mat getColorImage(cv::Mat &desc); typedef std::tuple Color; std::map _argmax_to_rgb; cv::Mat makeSCImage(Eigen::MatrixXd src_mat); private: double distance_thresh; // estimated distance between keyframes consisting a loop must be less than this distance double accum_distance_thresh; // Minimum distance beteen two edges of the loop double min_loop_interval_dist; // Minimum distance between a new loop edge and the last one double odom_drift_xy; // Odometry drift along X and Y axis double odom_drift_z; // Odometry drift along Z axis double drift_scale_xy; // Odometry drift scale double drift_scale_z; double max_baro_difference; double max_yaw_difference; double fitness_score_max_range; // maximum allowable distance between corresponding points double fitness_score_thresh; // threshold for scan matching double last_loop_edge_accum_distance = 0.0; size_t last_loop_edge_index = 0; pcl::Registration::Ptr registration; public: // Loop closure std::unique_ptr scManager; // loop detector std::unique_ptr inf_calclator; int historyKeyframeSearchNum; float historyKeyframeFitnessScore; std::multimap loopIndexContainer; // from new to old // giseop std::vector> loopIndexQueue; std::vector loopPoseQueue; std::vector loopInfoQueue; std::mutex mtx; double odom_check_trans_thresh; double odom_check_rot_thresh; double pairwise_check_trans_thresh; double pairwise_check_rot_thresh; bool enable_pf; bool enable_odom_check; std::vector pf_time; std::vector sc_time; std::vector oc_time; clock_t start_ms_pf; clock_t end_ms_pf; clock_t start_ms_sc; clock_t end_ms_sc; clock_t start_ms_oc; clock_t end_ms_oc; std::unique_ptr image_transport; image_transport::Publisher pub_cur_sc; image_transport::Publisher pub_pre_sc; }; } // namespace radar_graph_slam #endif // LOOP_DETECTOR_HPP ================================================ FILE: include/radar_graph_slam/map_cloud_generator.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef MAP_CLOUD_GENERATOR_HPP #define MAP_CLOUD_GENERATOR_HPP #include #include #include #include namespace radar_graph_slam { /** * @brief this class generates a map point cloud from registered keyframes */ class MapCloudGenerator { public: using PointT = pcl::PointXYZI; MapCloudGenerator(); ~MapCloudGenerator(); /** * @brief generates a map point cloud * @param keyframes snapshots of keyframes * @param resolution resolution of generated map * @return generated map point cloud */ pcl::PointCloud::Ptr generate(const std::vector& keyframes, double resolution) const; }; } // namespace radar_graph_slam #endif // MAP_POINTCLOUD_GENERATOR_HPP ================================================ FILE: include/radar_graph_slam/nmea_sentence_parser.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef NMEA_SENTENCE_PARSER_HPP #define NMEA_SENTENCE_PARSER_HPP #include #include #include #include #include namespace radar_graph_slam { struct GPRMC { public: GPRMC() { status = 'V'; } GPRMC(const std::vector& tokens) { if(tokens[0] != "$GPRMC" || tokens.size() < 12) { status = 'V'; return; } long time = std::stol(tokens[1]); hour = time / 10000; minute = (time % 10000) / 100; second = time % 100; status = tokens[2][0]; latitude = degmin2deg(std::stod(tokens[3])); latitude = tokens[4] == "N" ? latitude : -latitude; longitude = degmin2deg(std::stod(tokens[5])); longitude = tokens[6] == "E" ? longitude : -longitude; speed_knots = std::stod(tokens[7]); track_angle_degree = std::stod(tokens[8]); long date = std::stol(tokens[9]); year = date % 100; month = (date / 100) % 100; day = (date / 10000) % 100; magnetic_variation = std::stod(tokens[10]); magnetic_variation = tokens[11][0] == 'E' ? magnetic_variation : -magnetic_variation; } double degmin2deg(double degmin) { double d = std::floor(degmin / 100.0); double m = (degmin - d * 100.0) / 60.0; return d + m; } public: char status; // Status A=active or V=Void. int hour; // Fix taken at 12:35:19 UTC int minute; int second; double latitude; // double longitude; double speed_knots; // Speed over the ground in knots double track_angle_degree; // Track angle in degrees True int year; int month; int day; double magnetic_variation; }; class NmeaSentenceParser { public: NmeaSentenceParser() {} ~NmeaSentenceParser() {} GPRMC parse(const std::string& sentence) const { int checksum_loc = sentence.find('*'); if(checksum_loc == std::string::npos) { return GPRMC(); } int checksum = std::stoul(sentence.substr(checksum_loc + 1), nullptr, 16); std::string substr = sentence.substr(1, checksum_loc - 1); int sum = std::accumulate(substr.begin(), substr.end(), static_cast(0), [=](unsigned char n, unsigned char c) { return n ^ c; }); if(checksum != (sum & 0xf)) { std::cerr << "checksum doesn't match!!" << std::endl; std::cerr << sentence << " " << sum << std::endl; return GPRMC(); } std::vector tokens; boost::split(tokens, sentence, boost::is_any_of(",")); return GPRMC(tokens); } }; } // namespace radar_graph_slam #endif // NMEA_SENTENCE_PARSER_HPP ================================================ FILE: include/radar_graph_slam/polynomial_interpolation.hpp ================================================ #include #include class CubicInterpolation { public: Eigen::MatrixXd q_via; Eigen::VectorXd t_via; CubicInterpolation(Eigen::MatrixXd q_via_, Eigen::VectorXd t_via_){ /* :param: name: string name of objective :param: q_via: N x 3 array given q array :param: t_via: N x 1 array given t array */ q_via = q_via_; t_via = t_via_; if(q_via.rows() != t_via.rows()) ROS_ERROR("%s","The q_via and t_via must have a same length"); } virtual ~CubicInterpolation() {} private: int cnt = 0; // void resetIntegration() { // } public: Eigen::Vector4d cubic(double q0, double q1, double v0, double v1, double t0, double t1){ /* :param: q0: float the first data point :param: q1: float the second data point :param: v0: float the velocity of the first data point :param: v1: float the velocity of the second data point :param: t0: float the time of the first data point :param: t1: float the time of the second data point */ if(abs(t0 - t1) < 1e-6) ROS_ERROR("%s","t0 and t1 must be different"); double T = t1 - t0; double h = q1 - q0; double a0 = q0; double a1 = v0; double a2 = (3*h - (2*v0 + v1)*T) / (T*T); double a3 = (-2*h + (v0 + v1)*T) / (T*T*T); return Eigen::Vector4d(a0, a1, a2, a3); } double getPosition(double t){ /* :param: t: float specified time :return: q: float output of the interpolation at time t */ if(t < t_via(0) || t > t_via(t_via.size()-1)) ROS_ERROR("%s","The specific time error, time ranges error"); // j_array = np.where(self.t_via >= t); // find the index of t1 int j;// = j_array[0][0]; for (int i = 0; i < t_via.rows(); i++){ if (t > t_via(i)); else {j=i; break;} } int i; if (j == 0) {i = 0; j = 1;} else i = j-1; // get given position double q0 = q_via(i,0); double v0 = q_via(i,1); double t0 = t_via(i); double q1 = q_via(j,0); double v1 = q_via(j,1); double t1 = t_via(j); Eigen::Vector4d a = cubic(q0, q1, v0, v1, t0, t1); Eigen::Vector3d q; q(0) = a(0) + a(1)*(t - t0) + a(2)*pow(t-t0, 2) + a(3)*pow(t-t0, 3); // position // q(1) = a(1) + 2*a(2)*(t - t0) + 3*a(3)*pow(t-t0, 2); // velocity // q(2) = 2*a(2) + 6*a(3)*(t - t0); // acceleration return q(0); } }; ================================================ FILE: include/radar_graph_slam/registrations.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef HDL_GRAPH_SLAM_REGISTRATIONS_HPP #define HDL_GRAPH_SLAM_REGISTRATIONS_HPP #include #include namespace radar_graph_slam { /** * @brief select a scan matching algorithm according to rosparams * @param pnh * @return selected scan matching */ pcl::Registration::Ptr select_registration_method(ros::NodeHandle& pnh); } // namespace radar_graph_slam #endif // ================================================ FILE: include/radar_graph_slam/ros_time_hash.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef ROS_TIME_HASH_HPP #define ROS_TIME_HASH_HPP #include #include #include /** * @brief Hash calculation for ros::Time */ class RosTimeHash { public: size_t operator()(const ros::Time& val) const { size_t seed = 0; boost::hash_combine(seed, val.sec); boost::hash_combine(seed, val.nsec); return seed; } }; #endif // ROS_TIME_HASHER_HPP ================================================ FILE: include/radar_graph_slam/ros_utils.hpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #ifndef ROS_UTILS_HPP #define ROS_UTILS_HPP #include #include #include #include #include #include namespace radar_graph_slam { extern const int imuQueLength = 200; extern const float scanPeriod = 0.083333; double restrict_rad(double rad){ double out; if (rad < -M_PI/2) out = rad + M_PI; else if (rad > M_PI/2) out = rad - M_PI; else out = rad; return out; } Eigen::Vector3d R2ypr(const Eigen::Matrix3d& R) { Eigen::Vector3d n = R.col(0); Eigen::Vector3d o = R.col(1); Eigen::Vector3d a = R.col(2); Eigen::Vector3d ypr(3); double y = atan2(n(1), n(0)); double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); ypr(0) = y; ypr(1) = p; ypr(2) = r; return ypr; } /** * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped * @param stamp timestamp * @param pose Eigen::Matrix to be converted * @param frame_id tf frame_id * @param child_frame_id tf child frame_id * @return converted TransformStamped */ static geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4d& pose, const std::string& frame_id, const std::string& child_frame_id) { Eigen::Quaterniond quat(pose.block<3, 3>(0, 0)); quat.normalize(); geometry_msgs::Quaternion odom_quat; odom_quat.w = quat.w(); odom_quat.x = quat.x(); odom_quat.y = quat.y(); odom_quat.z = quat.z(); geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = stamp; odom_trans.header.frame_id = frame_id; odom_trans.child_frame_id = child_frame_id; odom_trans.transform.translation.x = pose(0, 3); odom_trans.transform.translation.y = pose(1, 3); odom_trans.transform.translation.z = pose(2, 3); odom_trans.transform.rotation = odom_quat; return odom_trans; } static nav_msgs::Odometry matrix2odom(const ros::Time& stamp, const Eigen::Matrix4d& pose, const std::string& frame_id, const std::string& child_frame_id){ Eigen::Quaterniond quat(pose.block<3, 3>(0, 0)); quat.normalize(); geometry_msgs::Quaternion odom_quat; odom_quat.w = quat.w(); odom_quat.x = quat.x(); odom_quat.y = quat.y(); odom_quat.z = quat.z(); nav_msgs::Odometry odom; odom.header.stamp = stamp; odom.header.frame_id = frame_id; odom.child_frame_id = child_frame_id; odom.pose.pose.position.x = pose(0, 3); odom.pose.pose.position.y = pose(1, 3); odom.pose.pose.position.z = pose(2, 3); odom.pose.pose.orientation = odom_quat; return odom; } static Eigen::Isometry3d pose2isometry(const geometry_msgs::Pose& pose) { Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); mat.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); mat.linear() = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z).toRotationMatrix(); return mat; } static Eigen::Isometry3d transform2isometry(const geometry_msgs::Transform& transf) { Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); mat.translation() = Eigen::Vector3d(transf.translation.x, transf.translation.y, transf.translation.z); mat.linear() = Eigen::Quaterniond(transf.rotation.w, transf.rotation.x, transf.rotation.y, transf.rotation.z).toRotationMatrix(); return mat; } static Eigen::Isometry3d tf2isometry(const tf::StampedTransform& trans) { Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); mat.translation() = Eigen::Vector3d(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z()); mat.linear() = Eigen::Quaterniond(trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()).toRotationMatrix(); return mat; } static Eigen::Isometry3d quaternion2isometry(const Eigen::Quaterniond& orient) { Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); mat.linear() = Eigen::Quaterniond(orient.w(), orient.x(), orient.y(), orient.z()).toRotationMatrix(); return mat; } static geometry_msgs::Pose isometry2pose(const Eigen::Isometry3d& mat) { Eigen::Quaterniond quat(mat.linear()); Eigen::Vector3d trans = mat.translation(); geometry_msgs::Pose pose; pose.position.x = trans.x(); pose.position.y = trans.y(); pose.position.z = trans.z(); pose.orientation.w = quat.w(); pose.orientation.x = quat.x(); pose.orientation.y = quat.y(); pose.orientation.z = quat.z(); return pose; } static nav_msgs::Odometry isometry2odom(const ros::Time& stamp, const Eigen::Isometry3d& mat, const std::string& frame_id, const std::string& child_frame_id) { Eigen::Quaterniond quat(mat.linear()); Eigen::Vector3d trans = mat.translation(); nav_msgs::Odometry odom; odom.header.stamp = stamp; odom.header.frame_id = frame_id; odom.child_frame_id = child_frame_id; odom.pose.pose.position.x = trans.x(); odom.pose.pose.position.y = trans.y(); odom.pose.pose.position.z = trans.z(); odom.pose.pose.orientation.w = quat.w(); odom.pose.pose.orientation.x = quat.x(); odom.pose.pose.orientation.y = quat.y(); odom.pose.pose.orientation.z = quat.z(); return odom; } static Eigen::Isometry3d odom2isometry(const nav_msgs::OdometryConstPtr& odom_msg) { const auto& orientation = odom_msg->pose.pose.orientation; const auto& position = odom_msg->pose.pose.position; Eigen::Quaterniond quat; quat.w() = orientation.w; quat.x() = orientation.x; quat.y() = orientation.y; quat.z() = orientation.z; Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity(); isometry.linear() = quat.toRotationMatrix(); isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z); return isometry; } } // namespace radar_graph_slam #endif // ROS_UTILS_HPP ================================================ FILE: include/rio_utils/data_types.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // 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 3 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, see . #pragma once #include #include #include #include #include #include #include namespace rio { typedef double Real; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Vector4d Vector4; typedef Eigen::Matrix Vector7; typedef Eigen::Matrix Vector11; typedef Eigen::VectorXd Vector; typedef Eigen::Matrix2d Matrix2; typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix4d Matrix4; typedef Eigen::MatrixXd Matrix; typedef Eigen::Quaterniond Quaternion; typedef Eigen::Isometry3d Isometry; typedef Eigen::AngleAxisd AngleAxis; struct EulerAngles : public Vector3 { EulerAngles() : Vector3(0, 0, 0) {} EulerAngles(const Real roll, const Real pitch, const Real yaw) : Vector3(roll, pitch, yaw) {} EulerAngles(const Vector3& eul_n_b) : Vector3(eul_n_b) {} EulerAngles from_degrees(const Vector3& eul_rad) { x() = angles::from_degrees(eul_rad.x()); y() = angles::from_degrees(eul_rad.y()); z() = angles::from_degrees(eul_rad.z()); return EulerAngles(x(), y(), z()); } Vector3 to_degrees() { return Vector3(angles::to_degrees(x()), angles::to_degrees(y()), angles::to_degrees(z())); } Real& roll() { return Vector3::x(); } Real roll() const { return Vector3::x(); } Real& pitch() { return Vector3::y(); } Real pitch() const { return Vector3::y(); } Real& yaw() { return Vector3::z(); } Real yaw() const { return Vector3::z(); } }; struct NavigationSolution { NavigationSolution() { pose_n_b.linear().setIdentity(); pose_n_b.translation().setZero(); v_n_b.setZero(); } NavigationSolution(const Isometry& pose_n_b, const Vector3& v_n_b) : pose_n_b{pose_n_b}, v_n_b{v_n_b} {} NavigationSolution(const Vector3& p_n_b, const Quaternion& q_n_b, const Vector3 v_n_b) : v_n_b{v_n_b} { setQuaternion(q_n_b); setPosition_n_b(p_n_b); } Vector3 getPosition_n_b() const { return pose_n_b.translation(); } void setPosition_n_b(const Vector3& p_n_b) { pose_n_b.translation() = p_n_b; } Quaternion getQuaternion_n_b() const { return Quaternion(pose_n_b.linear()); } void setQuaternion(const Quaternion& q_n_b) { pose_n_b.linear() = q_n_b.normalized().toRotationMatrix(); } EulerAngles getEuler_n_b() const { const Quaternion q = getQuaternion_n_b(); tf2::Quaternion q_msg(q.x(), q.y(), q.z(), q.w()); tf2::Matrix3x3 R(q_msg); EulerAngles eul; R.getEulerYPR(eul.yaw(), eul.pitch(), eul.roll()); return eul; } void setEuler_n_b(const EulerAngles& eul_n_b) { tf2::Quaternion q_n_b; q_n_b.setRPY(eul_n_b.roll(), eul_n_b.pitch(), eul_n_b.yaw()); pose_n_b.linear() = Matrix3(Quaternion(q_n_b.w(), q_n_b.x(), q_n_b.y(), q_n_b.z())); } Matrix3 getC_n_b() const { return pose_n_b.linear(); } Isometry getPoseRos() const { tf2::Quaternion q_n_b; q_n_b.setRPY(M_PI, 0, 0); return Matrix3(Quaternion(q_n_b.w(), q_n_b.x(), q_n_b.y(), q_n_b.z())) * pose_n_b; } Vector3 getVelocityRos() const { return Vector3(v_n_b.x(), -v_n_b.y(), -v_n_b.z()); } Isometry pose_n_b; // position and attitude in navigation frame Vector3 v_n_b; // [m/s] EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; struct ImuData { ImuData() : dt{0} {} ImuData(const double dt, const Vector3& a_b_ib, const Vector3& w_b_ib) : dt{dt}, a_b_ib{a_b_ib}, w_b_ib{w_b_ib} {} Real dt; // [s] Vector3 a_b_ib; // [m/s^2] Vector3 w_b_ib; // [rad/s] EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; struct ImuDataStamped { ImuDataStamped() : dt{0} {} ImuDataStamped(const ros::Time& time_stamp, const std::string frame_id, const double dt, const Vector3& a_b_ib, const Vector3& w_b_ib) : time_stamp{time_stamp}, frame_id{frame_id}, dt{dt}, a_b_ib{a_b_ib}, w_b_ib{w_b_ib} { } ImuDataStamped(const sensor_msgs::ImuConstPtr& imu_msg, const Real dt) : time_stamp{imu_msg->header.stamp}, frame_id{imu_msg->header.frame_id}, dt{dt}, a_b_ib{Vector3(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z)}, w_b_ib{Vector3(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z)} { } sensor_msgs::Imu toImuMsg() { sensor_msgs::Imu imu_msg; imu_msg.header.stamp = time_stamp; imu_msg.angular_velocity.x = w_b_ib.x(); imu_msg.angular_velocity.y = w_b_ib.y(); imu_msg.angular_velocity.z = w_b_ib.z(); imu_msg.linear_acceleration.x = a_b_ib.x(); imu_msg.linear_acceleration.y = a_b_ib.y(); imu_msg.linear_acceleration.z = a_b_ib.z(); return imu_msg; } ros::Time time_stamp; // ros::Time std::string frame_id; // frame id Real dt; // [s] Vector3 a_b_ib; // [m/s^2] Vector3 w_b_ib; // [rad/s] EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace rio ================================================ FILE: include/rio_utils/math_helper.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // 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 3 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, see . #pragma once #include #include namespace rio { namespace math_helper { /** * @brief Calculates the skew matrix from a 3D vector */ static Matrix3 skewVec(const Vector3& v) { Matrix3 S; S << 0, -v.z(), v.y(), v.z(), 0, -v.x(), -v.y(), v.x(), 0; return S; } /** * @brief Performs a quaternion multiplication following the Hamilton convention */ static Quaternion quaternionMultiplicationHamilton(const Quaternion& q, const Quaternion& p) { const Vector3 q_v(q.x(), q.y(), q.z()); const Vector3 p_v(p.x(), p.y(), p.z()); const Real q_p_w = p.w() * q.w() - q_v.transpose() * p_v; const Vector3 q_p_ = q.w() * p_v + p.w() * q_v + skewVec(q_v) * p_v; return Quaternion(q_p_w, q_p_.x(), q_p_.y(), q_p_.z()); } /** * @brief Wraps a scalar (e.g. an angle) to the intervall [0, max] */ static Real wrapToPositive(const Real d, const Real max = 360) { const auto m = std::fmod(d, max); if (m < 0) return m + max; else return m; } /** * @brief Wraps a scalar (e.g. an angle) to the intervall [-abs_max, abs_max] */ static Real wrapToCentered(const Real d, const Real abs_max = 180) { return wrapToPositive(d + abs_max, abs_max * 2) - abs_max; } /** * @brief Converts from Cartesian to Spherical coordinates * @param[in] v Cartesian vector * @param [out] r range * @param [out] azi azimuth * @param [out] ele elevation */ static void cartesianToSpherical(const Vector3& v, Real& r, Real& azi, Real& ele) { r = v.norm(); azi = std::atan2(v.y(), v.x()); ele = std::atan2(std::sqrt(v.x() * v.x() + v.y() * v.y()), v.z()); } /** * @brief The ConvolveType enum */ enum class ConvolveType { VALID, // consider only where both vectors overlap completely FULL // consider each point of overlap }; /** * @brief Convolution of v with k * @param[in] v vector to be convolved * @param[in] k convolution kernel * @param[in] type convolution type --> see ConvolveType * @param[out] c convolution result * @returns true, if result valid */ static bool convolve(const Vector v, const Vector k, const ConvolveType type, Vector& c) { if (k.size() < v.size()) { if (type == ConvolveType::FULL) { c = Vector::Zero(v.size() + k.size() - 1, 1); for (uint i = 0; i < c.size(); ++i) { if (i < k.size() - 1) c[i] = v.head(i + 1).transpose() * k.head(i + 1).reverse(); else if (i > v.size() - 1) c[i] = v.tail(c.size() - i).transpose() * k.tail(c.size() - i).reverse(); else c[i] = v.segment(i - k.size() + 1, k.size()).transpose() * k.reverse(); } return true; } else { c = Vector::Zero(v.size() - k.size() + 1, 1); for (uint i = 0; i < c.size(); ++i) c[i] = v.segment(i, k.size()).transpose() * k.reverse(); return true; } } return false; } } // namespace math_helper } // namespace rio ================================================ FILE: include/rio_utils/radar_point_cloud.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // 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 3 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, see . #pragma once #include #include #include #include #include #include #include namespace rio { // bool pcl2msgToPcl(const sensor_msgs::PointCloud2& pcl_msg, pcl::PointCloud& scan); } // namespace rio struct RadarPointCloudType { PCL_ADD_POINT4D // x,y,z position in [m] PCL_ADD_INTENSITY; union { struct { float doppler; }; float data_c[4]; }; EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 确保定义新类型点云内存与SSE对齐 } EIGEN_ALIGN16; // 强制SSE填充以正确对齐内存 POINT_CLOUD_REGISTER_POINT_STRUCT ( RadarPointCloudType, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, doppler, doppler) ) struct EaglePointXYZIVRAB { float x; float y; float z; float snr_db; float doppler; float range; float alpha; float beta; }; POINT_CLOUD_REGISTER_POINT_STRUCT ( EaglePointXYZIVRAB, (float, x, x) (float, y, y) (float, z, z) (float, snr_db, snr_db) (float, doppler, doppler) (float, range, range) (float, alpha, alpha) (float, beta, beta) ) ================================================ FILE: include/rio_utils/ros_helper.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // 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 3 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, see . #pragma once #include namespace rio { enum class RosParameterType { Required, Recommended, Optional }; template static bool getRosParameter(const ros::NodeHandle& nh, const std::string kPrefix, const RosParameterType& param_type, const std::string& param_name, T& param) { if (!nh.getParam(param_name, param)) { if (param_type == RosParameterType::Optional) { ROS_INFO_STREAM(kPrefix << "<" << param_name << "> is optional but not configured. Using default value: " << param); nh.setParam(param_name, param); } else if (param_type == RosParameterType::Recommended) { ROS_WARN_STREAM(kPrefix << "<" << param_name << "> is recommeded but not configured. Using default value: " << param); nh.setParam(param_name, param); } else { ROS_ERROR_STREAM(kPrefix << "<" << param_name << "> is required but not configured. Exiting!"); return false; } } return true; } } // namespace rio ================================================ FILE: include/rio_utils/simple_profiler.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // 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 3 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, see . #pragma once #include #include #include #include #include #include #include namespace rio { struct RuntimeStatistics { float min_ms = 0.0f; float max_ms = 0.0f; float mean_ms = 0.0f; float total_ms = 0.0f; std::string toStringMs() const { std::stringstream ss; ss << std::fixed << std::setprecision(2) << "mean_ms=" << mean_ms << ", max_ms=" << max_ms << ", min_ms=" << min_ms << ", total_s=" << total_ms; return ss.str(); } std::string toStringUs() const { std::stringstream ss; ss << std::fixed << std::setprecision(2) << "mean_us=" << mean_ms * 1.0e3f << ", max_us=" << max_ms * 1.0e3f << ", min_us=" << min_ms * 1.0e3f << ", total_us=" << total_ms * 1.0e3f; return ss.str(); } }; struct ProfileData { int id = -1; RuntimeStatistics statistics; std::vector execution_ms = {}; }; /** * @brief The SimpleProfiler class provides a lighweight tool for runtime evaluation */ class SimpleProfiler { public: /** * @brief SimpleProfiler constructor * @param is_on enables runtime measurements */ SimpleProfiler(const bool is_on = true); /** * @brief Starts a runtime measurement of the provided key */ void start(const std::string& key); /** * @brief Stops a runtime measurement of the provided key * @returns true if successful (=the measurement for the given key was started) */ bool stop(const std::string& key); /** * @brief Stops a runtime measurement of the provided key returning the runtime in [ms] */ float stopWithRuntimeMs(const std::string& key); /** * @brief Gets the runtime statistics (=mean, max, min) runtimes for this key */ RuntimeStatistics getStatistics(const std::string& key); /** * @brief Returns a summary of all runtime measurements as string * @param indent size of whitespace indet */ std::string toString(const uint indent = 0); /** * @brief Returns a summary of all runtime measurements as string formatted to a markdown table */ std::string toMarkdownTable(); /** * @brief Returns the sum of all stopped runtimes */ float getTotalRuntime(); private: /** * @brief Helper function to calculate the runtime statistics for a given key */ RuntimeStatistics calculateProfileStatistics(const std::string& key); const std::string kPrefix; bool is_on_; int next_id_; std::map start_times_; std::unordered_map profile_data_; }; } // namespace rio ================================================ FILE: include/rio_utils/strapdown.h ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // 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 3 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, see . #pragma once #include #include "rio_utils/data_types.h" namespace rio { /** * @brief The Strapdown class implements a strapdown which propagates a kinematic state using an IMU measurement * @note Uses the North East Down (NED) convention */ class Strapdown { public: /** * @brief Strapdown constructor * @param local_gravity local gravity using the NED convention */ Strapdown(const double local_gravity = 9.80665); /** * @brief Propagates the given navigation solution using an IMU measurement * @param nav_sol_prev previous navigation solution * @param a_b_ib measured body frame acceleration * @param w_b_ib measured body frame angular velocity * @param dt propagation time * @return */ NavigationSolution propagate(const NavigationSolution nav_sol_prev, const Vector3 a_b_ib, const Vector3 w_b_ib, const Real dt); private: Eigen::Matrix4d getQLeftMatrix(const Vector4& v); Vector3 local_gravity_; }; } // namespace rio ================================================ FILE: include/scan_context/KDTreeVectorOfVectorsAdaptor.h ================================================ /*********************************************************************** * Software License Agreement (BSD License) * * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *************************************************************************/ #pragma once #include "nanoflann.hpp" #include // ===== This example shows how to use nanoflann with these types of containers: ======= //typedef std::vector > my_vector_of_vectors_t; //typedef std::vector my_vector_of_vectors_t; // This requires #include // ===================================================================================== /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. * The i'th vector represents a point in the state space. * * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. * \tparam num_t The type of the point coordinates (typically, double or float). * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) */ template struct KDTreeVectorOfVectorsAdaptor { typedef KDTreeVectorOfVectorsAdaptor self_t; typedef typename Distance::template traits::distance_t metric_t; typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. /// Constructor: takes a const ref to the vector of vectors object with the data points KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) { assert(mat.size() != 0 && mat[0].size() != 0); const size_t dims = mat[0].size(); if (DIM>0 && static_cast(dims) != DIM) throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); index->buildIndex(); } ~KDTreeVectorOfVectorsAdaptor() { delete index; } const VectorOfVectorsType &m_data; /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). * Note that this is a short-cut method for index->findNeighbors(). * The user can also call index->... methods as desired. * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. */ inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); } /** @name Interface expected by KDTreeSingleIndexAdaptor * @{ */ const self_t & derived() const { return *this; } self_t & derived() { return *this; } // Must return the number of data points inline size_t kdtree_get_point_count() const { return m_data.size(); } // Returns the dim'th component of the idx'th point in the class: inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { return m_data[idx][dim]; } // Optional bounding-box computation: return false to default to a standard bbox computation loop. // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) template bool kdtree_get_bbox(BBOX & /*bb*/) const { return false; } /** @} */ }; // end of KDTreeVectorOfVectorsAdaptor ================================================ FILE: include/scan_context/Scancontext.h ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "nanoflann.hpp" #include "KDTreeVectorOfVectorsAdaptor.h" #include "tictoc.h" #include using namespace Eigen; using namespace nanoflann; using std::cout; using std::endl; using std::make_pair; using std::atan2; using std::cos; using std::sin; using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) using KeyMat = std::vector >; using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; struct PointXYZIRPYT { PCL_ADD_POINT4D PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding float roll; float pitch; float yaw; double time; EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) (double, time, time)) // giseop enum class SCInputType { SINGLE_SCAN_FULL, SINGLE_SCAN_FEAT, MULTI_SCAN_FEAT }; // namespace SC2 // { void coreImportTest ( void ); // sc param-independent helper functions float xy2theta( const float & _x, const float & _y ); MatrixXd circshift( MatrixXd &_mat, int _num_shift ); std::vector eig2stdvec( MatrixXd _eigmat ); class SCManager { public: SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. void setScDistThresh(double thresh); void setAzimuthRange(double range); Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) // User-side API void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); std::pair detectLoopClosureID( const std::vector& candidate_keyframes, const radar_graph_slam::KeyFrame::Ptr& new_keyframe ); // int: nearest node index, float: relative yaw // for ltmapper const Eigen::MatrixXd& getConstRefRecentSCD(void); public: // hyper parameters () const double LIDAR_HEIGHT = 1.2; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. double PC_AZIMUTH_ANGLE_MAX = 56.5; double PC_AZIMUTH_ANGLE_MIN = -56.6; const int PC_NUM_RING = 40; // 20 in the original paper (IROS 18) const int PC_NUM_SECTOR = 20; // 60 in the original paper (IROS 18) const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) double PC_UNIT_SECTOR_ANGLE = (PC_AZIMUTH_ANGLE_MAX - PC_AZIMUTH_ANGLE_MIN) / double(PC_NUM_SECTOR); // 360.0 / double(PC_NUM_SECTOR); const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); // tree const int NUM_EXCLUDE_RECENT = 10;//30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok. const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper) // loop thres const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver. // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) double SC_DIST_THRES = 0.5; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15 // config const int TREE_MAKING_PERIOD_ = 10; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N. int tree_making_period_conter = 0; // data std::vector polarcontexts_timestamp_; // optional. std::vector polarcontexts_; std::vector polarcontext_invkeys_; std::vector polarcontext_vkeys_; KeyMat polarcontext_invkeys_mat_; KeyMat polarcontext_invkeys_to_search_; std::unique_ptr polarcontext_tree_; }; // SCManager // } // namespace SC2 ================================================ FILE: include/scan_context/nanoflann.hpp ================================================ /*********************************************************************** * Software License Agreement (BSD License) * * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com). * All rights reserved. * * THE BSD LICENSE * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *************************************************************************/ /** \mainpage nanoflann C++ API documentation * nanoflann is a C++ header-only library for building KD-Trees, mostly * optimized for 2D or 3D point clouds. * * nanoflann does not require compiling or installing, just an * #include in your code. * * See: * - C++ API organized by modules * - Online README * - Doxygen * documentation */ #ifndef NANOFLANN_HPP_ #define NANOFLANN_HPP_ #include #include #include #include // for abs() #include // for fwrite() #include // for abs() #include #include // std::reference_wrapper #include #include /** Library version: 0xMmP (M=Major,m=minor,P=patch) */ #define NANOFLANN_VERSION 0x132 // Avoid conflicting declaration of min/max macros in windows headers #if !defined(NOMINMAX) && \ (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) #define NOMINMAX #ifdef max #undef max #undef min #endif #endif namespace nanoflann { /** @addtogroup nanoflann_grp nanoflann C++ library for ANN * @{ */ /** the PI constant (required to avoid MSVC missing symbols) */ template T pi_const() { return static_cast(3.14159265358979323846); } /** * Traits if object is resizable and assignable (typically has a resize | assign * method) */ template struct has_resize : std::false_type {}; template struct has_resize().resize(1), 0)> : std::true_type {}; template struct has_assign : std::false_type {}; template struct has_assign().assign(1, 0), 0)> : std::true_type {}; /** * Free function to resize a resizable object */ template inline typename std::enable_if::value, void>::type resize(Container &c, const size_t nElements) { c.resize(nElements); } /** * Free function that has no effects on non resizable containers (e.g. * std::array) It raises an exception if the expected size does not match */ template inline typename std::enable_if::value, void>::type resize(Container &c, const size_t nElements) { if (nElements != c.size()) throw std::logic_error("Try to change the size of a std::array."); } /** * Free function to assign to a container */ template inline typename std::enable_if::value, void>::type assign(Container &c, const size_t nElements, const T &value) { c.assign(nElements, value); } /** * Free function to assign to a std::array */ template inline typename std::enable_if::value, void>::type assign(Container &c, const size_t nElements, const T &value) { for (size_t i = 0; i < nElements; i++) c[i] = value; } /** @addtogroup result_sets_grp Result set classes * @{ */ template class KNNResultSet { public: typedef _DistanceType DistanceType; typedef _IndexType IndexType; typedef _CountType CountType; private: IndexType *indices; DistanceType *dists; CountType capacity; CountType count; public: inline KNNResultSet(CountType capacity_) : indices(0), dists(0), capacity(capacity_), count(0) {} inline void init(IndexType *indices_, DistanceType *dists_) { indices = indices_; dists = dists_; count = 0; if (capacity) dists[capacity - 1] = (std::numeric_limits::max)(); } inline CountType size() const { return count; } inline bool full() const { return count == capacity; } /** * Called during search to add an element matching the criteria. * @return true if the search should be continued, false if the results are * sufficient */ inline bool addPoint(DistanceType dist, IndexType index) { CountType i; for (i = count; i > 0; --i) { #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same // distance, the one with the lowest-index will be // returned first. if ((dists[i - 1] > dist) || ((dist == dists[i - 1]) && (indices[i - 1] > index))) { #else if (dists[i - 1] > dist) { #endif if (i < capacity) { dists[i] = dists[i - 1]; indices[i] = indices[i - 1]; } } else break; } if (i < capacity) { dists[i] = dist; indices[i] = index; } if (count < capacity) count++; // tell caller that the search shall continue return true; } inline DistanceType worstDist() const { return dists[capacity - 1]; } }; /** operator "<" for std::sort() */ struct IndexDist_Sorter { /** PairType will be typically: std::pair */ template inline bool operator()(const PairType &p1, const PairType &p2) const { return p1.second < p2.second; } }; /** * A result-set class used when performing a radius based search. */ template class RadiusResultSet { public: typedef _DistanceType DistanceType; typedef _IndexType IndexType; public: const DistanceType radius; std::vector> &m_indices_dists; inline RadiusResultSet( DistanceType radius_, std::vector> &indices_dists) : radius(radius_), m_indices_dists(indices_dists) { init(); } inline void init() { clear(); } inline void clear() { m_indices_dists.clear(); } inline size_t size() const { return m_indices_dists.size(); } inline bool full() const { return true; } /** * Called during search to add an element matching the criteria. * @return true if the search should be continued, false if the results are * sufficient */ inline bool addPoint(DistanceType dist, IndexType index) { if (dist < radius) m_indices_dists.push_back(std::make_pair(index, dist)); return true; } inline DistanceType worstDist() const { return radius; } /** * Find the worst result (furtherest neighbor) without copying or sorting * Pre-conditions: size() > 0 */ std::pair worst_item() const { if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " "an empty list of results."); typedef typename std::vector>::const_iterator DistIt; DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); return *it; } }; /** @} */ /** @addtogroup loadsave_grp Load/save auxiliary functions * @{ */ template void save_value(FILE *stream, const T &value, size_t count = 1) { fwrite(&value, sizeof(value), count, stream); } template void save_value(FILE *stream, const std::vector &value) { size_t size = value.size(); fwrite(&size, sizeof(size_t), 1, stream); fwrite(&value[0], sizeof(T), size, stream); } template void load_value(FILE *stream, T &value, size_t count = 1) { size_t read_cnt = fread(&value, sizeof(value), count, stream); if (read_cnt != count) { throw std::runtime_error("Cannot read from file"); } } template void load_value(FILE *stream, std::vector &value) { size_t size; size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); if (read_cnt != 1) { throw std::runtime_error("Cannot read from file"); } value.resize(size); read_cnt = fread(&value[0], sizeof(T), size, stream); if (read_cnt != size) { throw std::runtime_error("Cannot read from file"); } } /** @} */ /** @addtogroup metric_grp Metric (distance) classes * @{ */ struct Metric {}; /** Manhattan distance functor (generic version, optimized for * high-dimensionality data sets). Corresponding distance traits: * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) * (e.g. float, double, int64_t) */ template struct L1_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const { DistanceType result = DistanceType(); const T *last = a + size; const T *lastgroup = last - 3; size_t d = 0; /* Process 4 items with each loop for efficiency. */ while (a < lastgroup) { const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); result += diff0 + diff1 + diff2 + diff3; a += 4; if ((worst_dist > 0) && (result > worst_dist)) { return result; } } /* Process last 0-3 components. Not needed for standard vector lengths. */ while (a < last) { result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); } return result; } template inline DistanceType accum_dist(const U a, const V b, const size_t) const { return std::abs(a - b); } }; /** Squared Euclidean distance functor (generic version, optimized for * high-dimensionality data sets). Corresponding distance traits: * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) * (e.g. float, double, int64_t) */ template struct L2_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const { DistanceType result = DistanceType(); const T *last = a + size; const T *lastgroup = last - 3; size_t d = 0; /* Process 4 items with each loop for efficiency. */ while (a < lastgroup) { const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; a += 4; if ((worst_dist > 0) && (result > worst_dist)) { return result; } } /* Process last 0-3 components. Not needed for standard vector lengths. */ while (a < last) { const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); result += diff0 * diff0; } return result; } template inline DistanceType accum_dist(const U a, const V b, const size_t) const { return (a - b) * (a - b); } }; /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality * datasets, like 2D or 3D point clouds) Corresponding distance traits: * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, * float, uint8_t) \tparam _DistanceType Type of distance variables (must be * signed) (e.g. float, double, int64_t) */ template struct L2_Simple_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size) const { DistanceType result = DistanceType(); for (size_t i = 0; i < size; ++i) { const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); result += diff * diff; } return result; } template inline DistanceType accum_dist(const U a, const V b, const size_t) const { return (a - b) * (a - b); } }; /** SO2 distance functor * Corresponding distance traits: nanoflann::metric_SO2 * \tparam T Type of the elements (e.g. double, float) * \tparam _DistanceType Type of distance variables (must be signed) (e.g. * float, double) orientation is constrained to be in [-pi, pi] */ template struct SO2_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; const DataSource &data_source; SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size) const { return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1); } /** Note: this assumes that input angles are already in the range [-pi,pi] */ template inline DistanceType accum_dist(const U a, const V b, const size_t) const { DistanceType result = DistanceType(); DistanceType PI = pi_const(); result = b - a; if (result > PI) result -= 2 * PI; else if (result < -PI) result += 2 * PI; return result; } }; /** SO3 distance functor (Uses L2_Simple) * Corresponding distance traits: nanoflann::metric_SO3 * \tparam T Type of the elements (e.g. double, float) * \tparam _DistanceType Type of distance variables (must be signed) (e.g. * float, double) */ template struct SO3_Adaptor { typedef T ElementType; typedef _DistanceType DistanceType; L2_Simple_Adaptor distance_L2_Simple; SO3_Adaptor(const DataSource &_data_source) : distance_L2_Simple(_data_source) {} inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size) const { return distance_L2_Simple.evalMetric(a, b_idx, size); } template inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { return distance_L2_Simple.accum_dist(a, b, idx); } }; /** Metaprogramming helper traits class for the L1 (Manhattan) metric */ struct metric_L1 : public Metric { template struct traits { typedef L1_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the L2 (Euclidean) metric */ struct metric_L2 : public Metric { template struct traits { typedef L2_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ struct metric_L2_Simple : public Metric { template struct traits { typedef L2_Simple_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ struct metric_SO2 : public Metric { template struct traits { typedef SO2_Adaptor distance_t; }; }; /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ struct metric_SO3 : public Metric { template struct traits { typedef SO3_Adaptor distance_t; }; }; /** @} */ /** @addtogroup param_grp Parameter structs * @{ */ /** Parameters (see README.md) */ struct KDTreeSingleIndexAdaptorParams { KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) : leaf_max_size(_leaf_max_size) {} size_t leaf_max_size; }; /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ struct SearchParams { /** Note: The first argument (checks_IGNORED_) is ignored, but kept for * compatibility with the FLANN interface */ SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} int checks; //!< Ignored parameter (Kept for compatibility with the FLANN //!< interface). float eps; //!< search for eps-approximate neighbours (default: 0) bool sorted; //!< only for radius search, require neighbours sorted by //!< distance (default: true) }; /** @} */ /** @addtogroup memalloc_grp Memory allocation * @{ */ /** * Allocates (using C's malloc) a generic type T. * * Params: * count = number of instances to allocate. * Returns: pointer (of type T*) to memory buffer */ template inline T *allocate(size_t count = 1) { T *mem = static_cast(::malloc(sizeof(T) * count)); return mem; } /** * Pooled storage allocator * * The following routines allow for the efficient allocation of storage in * small chunks from a specified pool. Rather than allowing each structure * to be freed individually, an entire pool of storage is freed at once. * This method has two advantages over just using malloc() and free(). First, * it is far more efficient for allocating small objects, as there is * no overhead for remembering all the information needed to free each * object or consolidating fragmented memory. Second, the decision about * how long to keep an object is made at the time of allocation, and there * is no need to track down all the objects to free them. * */ const size_t WORDSIZE = 16; const size_t BLOCKSIZE = 8192; class PooledAllocator { /* We maintain memory alignment to word boundaries by requiring that all allocations be in multiples of the machine wordsize. */ /* Size of machine word in bytes. Must be power of 2. */ /* Minimum number of bytes requested at a time from the system. Must be * multiple of WORDSIZE. */ size_t remaining; /* Number of bytes left in current block of storage. */ void *base; /* Pointer to base of current block of storage. */ void *loc; /* Current location in block to next allocate memory. */ void internal_init() { remaining = 0; base = NULL; usedMemory = 0; wastedMemory = 0; } public: size_t usedMemory; size_t wastedMemory; /** Default constructor. Initializes a new pool. */ PooledAllocator() { internal_init(); } /** * Destructor. Frees all the memory allocated in this pool. */ ~PooledAllocator() { free_all(); } /** Frees all allocated memory chunks */ void free_all() { while (base != NULL) { void *prev = *(static_cast(base)); /* Get pointer to prev block. */ ::free(base); base = prev; } internal_init(); } /** * Returns a pointer to a piece of new memory of the given size in bytes * allocated from the pool. */ void *malloc(const size_t req_size) { /* Round size up to a multiple of wordsize. The following expression only works for WORDSIZE that is a power of 2, by masking last bits of incremented size to zero. */ const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); /* Check whether a new block must be allocated. Note that the first word of a block is reserved for a pointer to the previous block. */ if (size > remaining) { wastedMemory += remaining; /* Allocate new storage. */ const size_t blocksize = (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) ? size + sizeof(void *) + (WORDSIZE - 1) : BLOCKSIZE; // use the standard C malloc to allocate memory void *m = ::malloc(blocksize); if (!m) { fprintf(stderr, "Failed to allocate memory.\n"); return NULL; } /* Fill first word of new block with pointer to previous block. */ static_cast(m)[0] = base; base = m; size_t shift = 0; // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & // (WORDSIZE-1))) & (WORDSIZE-1); remaining = blocksize - sizeof(void *) - shift; loc = (static_cast(m) + sizeof(void *) + shift); } void *rloc = loc; loc = static_cast(loc) + size; remaining -= size; usedMemory += size; return rloc; } /** * Allocates (using this pool) a generic type T. * * Params: * count = number of instances to allocate. * Returns: pointer (of type T*) to memory buffer */ template T *allocate(const size_t count = 1) { T *mem = static_cast(this->malloc(sizeof(T) * count)); return mem; } }; /** @} */ /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff * @{ */ /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors * when DIM=-1. Fixed size version for a generic DIM: */ template struct array_or_vector_selector { typedef std::array container_t; }; /** Dynamic size version */ template struct array_or_vector_selector<-1, T> { typedef std::vector container_t; }; /** @} */ /** kd-tree base-class * * Contains the member functions common to the classes KDTreeSingleIndexAdaptor * and KDTreeSingleIndexDynamicAdaptor_. * * \tparam Derived The name of the class which inherits this class. * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use, these are all classes derived * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for * 3D points) \tparam IndexType Will be typically size_t or int */ template class KDTreeBaseClass { public: /** Frees the previously-built index. Automatically called within * buildIndex(). */ void freeIndex(Derived &obj) { obj.pool.free_all(); obj.root_node = NULL; obj.m_size_at_index_build = 0; } typedef typename Distance::ElementType ElementType; typedef typename Distance::DistanceType DistanceType; /*--------------------- Internal Data Structures --------------------------*/ struct Node { /** Union used because a node can be either a LEAF node or a non-leaf node, * so both data fields are never used simultaneously */ union { struct leaf { IndexType left, right; //!< Indices of points in leaf node } lr; struct nonleaf { int divfeat; //!< Dimension used for subdivision. DistanceType divlow, divhigh; //!< The values used for subdivision. } sub; } node_type; Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) }; typedef Node *NodePtr; struct Interval { ElementType low, high; }; /** * Array of indices to vectors in the dataset. */ std::vector vind; NodePtr root_node; size_t m_leaf_max_size; size_t m_size; //!< Number of current points in the dataset size_t m_size_at_index_build; //!< Number of points in the dataset when the //!< index was built int dim; //!< Dimensionality of each data point /** Define "BoundingBox" as a fixed-size or variable-size container depending * on "DIM" */ typedef typename array_or_vector_selector::container_t BoundingBox; /** Define "distance_vector_t" as a fixed-size or variable-size container * depending on "DIM" */ typedef typename array_or_vector_selector::container_t distance_vector_t; /** The KD-tree used to find neighbours */ BoundingBox root_bbox; /** * Pooled memory allocator. * * Using a pooled memory allocator is more efficient * than allocating memory directly when there is a large * number small of memory allocations. */ PooledAllocator pool; /** Returns number of points in dataset */ size_t size(const Derived &obj) const { return obj.m_size; } /** Returns the length of each point in the dataset */ size_t veclen(const Derived &obj) { return static_cast(DIM > 0 ? DIM : obj.dim); } /// Helper accessor to the dataset points: inline ElementType dataset_get(const Derived &obj, size_t idx, int component) const { return obj.dataset.kdtree_get_pt(idx, component); } /** * Computes the inde memory usage * Returns: memory used by the index */ size_t usedMemory(Derived &obj) { return obj.pool.usedMemory + obj.pool.wastedMemory + obj.dataset.kdtree_get_point_count() * sizeof(IndexType); // pool memory and vind array memory } void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, int element, ElementType &min_elem, ElementType &max_elem) { min_elem = dataset_get(obj, ind[0], element); max_elem = dataset_get(obj, ind[0], element); for (IndexType i = 1; i < count; ++i) { ElementType val = dataset_get(obj, ind[i], element); if (val < min_elem) min_elem = val; if (val > max_elem) max_elem = val; } } /** * Create a tree node that subdivides the list of vecs from vind[first] * to vind[last]. The routine is called recursively on each sublist. * * @param left index of the first vector * @param right index of the last vector */ NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, BoundingBox &bbox) { NodePtr node = obj.pool.template allocate(); // allocate memory /* If too few exemplars remain, then make this a leaf node. */ if ((right - left) <= static_cast(obj.m_leaf_max_size)) { node->child1 = node->child2 = NULL; /* Mark as leaf node. */ node->node_type.lr.left = left; node->node_type.lr.right = right; // compute bounding-box of leaf points for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { bbox[i].low = dataset_get(obj, obj.vind[left], i); bbox[i].high = dataset_get(obj, obj.vind[left], i); } for (IndexType k = left + 1; k < right; ++k) { for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) bbox[i].low = dataset_get(obj, obj.vind[k], i); if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) bbox[i].high = dataset_get(obj, obj.vind[k], i); } } } else { IndexType idx; int cutfeat; DistanceType cutval; middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, bbox); node->node_type.sub.divfeat = cutfeat; BoundingBox left_bbox(bbox); left_bbox[cutfeat].high = cutval; node->child1 = divideTree(obj, left, left + idx, left_bbox); BoundingBox right_bbox(bbox); right_bbox[cutfeat].low = cutval; node->child2 = divideTree(obj, left + idx, right, right_bbox); node->node_type.sub.divlow = left_bbox[cutfeat].high; node->node_type.sub.divhigh = right_bbox[cutfeat].low; for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); } } return node; } void middleSplit_(Derived &obj, IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox) { const DistanceType EPS = static_cast(0.00001); ElementType max_span = bbox[0].high - bbox[0].low; for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { ElementType span = bbox[i].high - bbox[i].low; if (span > max_span) { max_span = span; } } ElementType max_spread = -1; cutfeat = 0; for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { ElementType span = bbox[i].high - bbox[i].low; if (span > (1 - EPS) * max_span) { ElementType min_elem, max_elem; computeMinMax(obj, ind, count, i, min_elem, max_elem); ElementType spread = max_elem - min_elem; ; if (spread > max_spread) { cutfeat = i; max_spread = spread; } } } // split in the middle DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; ElementType min_elem, max_elem; computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); if (split_val < min_elem) cutval = min_elem; else if (split_val > max_elem) cutval = max_elem; else cutval = split_val; IndexType lim1, lim2; planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); if (lim1 > count / 2) index = lim1; else if (lim2 < count / 2) index = lim2; else index = count / 2; } /** * Subdivide the list of points by a plane perpendicular on axe corresponding * to the 'cutfeat' dimension at 'cutval' position. * * On return: * dataset[ind[0..lim1-1]][cutfeat]cutval */ void planeSplit(Derived &obj, IndexType *ind, const IndexType count, int cutfeat, DistanceType &cutval, IndexType &lim1, IndexType &lim2) { /* Move vector indices for left subtree to front of list. */ IndexType left = 0; IndexType right = count - 1; for (;;) { while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) ++left; while (right && left <= right && dataset_get(obj, ind[right], cutfeat) >= cutval) --right; if (left > right || !right) break; // "!right" was added to support unsigned Index types std::swap(ind[left], ind[right]); ++left; --right; } /* If either list is empty, it means that all remaining features * are identical. Split in the middle to maintain a balanced tree. */ lim1 = left; right = count - 1; for (;;) { while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) ++left; while (right && left <= right && dataset_get(obj, ind[right], cutfeat) > cutval) --right; if (left > right || !right) break; // "!right" was added to support unsigned Index types std::swap(ind[left], ind[right]); ++left; --right; } lim2 = left; } DistanceType computeInitialDistances(const Derived &obj, const ElementType *vec, distance_vector_t &dists) const { assert(vec); DistanceType distsq = DistanceType(); for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { if (vec[i] < obj.root_bbox[i].low) { dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); distsq += dists[i]; } if (vec[i] > obj.root_bbox[i].high) { dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); distsq += dists[i]; } } return distsq; } void save_tree(Derived &obj, FILE *stream, NodePtr tree) { save_value(stream, *tree); if (tree->child1 != NULL) { save_tree(obj, stream, tree->child1); } if (tree->child2 != NULL) { save_tree(obj, stream, tree->child2); } } void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { tree = obj.pool.template allocate(); load_value(stream, *tree); if (tree->child1 != NULL) { load_tree(obj, stream, tree->child1); } if (tree->child2 != NULL) { load_tree(obj, stream, tree->child2); } } /** Stores the index in a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when * loading the index object it must be constructed associated to the same * source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ void saveIndex_(Derived &obj, FILE *stream) { save_value(stream, obj.m_size); save_value(stream, obj.dim); save_value(stream, obj.root_bbox); save_value(stream, obj.m_leaf_max_size); save_value(stream, obj.vind); save_tree(obj, stream, obj.root_node); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the * index object must be constructed associated to the same source of data * points used while building the index. See the example: * examples/saveload_example.cpp \sa loadIndex */ void loadIndex_(Derived &obj, FILE *stream) { load_value(stream, obj.m_size); load_value(stream, obj.dim); load_value(stream, obj.root_bbox); load_value(stream, obj.m_leaf_max_size); load_value(stream, obj.vind); load_tree(obj, stream, obj.root_node); } }; /** @addtogroup kdtrees_grp KD-tree classes and adaptors * @{ */ /** kd-tree static index * * Contains the k-d trees and other information for indexing a set of points * for nearest-neighbor matching. * * The class "DatasetAdaptor" must provide the following interface (can be * non-virtual, inlined methods): * * \code * // Must return the number of data poins * inline size_t kdtree_get_point_count() const { ... } * * * // Must return the dim'th component of the idx'th point in the class: * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } * * // Optional bounding-box computation: return false to default to a standard * bbox computation loop. * // Return true if the BBOX was already computed by the class and returned * in "bb" so it can be avoided to redo it again. * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const * { * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits * ... * return true; * } * * \endcode * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ template class KDTreeSingleIndexAdaptor : public KDTreeBaseClass< KDTreeSingleIndexAdaptor, Distance, DatasetAdaptor, DIM, IndexType> { public: /** Deleted copy constructor*/ KDTreeSingleIndexAdaptor( const KDTreeSingleIndexAdaptor &) = delete; /** * The dataset used by this index */ const DatasetAdaptor &dataset; //!< The source of our data const KDTreeSingleIndexAdaptorParams index_params; Distance distance; typedef typename nanoflann::KDTreeBaseClass< nanoflann::KDTreeSingleIndexAdaptor, Distance, DatasetAdaptor, DIM, IndexType> BaseClassRef; typedef typename BaseClassRef::ElementType ElementType; typedef typename BaseClassRef::DistanceType DistanceType; typedef typename BaseClassRef::Node Node; typedef Node *NodePtr; typedef typename BaseClassRef::Interval Interval; /** Define "BoundingBox" as a fixed-size or variable-size container depending * on "DIM" */ typedef typename BaseClassRef::BoundingBox BoundingBox; /** Define "distance_vector_t" as a fixed-size or variable-size container * depending on "DIM" */ typedef typename BaseClassRef::distance_vector_t distance_vector_t; /** * KDTree constructor * * Refer to docs in README.md or online in * https://github.com/jlblancoc/nanoflann * * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 * for 3D points) is determined by means of: * - The \a DIM template parameter if >0 (highest priority) * - Otherwise, the \a dimensionality parameter of this constructor. * * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms = KDTreeSingleIndexAdaptorParams()) : dataset(inputData), index_params(params), distance(inputData) { BaseClassRef::root_node = NULL; BaseClassRef::m_size = dataset.kdtree_get_point_count(); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; BaseClassRef::dim = dimensionality; if (DIM > 0) BaseClassRef::dim = DIM; BaseClassRef::m_leaf_max_size = params.leaf_max_size; // Create a permutable array of indices to the input vectors. init_vind(); } /** * Builds the index */ void buildIndex() { BaseClassRef::m_size = dataset.kdtree_get_point_count(); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; init_vind(); this->freeIndex(*this); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; if (BaseClassRef::m_size == 0) return; computeBoundingBox(BaseClassRef::root_bbox); BaseClassRef::root_node = this->divideTree(*this, 0, BaseClassRef::m_size, BaseClassRef::root_bbox); // construct the tree } /** \name Query methods * @{ */ /** * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored * inside the result object. * * Params: * result = the result object in which the indices of the * nearest-neighbors are stored vec = the vector for which to search the * nearest neighbors * * \tparam RESULTSET Should be any ResultSet * \return True if the requested neighbors could be found. * \sa knnSearch, radiusSearch */ template bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const { assert(vec); if (this->size(*this) == 0) return false; if (!BaseClassRef::root_node) throw std::runtime_error( "[nanoflann] findNeighbors() called before building the index."); float epsError = 1 + searchParams.eps; distance_vector_t dists; // fixed or variable-sized container (depending on DIM) auto zero = static_cast(0); assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), zero); // Fill it with zeros. DistanceType distsq = this->computeInitialDistances(*this, vec, dists); searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither // used nor returned to the user. return result.full(); } /** * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. * Their indices are stored inside the result object. \sa radiusSearch, * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility * with the original FLANN interface. \return Number `N` of valid points in * the result set. Only the first `N` entries in `out_indices` and * `out_distances_sq` will be valid. Return may be less than `num_closest` * only if the number of elements in the tree is less than `num_closest`. */ size_t knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); return resultSet.size(); } /** * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. * The output is given as a vector of pairs, of which the first element is a * point index and the second the corresponding distance. Previous contents of * \a IndicesDists are cleared. * * If searchParams.sorted==true, the output list is sorted by ascending * distances. * * For a better performance, it is advisable to do a .reserve() on the vector * if you have any wild guess about the number of expected matches. * * \sa knnSearch, findNeighbors, radiusSearchCustomCallback * \return The number of points within the given radius (i.e. indices.size() * or dists.size() ) */ size_t radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector> &IndicesDists, const SearchParams &searchParams) const { RadiusResultSet resultSet(radius, IndicesDists); const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); if (searchParams.sorted) std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); return nFound; } /** * Just like radiusSearch() but with a custom callback class for each point * found in the radius of the query. See the source of RadiusResultSet<> as a * start point for your own classes. \sa radiusSearch */ template size_t radiusSearchCustomCallback( const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams = SearchParams()) const { this->findNeighbors(resultSet, query_point, searchParams); return resultSet.size(); } /** @} */ public: /** Make sure the auxiliary list \a vind has the same size than the current * dataset, and re-generate if size has changed. */ void init_vind() { // Create a permutable array of indices to the input vectors. BaseClassRef::m_size = dataset.kdtree_get_point_count(); if (BaseClassRef::vind.size() != BaseClassRef::m_size) BaseClassRef::vind.resize(BaseClassRef::m_size); for (size_t i = 0; i < BaseClassRef::m_size; i++) BaseClassRef::vind[i] = i; } void computeBoundingBox(BoundingBox &bbox) { resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dataset.kdtree_get_bbox(bbox)) { // Done! It was implemented in derived class } else { const size_t N = dataset.kdtree_get_point_count(); if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but " "no data points found."); for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); } for (size_t k = 1; k < N; ++k) { for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { if (this->dataset_get(*this, k, i) < bbox[i].low) bbox[i].low = this->dataset_get(*this, k, i); if (this->dataset_get(*this, k, i) > bbox[i].high) bbox[i].high = this->dataset_get(*this, k, i); } } } } /** * Performs an exact search in the tree starting from a node. * \tparam RESULTSET Should be any ResultSet * \return true if the search should be continued, false if the results are * sufficient */ template bool searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const { /* If this is a leaf node, then do check and return. */ if ((node->child1 == NULL) && (node->child2 == NULL)) { // count_leaf += (node->lr.right-node->lr.left); // Removed since was // neither used nor returned to the user. DistanceType worst_dist = result_set.worstDist(); for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { const IndexType index = BaseClassRef::vind[i]; // reorder... : i; DistanceType dist = distance.evalMetric( vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dist < worst_dist) { if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { // the resultset doesn't want to receive any more points, we're done // searching! return false; } } } return true; } /* Which child branch should be taken first? */ int idx = node->node_type.sub.divfeat; ElementType val = vec[idx]; DistanceType diff1 = val - node->node_type.sub.divlow; DistanceType diff2 = val - node->node_type.sub.divhigh; NodePtr bestChild; NodePtr otherChild; DistanceType cut_dist; if ((diff1 + diff2) < 0) { bestChild = node->child1; otherChild = node->child2; cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); } else { bestChild = node->child2; otherChild = node->child1; cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); } /* Call recursively to search next level down. */ if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { // the resultset doesn't want to receive any more points, we're done // searching! return false; } DistanceType dst = dists[idx]; mindistsq = mindistsq + cut_dist - dst; dists[idx] = cut_dist; if (mindistsq * epsError <= result_set.worstDist()) { if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError)) { // the resultset doesn't want to receive any more points, we're done // searching! return false; } } dists[idx] = dst; return true; } public: /** Stores the index in a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when * loading the index object it must be constructed associated to the same * source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the * index object must be constructed associated to the same source of data * points used while building the index. See the example: * examples/saveload_example.cpp \sa loadIndex */ void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } }; // class KDTree /** kd-tree dynamic index * * Contains the k-d trees and other information for indexing a set of points * for nearest-neighbor matching. * * The class "DatasetAdaptor" must provide the following interface (can be * non-virtual, inlined methods): * * \code * // Must return the number of data poins * inline size_t kdtree_get_point_count() const { ... } * * // Must return the dim'th component of the idx'th point in the class: * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } * * // Optional bounding-box computation: return false to default to a standard * bbox computation loop. * // Return true if the BBOX was already computed by the class and returned * in "bb" so it can be avoided to redo it again. * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const * { * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits * ... * return true; * } * * \endcode * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ template class KDTreeSingleIndexDynamicAdaptor_ : public KDTreeBaseClass, Distance, DatasetAdaptor, DIM, IndexType> { public: /** * The dataset used by this index */ const DatasetAdaptor &dataset; //!< The source of our data KDTreeSingleIndexAdaptorParams index_params; std::vector &treeIndex; Distance distance; typedef typename nanoflann::KDTreeBaseClass< nanoflann::KDTreeSingleIndexDynamicAdaptor_, Distance, DatasetAdaptor, DIM, IndexType> BaseClassRef; typedef typename BaseClassRef::ElementType ElementType; typedef typename BaseClassRef::DistanceType DistanceType; typedef typename BaseClassRef::Node Node; typedef Node *NodePtr; typedef typename BaseClassRef::Interval Interval; /** Define "BoundingBox" as a fixed-size or variable-size container depending * on "DIM" */ typedef typename BaseClassRef::BoundingBox BoundingBox; /** Define "distance_vector_t" as a fixed-size or variable-size container * depending on "DIM" */ typedef typename BaseClassRef::distance_vector_t distance_vector_t; /** * KDTree constructor * * Refer to docs in README.md or online in * https://github.com/jlblancoc/nanoflann * * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 * for 3D points) is determined by means of: * - The \a DIM template parameter if >0 (highest priority) * - Otherwise, the \a dimensionality parameter of this constructor. * * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ KDTreeSingleIndexDynamicAdaptor_( const int dimensionality, const DatasetAdaptor &inputData, std::vector &treeIndex_, const KDTreeSingleIndexAdaptorParams ¶ms = KDTreeSingleIndexAdaptorParams()) : dataset(inputData), index_params(params), treeIndex(treeIndex_), distance(inputData) { BaseClassRef::root_node = NULL; BaseClassRef::m_size = 0; BaseClassRef::m_size_at_index_build = 0; BaseClassRef::dim = dimensionality; if (DIM > 0) BaseClassRef::dim = DIM; BaseClassRef::m_leaf_max_size = params.leaf_max_size; } /** Assignment operator definiton */ KDTreeSingleIndexDynamicAdaptor_ operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); std::swap(index_params, tmp.index_params); std::swap(treeIndex, tmp.treeIndex); std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); std::swap(BaseClassRef::m_size_at_index_build, tmp.BaseClassRef::m_size_at_index_build); std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); return *this; } /** * Builds the index */ void buildIndex() { BaseClassRef::m_size = BaseClassRef::vind.size(); this->freeIndex(*this); BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; if (BaseClassRef::m_size == 0) return; computeBoundingBox(BaseClassRef::root_bbox); BaseClassRef::root_node = this->divideTree(*this, 0, BaseClassRef::m_size, BaseClassRef::root_bbox); // construct the tree } /** \name Query methods * @{ */ /** * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored * inside the result object. * * Params: * result = the result object in which the indices of the * nearest-neighbors are stored vec = the vector for which to search the * nearest neighbors * * \tparam RESULTSET Should be any ResultSet * \return True if the requested neighbors could be found. * \sa knnSearch, radiusSearch */ template bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const { assert(vec); if (this->size(*this) == 0) return false; if (!BaseClassRef::root_node) return false; float epsError = 1 + searchParams.eps; // fixed or variable-sized container (depending on DIM) distance_vector_t dists; // Fill it with zeros. assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), static_cast(0)); DistanceType distsq = this->computeInitialDistances(*this, vec, dists); searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, epsError); // "count_leaf" parameter removed since was neither // used nor returned to the user. return result.full(); } /** * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. * Their indices are stored inside the result object. \sa radiusSearch, * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility * with the original FLANN interface. \return Number `N` of valid points in * the result set. Only the first `N` entries in `out_indices` and * `out_distances_sq` will be valid. Return may be less than `num_closest` * only if the number of elements in the tree is less than `num_closest`. */ size_t knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); return resultSet.size(); } /** * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. * The output is given as a vector of pairs, of which the first element is a * point index and the second the corresponding distance. Previous contents of * \a IndicesDists are cleared. * * If searchParams.sorted==true, the output list is sorted by ascending * distances. * * For a better performance, it is advisable to do a .reserve() on the vector * if you have any wild guess about the number of expected matches. * * \sa knnSearch, findNeighbors, radiusSearchCustomCallback * \return The number of points within the given radius (i.e. indices.size() * or dists.size() ) */ size_t radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector> &IndicesDists, const SearchParams &searchParams) const { RadiusResultSet resultSet(radius, IndicesDists); const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); if (searchParams.sorted) std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); return nFound; } /** * Just like radiusSearch() but with a custom callback class for each point * found in the radius of the query. See the source of RadiusResultSet<> as a * start point for your own classes. \sa radiusSearch */ template size_t radiusSearchCustomCallback( const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams = SearchParams()) const { this->findNeighbors(resultSet, query_point, searchParams); return resultSet.size(); } /** @} */ public: void computeBoundingBox(BoundingBox &bbox) { resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dataset.kdtree_get_bbox(bbox)) { // Done! It was implemented in derived class } else { const size_t N = BaseClassRef::m_size; if (!N) throw std::runtime_error("[nanoflann] computeBoundingBox() called but " "no data points found."); for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { bbox[i].low = bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[0], i); } for (size_t k = 1; k < N; ++k) { for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); } } } } /** * Performs an exact search in the tree starting from a node. * \tparam RESULTSET Should be any ResultSet */ template void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const { /* If this is a leaf node, then do check and return. */ if ((node->child1 == NULL) && (node->child2 == NULL)) { // count_leaf += (node->lr.right-node->lr.left); // Removed since was // neither used nor returned to the user. DistanceType worst_dist = result_set.worstDist(); for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { const IndexType index = BaseClassRef::vind[i]; // reorder... : i; if (treeIndex[index] == -1) continue; DistanceType dist = distance.evalMetric( vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); if (dist < worst_dist) { if (!result_set.addPoint( static_cast(dist), static_cast( BaseClassRef::vind[i]))) { // the resultset doesn't want to receive any more points, we're done // searching! return; // false; } } } return; } /* Which child branch should be taken first? */ int idx = node->node_type.sub.divfeat; ElementType val = vec[idx]; DistanceType diff1 = val - node->node_type.sub.divlow; DistanceType diff2 = val - node->node_type.sub.divhigh; NodePtr bestChild; NodePtr otherChild; DistanceType cut_dist; if ((diff1 + diff2) < 0) { bestChild = node->child1; otherChild = node->child2; cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); } else { bestChild = node->child2; otherChild = node->child1; cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); } /* Call recursively to search next level down. */ searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); DistanceType dst = dists[idx]; mindistsq = mindistsq + cut_dist - dst; dists[idx] = cut_dist; if (mindistsq * epsError <= result_set.worstDist()) { searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); } dists[idx] = dst; } public: /** Stores the index in a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when * loading the index object it must be constructed associated to the same * source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the * index object must be constructed associated to the same source of data * points used while building the index. See the example: * examples/saveload_example.cpp \sa loadIndex */ void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } }; /** kd-tree dynaimic index * * class to create multiple static index and merge their results to behave as * single dynamic index as proposed in Logarithmic Approach. * * Example of usage: * examples/dynamic_pointcloud_example.cpp * * \tparam DatasetAdaptor The user-provided adaptor (see comments above). * \tparam Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ template class KDTreeSingleIndexDynamicAdaptor { public: typedef typename Distance::ElementType ElementType; typedef typename Distance::DistanceType DistanceType; protected: size_t m_leaf_max_size; size_t treeCount; size_t pointCount; /** * The dataset used by this index */ const DatasetAdaptor &dataset; //!< The source of our data std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which //!< point at idx is stored. treeIndex[idx]=-1 //!< means that point has been removed. KDTreeSingleIndexAdaptorParams index_params; int dim; //!< Dimensionality of each data point typedef KDTreeSingleIndexDynamicAdaptor_ index_container_t; std::vector index; public: /** Get a const ref to the internal list of indices; the number of indices is * adapted dynamically as the dataset grows in size. */ const std::vector &getAllIndices() const { return index; } private: /** finds position of least significant unset bit */ int First0Bit(IndexType num) { int pos = 0; while (num & 1) { num = num >> 1; pos++; } return pos; } /** Creates multiple empty trees to handle dynamic support */ void init() { typedef KDTreeSingleIndexDynamicAdaptor_ my_kd_tree_t; std::vector index_( treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); index = index_; } public: Distance distance; /** * KDTree constructor * * Refer to docs in README.md or online in * https://github.com/jlblancoc/nanoflann * * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 * for 3D points) is determined by means of: * - The \a DIM template parameter if >0 (highest priority) * - Otherwise, the \a dimensionality parameter of this constructor. * * @param inputData Dataset with the input features * @param params Basically, the maximum leaf node size */ KDTreeSingleIndexDynamicAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms = KDTreeSingleIndexAdaptorParams(), const size_t maximumPointCount = 1000000000U) : dataset(inputData), index_params(params), distance(inputData) { treeCount = static_cast(std::log2(maximumPointCount)); pointCount = 0U; dim = dimensionality; treeIndex.clear(); if (DIM > 0) dim = DIM; m_leaf_max_size = params.leaf_max_size; init(); const size_t num_initial_points = dataset.kdtree_get_point_count(); if (num_initial_points > 0) { addPoints(0, num_initial_points - 1); } } /** Deleted copy constructor*/ KDTreeSingleIndexDynamicAdaptor( const KDTreeSingleIndexDynamicAdaptor &) = delete; /** Add points to the set, Inserts all points from [start, end] */ void addPoints(IndexType start, IndexType end) { size_t count = end - start + 1; treeIndex.resize(treeIndex.size() + count); for (IndexType idx = start; idx <= end; idx++) { int pos = First0Bit(pointCount); index[pos].vind.clear(); treeIndex[pointCount] = pos; for (int i = 0; i < pos; i++) { for (int j = 0; j < static_cast(index[i].vind.size()); j++) { index[pos].vind.push_back(index[i].vind[j]); if (treeIndex[index[i].vind[j]] != -1) treeIndex[index[i].vind[j]] = pos; } index[i].vind.clear(); index[i].freeIndex(index[i]); } index[pos].vind.push_back(idx); index[pos].buildIndex(); pointCount++; } } /** Remove a point from the set (Lazy Deletion) */ void removePoint(size_t idx) { if (idx >= pointCount) return; treeIndex[idx] = -1; } /** * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored * inside the result object. * * Params: * result = the result object in which the indices of the * nearest-neighbors are stored vec = the vector for which to search the * nearest neighbors * * \tparam RESULTSET Should be any ResultSet * \return True if the requested neighbors could be found. * \sa knnSearch, radiusSearch */ template bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const { for (size_t i = 0; i < treeCount; i++) { index[i].findNeighbors(result, &vec[0], searchParams); } return result.full(); } }; /** An L2-metric KD-tree adaptor for working with data directly stored in an * Eigen Matrix, without duplicating the data storage. Each row in the matrix * represents a point in the state space. * * Example of usage: * \code * Eigen::Matrix mat; * // Fill out "mat"... * * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf * ); mat_index.index->buildIndex(); mat_index.index->... \endcode * * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality * for the points in the data set, allowing more compiler optimizations. \tparam * Distance The distance metric to use: nanoflann::metric_L1, * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. */ template struct KDTreeEigenMatrixAdaptor { typedef KDTreeEigenMatrixAdaptor self_t; typedef typename MatrixType::Scalar num_t; typedef typename MatrixType::Index IndexType; typedef typename Distance::template traits::distance_t metric_t; typedef KDTreeSingleIndexAdaptor index_t; index_t *index; //! The kd-tree index for the user to call its methods as //! usual with any other FLANN index. /// Constructor: takes a const ref to the matrix object with the data points KDTreeEigenMatrixAdaptor(const size_t dimensionality, const std::reference_wrapper &mat, const int leaf_max_size = 10) : m_data_matrix(mat) { const auto dims = mat.get().cols(); if (size_t(dims) != dimensionality) throw std::runtime_error( "Error: 'dimensionality' must match column count in data matrix"); if (DIM > 0 && int(dims) != DIM) throw std::runtime_error( "Data set dimensionality does not match the 'DIM' template argument"); index = new index_t(static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); index->buildIndex(); } public: /** Deleted copy constructor */ KDTreeEigenMatrixAdaptor(const self_t &) = delete; ~KDTreeEigenMatrixAdaptor() { delete index; } const std::reference_wrapper m_data_matrix; /** Query for the \a num_closest closest points to a given point (entered as * query_point[0:dim-1]). Note that this is a short-cut method for * index->findNeighbors(). The user can also call index->... methods as * desired. \note nChecks_IGNORED is ignored but kept for compatibility with * the original FLANN interface. */ inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int /* nChecks_IGNORED */ = 10) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); } /** @name Interface expected by KDTreeSingleIndexAdaptor * @{ */ const self_t &derived() const { return *this; } self_t &derived() { return *this; } // Must return the number of data points inline size_t kdtree_get_point_count() const { return m_data_matrix.get().rows(); } // Returns the dim'th component of the idx'th point in the class: inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { return m_data_matrix.get().coeff(idx, IndexType(dim)); } // Optional bounding-box computation: return false to default to a standard // bbox computation loop. // Return true if the BBOX was already computed by the class and returned in // "bb" so it can be avoided to redo it again. Look at bb.size() to find out // the expected dimensionality (e.g. 2 or 3 for point clouds) template bool kdtree_get_bbox(BBOX & /*bb*/) const { return false; } /** @} */ }; // end of KDTreeEigenMatrixAdaptor /** @} */ /** @} */ // end of grouping } // namespace nanoflann #endif /* NANOFLANN_HPP_ */ ================================================ FILE: include/scan_context/tictoc.h ================================================ // Author: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk #pragma once #include #include #include #include #include class TicToc { public: TicToc() { tic(); } TicToc( bool _disp ) { disp_ = _disp; tic(); } void tic() { start = std::chrono::system_clock::now(); } void toc( std::string _about_task ) { end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; double elapsed_ms = elapsed_seconds.count() * 1000; if( disp_ ) { std::cout.precision(3); // 10 for sec, 3 for ms std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; } } private: std::chrono::time_point start, end; bool disp_ = false; }; ================================================ FILE: include/utility_radar.h ================================================ #pragma once #ifndef _UTILITY_RADAR_ODOMETRY_H_ #define _UTILITY_RADAR_ODOMETRY_H_ #include #include #include #include #include #include #include #include #include #include // #include // #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; typedef pcl::PointXYZI PointType; class ParamServer { public: ros::NodeHandle nh; std::string robot_id; //Topics string pointCloudTopic; string imuTopic; string odomTopic; string gpsTopic; //Frames string lidarFrame; string baselinkFrame; string odometryFrame; string mapFrame; // GPS Settings bool useGpsElevation; float gpsCovThreshold; float poseCovThreshold; // Save pcd bool savePCD; string savePCDDirectory; // Sensor Configuration: int downsampleRate; float lidarMinRange; float lidarMaxRange; // IMU float imuAccNoise; float imuGyrNoise; float imuAccBiasN; float imuGyrBiasN; float imuGravity; float imuRPYWeight; vector extRotV; vector extRPYV; vector extTransV; Eigen::Matrix3d extRot; Eigen::Matrix3d extRPY; Eigen::Vector3d extTrans; Eigen::Quaterniond extQRPY; // voxel filter paprams float odometrySurfLeafSize; float mappingCornerLeafSize; float mappingSurfLeafSize ; // CPU Params int numberOfCores; double mappingProcessInterval; // Surrounding map float surroundingkeyframeAddingDistThreshold; float surroundingkeyframeAddingAngleThreshold; float surroundingKeyframeDensity; float surroundingKeyframeSearchRadius; // Loop closure bool loopClosureEnableFlag; float loopClosureFrequency; int surroundingKeyframeSize; float historyKeyframeSearchRadius; float historyKeyframeSearchTimeDiff; int historyKeyframeSearchNum; float historyKeyframeFitnessScore; // global map visualization radius float globalMapVisualizationSearchRadius; float globalMapVisualizationPoseDensity; float globalMapVisualizationLeafSize; ParamServer() { nh.param("/robot_id", robot_id, "nuc_handcart"); nh.param("radar_slam/pointCloudTopic", pointCloudTopic, "/radar_enhanced_pcl"); nh.param("radar_slam/imuTopic", imuTopic, "/vectornav/imu"); nh.param("radar_slam/odomTopic", odomTopic, "/odom"); nh.param("radar_slam/gpsTopic", gpsTopic, "/ublox/fix"); nh.param("radar_slam/lidarFrame", lidarFrame, "livox"); nh.param("radar_slam/baselinkFrame", baselinkFrame, "base_link"); nh.param("radar_slam/odometryFrame", odometryFrame, "odom_imu"); nh.param("radar_slam/mapFrame", mapFrame, "map"); nh.param("radar_slam/useGpsElevation", useGpsElevation, false); nh.param("radar_slam/gpsCovThreshold", gpsCovThreshold, 2.0); nh.param("radar_slam/poseCovThreshold", poseCovThreshold, 25.0); nh.param("radar_slam/savePCD", savePCD, false); nh.param("radar_slam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/"); nh.param("radar_slam/downsampleRate", downsampleRate, 1); nh.param("radar_slam/lidarMinRange", lidarMinRange, 1.0); nh.param("radar_slam/lidarMaxRange", lidarMaxRange, 1000.0); nh.param("radar_slam/imuAccNoise", imuAccNoise, 0.01); nh.param("radar_slam/imuGyrNoise", imuGyrNoise, 0.001); nh.param("radar_slam/imuAccBiasN", imuAccBiasN, 0.0002); nh.param("radar_slam/imuGyrBiasN", imuGyrBiasN, 0.00003); nh.param("radar_slam/imuGravity", imuGravity, 9.80511); nh.param("radar_slam/imuRPYWeight", imuRPYWeight, 0.01); nh.param>("radar_slam/extrinsicRot", extRotV, vector()); nh.param>("radar_slam/extrinsicRPY", extRPYV, vector()); nh.param>("radar_slam/extrinsicTrans", extTransV, vector()); extRot = Eigen::Map>(extRotV.data(), 3, 3); extRPY = Eigen::Map>(extRPYV.data(), 3, 3); extTrans = Eigen::Map>(extTransV.data(), 3, 1); extQRPY = Eigen::Quaterniond(extRPY); nh.param("radar_slam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2); nh.param("radar_slam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2); nh.param("radar_slam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2); nh.param("radar_slam/numberOfCores", numberOfCores, 2); nh.param("radar_slam/mappingProcessInterval", mappingProcessInterval, 0.15); nh.param("radar_slam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0); nh.param("radar_slam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2); nh.param("radar_slam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0); nh.param("radar_slam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0); nh.param("radar_slam/loopClosureEnableFlag", loopClosureEnableFlag, false); nh.param("radar_slam/loopClosureFrequency", loopClosureFrequency, 1.0); nh.param("radar_slam/surroundingKeyframeSize", surroundingKeyframeSize, 50); nh.param("radar_slam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0); nh.param("radar_slam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0); nh.param("radar_slam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25); nh.param("radar_slam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3); nh.param("radar_slam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3); nh.param("radar_slam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0); nh.param("radar_slam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0); usleep(100); } sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in) { sensor_msgs::Imu imu_out = imu_in; // rotate acceleration Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); acc = extRot * acc; imu_out.linear_acceleration.x = acc.x(); imu_out.linear_acceleration.y = acc.y(); imu_out.linear_acceleration.z = acc.z(); // rotate gyroscope Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); gyr = extRot * gyr; imu_out.angular_velocity.x = gyr.x(); imu_out.angular_velocity.y = gyr.y(); imu_out.angular_velocity.z = gyr.z(); // rotate roll pitch yaw Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z); Eigen::Quaterniond q_final = q_from * extQRPY; imu_out.orientation.x = q_final.x(); imu_out.orientation.y = q_final.y(); imu_out.orientation.z = q_final.z(); imu_out.orientation.w = q_final.w(); if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1) { ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!"); ros::shutdown(); } return imu_out; } }; sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame) { sensor_msgs::PointCloud2 tempCloud; pcl::toROSMsg(*thisCloud, tempCloud); tempCloud.header.stamp = thisStamp; tempCloud.header.frame_id = thisFrame; if (thisPub->getNumSubscribers() != 0) thisPub->publish(tempCloud); return tempCloud; } template double ROS_TIME(T msg) { return msg->header.stamp.toSec(); } template void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z) { *angular_x = thisImuMsg->angular_velocity.x; *angular_y = thisImuMsg->angular_velocity.y; *angular_z = thisImuMsg->angular_velocity.z; } template void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z) { *acc_x = thisImuMsg->linear_acceleration.x; *acc_y = thisImuMsg->linear_acceleration.y; *acc_z = thisImuMsg->linear_acceleration.z; } template void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw) { double imuRoll, imuPitch, imuYaw; tf::Quaternion orientation; tf::quaternionMsgToTF(thisImuMsg->orientation, orientation); tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw); *rosRoll = imuRoll; *rosPitch = imuPitch; *rosYaw = imuYaw; } float pointDistance(PointType p) { return sqrt(p.x*p.x + p.y*p.y + p.z*p.z); } float pointDistance(PointType p1, PointType p2) { return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z)); } #endif ================================================ FILE: launch/radar_graph_slam.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_carpark1.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_carpark3.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_garden.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_nanyanglink.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_ntuloop1.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_ntuloop2.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_ntuloop3.launch ================================================ ================================================ FILE: launch/rosbag_play_radar_smoke.launch ================================================ ================================================ FILE: msg/FloorCoeffs.msg ================================================ Header header float32[] coeffs ================================================ FILE: msg/ScanMatchingStatus.msg ================================================ Header header bool has_converged float32 matching_error float32 inlier_fraction geometry_msgs/Pose relative_pose std_msgs/String[] prediction_labels geometry_msgs/Pose[] prediction_errors ================================================ FILE: nodelet_plugins.xml ================================================ ================================================ FILE: package.xml ================================================ radar_graph_slam 0.0.0 The radar_graph_slam package koide BSD catkin ndt_omp fast_gicp pcl_ros roscpp rospy nodelet geodesy nmea_msgs sensor_msgs geometry_msgs message_generation libg2o cv_bridge image_transport ndt_omp fast_gicp pcl_ros roscpp rospy geodesy nodelet nmea_msgs sensor_msgs message_generation cv_bridge image_transport python-scipy python3-scipy python-progressbar python3-progressbar ================================================ FILE: rviz/radar_graph_slam.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Grid1 - /TF1/Frames1 - /MarkerArray1/Namespaces1 - //odom1/Shape1 - /imu_odom1/Shape1 - /imu_odom_incre1/Shape1 - /frame2frame1/Shape1 - /ground_truth1/Shape1 - /orient_incre1/Shape1 Splitter Ratio: 0.5778490900993347 Tree Height: 539 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Name: Time SyncMode: 0 SyncSource: /filtered_points Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 5 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 50 Reference Frame: Value: true - Class: rviz/TF Enabled: true Frame Timeout: 50 Frames: All Enabled: false base_link: Value: true map: Value: false odom: Value: false velodyne: Value: false Marker Alpha: 1 Marker Scale: 10 Name: TF Show Arrows: false Show Axes: true Show Names: false Tree: map: base_link: velodyne: {} odom: {} Update Interval: 0 Value: true - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 24.719999313354492 Min Color: 0; 0; 0 Min Intensity: 10.010000228881836 Name: oculli_points Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.20000000298023224 Style: Points Topic: /eagle_data/pc2_raw Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: z Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 2 Min Color: 0; 0; 0 Min Intensity: -1 Name: map_points Position Transformer: XYZ Queue Size: 1 Selectable: true Size (Pixels): 1.5 Size (m): 0.05000000074505806 Style: Points Topic: /radar_graph_slam/map_points Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/MarkerArray Enabled: true Marker Topic: /radar_graph_slam/markers Name: MarkerArray Namespaces: edges: true nodes: true Queue Size: 100 Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: /odom Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 2 Axes Radius: 0.20000000298023224 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /odom Unreliable: false Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: /filtered_points Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.10000000149011612 Style: Points Topic: /filtered_points Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 264.5556640625 Min Value: 31.884599685668945 Value: true Axis: X Channel Name: doppler Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: inlier_pc2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.20000000298023224 Style: Squares Topic: /eagle_data/inlier_pc2 Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: pc2_xyz_filtered Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 4 Size (m): 0.009999999776482582 Style: Points Topic: /eagle_data/pc2_xyz_filtered Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: segmented Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points Topic: /eagle_data/segmented Unreliable: false Use Fixed Frame: true Use rainbow: true Value: false - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: imu_odom Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 2 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /imu_pre_integ/imu_odom Unreliable: false Value: false - Angle Tolerance: 0 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: imu_odom_incre Position Tolerance: 0 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1.5 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /imu_pre_integ/imu_odom_incre Unreliable: false Value: true - Angle Tolerance: 0 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: frame2frame Position Tolerance: 0 Queue Size: 10 Shape: Alpha: 1 Axes Length: 2 Axes Radius: 0.20000000298023224 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /radar_graph_slam/odom_frame2frame Unreliable: false Value: false - Angle Tolerance: 0 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: ground_truth Position Tolerance: 0 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 252; 233; 79 Head Length: 0.30000001192092896 Head Radius: 0.20000000298023224 Shaft Length: 1.5 Shaft Radius: 0.10000000149011612 Value: Arrow Topic: /aftmapped_to_init Unreliable: false Value: false - Class: rviz/Image Enabled: false Image Topic: /sc/cur Max Value: 1 Median window: 5 Min Value: 0 Name: sc_cur Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Image Enabled: false Image Topic: /sc/pre Max Value: 1 Median window: 5 Min Value: 0 Name: sc_pre Normalize Range: true Queue Size: 2 Transport Hint: raw Unreliable: false Value: false - Class: rviz/Image Enabled: true Image Topic: /rgb_cam/image_raw Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Queue Size: 2 Transport Hint: compressed Unreliable: false Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: false Keep: 1 Name: orient_incre Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 2 Axes Radius: 0.20000000298023224 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /imu_pre_integ/orient_incre Unreliable: false Value: false Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: map Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/ThirdPersonFollower Distance: 85.2797622680664 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: 1.0382673740386963 Y: -3.4685535430908203 Z: -9.450655488763005e-05 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.4747973680496216 Target Frame: base_link Yaw: 3.460458517074585 Saved: - Class: rviz/ThirdPersonFollower Distance: 53.36624526977539 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: -12.508434295654297 Y: -8.800935745239258 Z: 1.2215999364852905 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: ThirdPersonFollower Near Clip Distance: 0.009999999776482582 Pitch: 0.6847967505455017 Target Frame: base_link Yaw: -2.4200901985168457 Window Geometry: Displays: collapsed: false Height: 1169 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false QMainWindow State: 000000ff00000000fd000000040000000000000156000003d1fc020000000bfb000000100044006900730070006c006100790073010000003d00000258000000c900fffffffb0000000a0049006d006100670065010000029b000001730000001600fffffffb0000000a0063006f006c006f007200000002bb000001450000000000000000fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000c00730063005f00630075007202000001e80000004700000280000001e0fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014c000003d1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000b44000000a9fc0100000002fb0000000c00730063005f00700072006502000000480000004700000280000001e0fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006c300000060fc0100000002fb0000000800540069006d00650100000000000006c3000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000415000003d100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1731 X: 514 Y: 27 sc_cur: collapsed: false sc_pre: collapsed: false ================================================ FILE: setup.py ================================================ ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup # fetch values from package.xml setup_args = generate_distutils_setup( packages=['radar_graph_slam'], package_dir={'': 'src'}) setup(**setup_args) ================================================ FILE: src/g2o/robust_kernel_io.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include #include #include namespace g2o { std::string kernel_type(g2o::RobustKernel* kernel) { if(dynamic_cast(kernel)) { return "Huber"; } if(dynamic_cast(kernel)) { return "Cauchy"; } if(dynamic_cast(kernel)) { return "DCS"; } if(dynamic_cast(kernel)) { return "Fair"; } if(dynamic_cast(kernel)) { return "GemanMcClure"; } if(dynamic_cast(kernel)) { return "PseudoHuber"; } if(dynamic_cast(kernel)) { return "Saturated"; } if(dynamic_cast(kernel)) { return "Tukey"; } if(dynamic_cast(kernel)) { return "Welsch"; } return ""; } bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { std::ofstream ofs(filename); if(!ofs) { std::cerr << "failed to open output stream!!" << std::endl; return false; } for(const auto& edge_ : graph->edges()) { g2o::OptimizableGraph::Edge* edge = static_cast(edge_); g2o::RobustKernel* kernel = edge->robustKernel(); if(!kernel) { continue; } std::string type = kernel_type(kernel); if(type.empty()) { std::cerr << "unknown kernel type!!" << std::endl; continue; } ofs << edge->vertices().size() << " "; for(size_t i = 0; i < edge->vertices().size(); i++) { ofs << edge->vertices()[i]->id() << " "; } ofs << type << " " << kernel->delta() << std::endl; } return true; } class KernelData { public: KernelData(const std::string& line) { std::stringstream sst(line); size_t num_vertices; sst >> num_vertices; vertex_indices.resize(num_vertices); for(size_t i = 0; i < num_vertices; i++) { sst >> vertex_indices[i]; } sst >> type >> delta; } bool match(g2o::OptimizableGraph::Edge* edge) const { if(edge->vertices().size() != vertex_indices.size()) { return false; } for(size_t i = 0; i < edge->vertices().size(); i++) { if(edge->vertices()[i]->id() != vertex_indices[i]) { return false; } } return true; } g2o::RobustKernel* create() const { auto factory = g2o::RobustKernelFactory::instance(); g2o::RobustKernel* kernel = factory->construct(type); kernel->setDelta(delta); return kernel; } public: std::vector vertex_indices; std::string type; double delta; }; bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) { std::ifstream ifs(filename); if(!ifs) { std::cerr << "warning: failed to open input stream!!" << std::endl; return true; } std::vector kernels; while(!ifs.eof()) { std::string line; std::getline(ifs, line); if(line.empty()) { continue; } kernels.push_back(KernelData(line)); } std::cout << "kernels: " << kernels.size() << std::endl; for(auto& edge_ : graph->edges()) { g2o::OptimizableGraph::Edge* edge = static_cast(edge_); for(auto itr = kernels.begin(); itr != kernels.end(); itr++) { if(itr->match(edge)) { edge->setRobustKernel(itr->create()); kernels.erase(itr); break; } } } if(kernels.size() != 0) { std::cerr << "warning: there is non-associated kernels!!" << std::endl; } return true; } } // namespace g2o ================================================ FILE: src/gps_traj_align.cpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace radar_graph_slam; std::vector> associate(std::vector first_stamps, std::vector second_stamps, double offset, double max_difference) { /* associate timestamps first_stamps, second_stamps: list of timestamps to associate Output: sorted list of matches (match_first_idx, match_second_idx) */ // potential_matches = [(abs(a - (b + offset)), idx_a, idx_b) // for idx_a, a in enumerate(first_stamps) // for idx_b, b in enumerate(second_stamps) // if abs(a - (b + offset)) < max_difference] std::vector> potential_matches; for (int i=0; i < first_stamps.size(); i++){ for (int j; j < second_stamps.size(); j++){ double& a = first_stamps.at(i); double& b = second_stamps.at(j); if (abs(a - (b + offset)) < max_difference) potential_matches.push_back(std::pair(i,j)); } } std::sort(potential_matches.begin(), potential_matches.end()); // potential_matches.sort() // prefer the closest #if 0 std::vector first_idxes, second_idxes; for (int i=0; i> matches; for (int i=0; i < potential_matches.size(); i++){ int idx_a = potential_matches.at(i).first; int idx_b = potential_matches.at(i).second; vector::iterator ret_a, ret_b; ret_a = std::find(first_idxes.begin(), first_idxes.end(), idx_a); ret_b = std::find(second_idxes.begin(), second_idxes.end(), idx_b); bool idx_a_in_first_idxes, idx_b_in_second_idxes; if(ret_a == first_idxes.end()) idx_a_in_first_idxes = false; else idx_a_in_first_idxes = true; if(ret_b == second_idxes.end()) idx_b_in_second_idxes = false; else idx_b_in_second_idxes = true; if (idx_a_in_first_idxes && idx_b_in_second_idxes) { cout << first_idxes.size() << " " << idx_a << endl; cout << second_idxes.size() << " " << idx_b << endl; first_idxes.erase(first_idxes.begin()+idx_a, first_idxes.begin()+idx_a+1); second_idxes.erase(second_idxes.begin()+idx_b, second_idxes.begin()+idx_b+1); matches.push_back(std::pair(idx_a, idx_b)); } } std::sort(matches.begin(), matches.end()); // for diff, idx_a, idx_b in potential_matches: // if idx_a in first_idxes and idx_b in second_idxes: // first_idxes.remove(idx_a) // second_idxes.remove(idx_b) // matches.append((int(idx_a), int(idx_b))) // matches.sort() return matches; #endif return potential_matches; } void read_topic_from_rosbag(rosbag::View::iterator& iter, rosbag::View& viewer, std::vector& msgs_queue){ for (; iter != viewer.end(); ++iter) { // Get a frame of data auto m = *iter; std::string topic = m.getTopic(); if (topic == "/ublox/fix") { sensor_msgs::NavSatFix::Ptr navsat_msg = m.instantiate(); // cout << "/ublox/fix " << setprecision(19) << navsat_msg->header.stamp.toSec() << endl; msgs_queue.push_back(navsat_msg); } } } int main() { ifstream file_in("/home/zhuge/datasets/Radar/gt_loop3.txt"); if (!file_in.is_open()) { cout << "Can not open the groundtruth file" << endl; return 0; } rosbag::Bag bag0, bag1, bag2, bag3; // bag0.open("/home/zhuge/datasets/Radar/ntu_loop2_car/ntu-car-day-loop2_2022-06-03_0.bag", rosbag::bagmode::Read); // bag1.open("/home/zhuge/datasets/Radar/ntu_loop2_car/ntu-car-day-loop2_2022-06-03_1.bag", rosbag::bagmode::Read); // bag2.open("/home/zhuge/datasets/Radar/ntu_loop2_car/ntu-car-day-loop2_2022-06-03_2.bag", rosbag::bagmode::Read); // bag3.open("/home/zhuge/datasets/Radar/ntu_loop2_car/ntu-car-day-loop2_2022-06-03_3.bag", rosbag::bagmode::Read); bag0.open("/home/zhuge/datasets/Radar/ntu_loop3_car/ntu-car-day-loop3_2022-06-03_0.bag", rosbag::bagmode::Read); bag1.open("/home/zhuge/datasets/Radar/ntu_loop3_car/ntu-car-day-loop3_2022-06-03_1.bag", rosbag::bagmode::Read); bag2.open("/home/zhuge/datasets/Radar/ntu_loop3_car/ntu-car-day-loop3_2022-06-03_2.bag", rosbag::bagmode::Read); std::vector topics; topics.push_back(std::string("/ublox/fix")); rosbag::View view_0(bag0, rosbag::TopicQuery(topics)); rosbag::View view_1(bag1, rosbag::TopicQuery(topics)); rosbag::View view_2(bag2, rosbag::TopicQuery(topics)); // rosbag::View view_3(bag3, rosbag::TopicQuery(topics)); // Using iterators to iterate rosbag::View::iterator it_0 = view_0.begin(); rosbag::View::iterator it_1 = view_1.begin(); rosbag::View::iterator it_2 = view_2.begin(); // rosbag::View::iterator it_3 = view_3.begin(); std::vector navsat_msgs; read_topic_from_rosbag(it_0, view_0, navsat_msgs); read_topic_from_rosbag(it_1, view_1, navsat_msgs); read_topic_from_rosbag(it_2, view_2, navsat_msgs); // read_topic_from_rosbag(it_3, view_3, navsat_msgs); cout << "The Number of navsat message in bag is " << navsat_msgs.size() << endl; bag0.close(); bag1.close(); bag2.close(); // bag3.close(); std::vector utm_coordinates; std::vector utm_stamps; std::vector utm_covariences; for (int i; i < navsat_msgs.size(); i++){ if (navsat_msgs.at(i)->position_covariance.at(0) > 3 || navsat_msgs.at(i)->position_covariance.at(8) > 8) continue; geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped()); gps_msg->header = navsat_msgs.at(i)->header; gps_msg->position.latitude = navsat_msgs.at(i)->latitude; gps_msg->position.longitude = navsat_msgs.at(i)->longitude; gps_msg->position.altitude = navsat_msgs.at(i)->altitude; // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate geodesy::UTMPoint utm; geodesy::fromMsg(gps_msg->position, utm); Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude); Eigen::Vector3d cov(navsat_msgs.at(i)->position_covariance.at(0), navsat_msgs.at(i)->position_covariance.at(4), navsat_msgs.at(i)->position_covariance.at(8)); utm_covariences.push_back(cov); utm_coordinates.push_back(xyz); utm_stamps.push_back(navsat_msgs.at(i)->header.stamp.toSec()); } cout << "The Number of chosen utm coordinate points is " << utm_coordinates.size() << endl; std::vector vectorLines; std::string line; while (getline(file_in, line)) vectorLines.push_back(line); std::vector gt_coordinates; std::vector gt_stamps; for (size_t i = 1; i < vectorLines.size(); i++) { std::string line_ = vectorLines.at(i); double stamp,tx,ty,tz,qx,qy,qz,qw; stringstream data(line_); data >> stamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw; Eigen::Vector3d point(Eigen::Vector3d(tx,ty,tz)); gt_coordinates.push_back(point); gt_stamps.push_back(stamp); } cout << "The Number of groundtruth points is " << gt_coordinates.size() << endl; vector> associated_pairs = associate(gt_stamps, utm_stamps, 0, 0.02); cout << "The Number of associated point pairs is " << associated_pairs.size() << endl; // cout << "associated_pairs:" << endl; // for (auto p:associated_pairs){ // cout << p.first << " " << p.second << endl; // } // // Calculate the initial guess, to save computation // int pair_size = associated_pairs.size(); // Eigen::Matrix4Xd A (4, pair_size); // we have transposed A here for the convenience of computation // Eigen::Matrix4Xd b (4, pair_size); // for (int k = 0; k < pair_size; ++k) { // A.block<3, 1>(0, k) = gt_coordinates.at(associated_pairs.at(k).first); // A(3, k) = 1; // b.block<3, 1>(0, k) = utm_coordinates.at(associated_pairs.at(k).second); // b(3, k) = 1; // } // // solve for T // Eigen::Matrix4d result = A * b.inverse(); // Eigen::Matrix3d R_0 = result.topLeftCorner(3,3); // Eigen::AngleAxisd aa(R_0); // RotationMatrix to AxisAngle // cout << aa.axis() << endl; // cout << aa.axis().norm() << endl; // cout << aa.angle() << endl; // Eigen::Matrix3d R = aa.toRotationMatrix(); // AxisAngle to RotationMatrix // Eigen::Vector3d t = result.block<3,1>(0,3); // Eigen::Matrix4d initial_transform = Eigen::Matrix4d::Identity(); // initial_transform.topLeftCorner(3,3) = R; // initial_transform.block<3,1>(0,3) = t; // cout << "The guess transform matrix is :" << endl; // cout << initial_transform << endl; /**** Construct the pose graph ****/ std::unique_ptr graph_slam; graph_slam.reset(new GraphSLAM("lm_var")); g2o::VertexSE3* node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity()); for (int i=0; i < associated_pairs.size(); i++){ Eigen::Vector3d& gt = gt_coordinates.at(associated_pairs.at(i).first); Eigen::Vector3d& utm = utm_coordinates.at(associated_pairs.at(i).second); Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity(); information_matrix(0,0) /= utm_covariences.at(i)(0); information_matrix(1,1) /= utm_covariences.at(i)(1); information_matrix(2,2) /= utm_covariences.at(i)(2); auto edge = graph_slam->add_se3_gt_utm_edge(node, gt, utm, information_matrix); } // optimize the pose graph int num_iterations = 10240; graph_slam->optimize(num_iterations); cout << "optimized !" << endl; Eigen::Matrix4d transform = node->estimate().matrix(); cout << "The final transform matrix form UTM to World is :" << endl; cout << fixed << setprecision(6); cout << transform << endl; return 0; } ================================================ FILE: src/gt_adjust.cpp ================================================ #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace radar_graph_slam; // Loop 3 matches: 0-5364 947-6168 // Loop 2 matches: 0-8240 583-8658 1350-9373 1573-9582 // Loop 1 matches: 0-10133(0-1654230309) 1961-11949(1654229491.74-1654230490.55) int main() { ifstream file_in("/home/zhuge/stamped_groundtruth_1.txt"); if (!file_in.is_open()) { cout << "Can not open this file" << endl; return 0; } std::vector vectorLines; std::string line; while (getline(file_in, line)) { vectorLines.push_back(line); } Eigen::MatrixXd pose_data; pose_data.resize(vectorLines.size()-1, 8); std::vector odoms; for (size_t i = 1; i < vectorLines.size(); i++) { std::string line_ = vectorLines.at(i); double stamp,tx,ty,tz,qx,qy,qz,qw; stringstream data(line_); data >> stamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw; pose_data.block<1, 8>(i-1, 0) << stamp, tx, ty, tz, qx, qy, qz, qw; Eigen::Isometry3d odom(Eigen::Isometry3d::Identity()); odom.pretranslate(Eigen::Vector3d(tx, ty, tz)); odom.rotate(Eigen::Quaterniond(qw, qx, qy, qz)); odoms.push_back(odom); } // Construct Pose Graph std::unique_ptr graph_slam; graph_slam.reset(new GraphSLAM("lm_var")); std::vector vertices; for (size_t i = 0; i < odoms.size(); i++) { g2o::VertexSE3* node = graph_slam->add_se3_node(odoms.at(i)); if (i == 0) node->setFixed(true); vertices.push_back(node); } for (size_t i = 0; i < odoms.size()-1; i++) { Eigen::Isometry3d rel_odom = odoms.at(i).inverse() * odoms.at(i+1); Eigen::MatrixXd information_matrix_se3 = Eigen::MatrixXd::Identity(6,6) / 0.05; auto edge_odom = graph_slam->add_se3_edge(vertices.at(i), vertices.at(i+1), rel_odom, information_matrix_se3); graph_slam->add_robust_kernel(edge_odom, "Huber", 1.0); } // Add Loop Edges // Eigen::Matrix3d information_matrix_xyz = Eigen::Matrix3d::Identity() / 1.0; // graph_slam->add_se3_prior_xyz_edge(vertices.at(8240), Eigen::Vector3d(0,0,0), information_matrix_xyz); Eigen::MatrixXd information_matrix = Eigen::MatrixXd::Identity(6, 6); information_matrix.block<3,3>(0,0) /= 0.5; information_matrix.block<3,3>(3,3) /= 1; // graph_slam->add_se3_edge(vertices.at(1961), vertices.at(11949), Eigen::Isometry3d::Identity(), information_matrix); graph_slam->add_se3_edge(vertices.at(0), vertices.at(10133), Eigen::Isometry3d::Identity(), information_matrix); // optimize the pose graph int num_iterations = 1024; graph_slam->optimize(num_iterations); cout << "optimized !" << endl; ofstream file_out("/home/zhuge/stamped_groundtruth.txt", ios::trunc); file_out << "# timestamp tx ty tz qx qy qz qw" << endl; file_out.setf(ios::fixed, ios::floatfield); // Set to fixed mode; floating-point numbers with decimal points file_out.precision(8); // Set precision for(size_t i = 0; i < vertices.size(); i++) { Eigen::Vector3d pos_ = vertices.at(i)->estimate().translation(); Eigen::Matrix3d rot_ = vertices.at(i)->estimate().rotation(); Eigen::Quaterniond quat_(rot_); double timestamp = pose_data(i, 0); double tx = pos_(0), ty = pos_(1), tz = pos_(2); double qx = quat_.x(), qy = quat_.y(), qz = quat_.z(), qw = quat_.w(); file_out << timestamp << " " << tx << " " << ty << " " << tz << " " << qx << " " << qy << " " << qz << " " << qw << endl; } file_in.close(); file_out.close(); return 0; } ================================================ FILE: src/radar_ego_velocity_estimator.cpp ================================================ // This file is part of RIO - Radar Inertial Odometry and Radar ego velocity estimation. // Copyright (C) 2021 Christopher Doer // (Institute of Control Systems, Karlsruhe Institute of Technology) // RIO 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 3 of the License, or //(at your option) any later version. // RIO 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 RIO. If not, see . #define PCL_NO_PRECOMPILE #include #include #include #include #include #include #include #include #include #include #include #include using namespace std; using namespace rio; static RadarPointCloudType toRadarPointCloudType(const Vector11& item, const RadarEgoVelocityEstimatorIndices& idx) { RadarPointCloudType point; point.x = item[idx.x_r]; point.y = item[idx.y_r]; point.z = item[idx.z_r]; point.doppler = -item[idx.doppler]; point.intensity = item[idx.snr_db]; return point; } bool RadarEgoVelocityEstimator::estimate(const sensor_msgs::PointCloud2& radar_scan_msg, Vector3& v_r, Vector3& sigma_v_r) { sensor_msgs::PointCloud2 inlier_radar_msg, outlier_radar_msg; return estimate(radar_scan_msg, v_r, sigma_v_r, inlier_radar_msg, outlier_radar_msg); } bool RadarEgoVelocityEstimator::estimate(const sensor_msgs::PointCloud2& radar_scan_msg, Vector3& v_r, Vector3& sigma_v_r, sensor_msgs::PointCloud2& inlier_radar_msg, sensor_msgs::PointCloud2& outlier_radar_msg) { auto radar_scan(new pcl::PointCloud); auto radar_scan_inlier(new pcl::PointCloud); // static objects auto radar_scan_outlier(new pcl::PointCloud); bool success = false; pcl::fromROSMsg (radar_scan_msg, *radar_scan); if (1) // pcl2msgToPcl(radar_scan_msg, *radar_scan) { std::vector valid_targets; for (uint i = 0; i < radar_scan->size(); ++i) { const auto target = radar_scan->at(i); const double r = Vector3(target.x, target.y, target.z).norm(); Real azimuth = std::atan2(target.y, target.x); Real elevation = std::atan2(std::sqrt(target.x * target.x + target.y * target.y), target.z)- M_PI_2; if (r > config_.min_dist && r < config_.max_dist && target.intensity > config_.min_db && std::fabs(azimuth) < angles::from_degrees(config_.azimuth_thresh_deg) && std::fabs(elevation) < angles::from_degrees(config_.elevation_thresh_deg)) { Vector11 v_pt; // features of a point v_pt << target.x, target.y, target.z, target.intensity, -target.doppler * config_.doppler_velocity_correction_factor, r, azimuth, elevation, target.x / r, target.y / r, target.z / r; valid_targets.emplace_back(v_pt); } // else{ // cout << (r > config_.min_dist) << (r < config_.max_dist) << (target.intensity > config_.min_db) << \ // (std::fabs(azimuth) < angles::from_degrees(config_.azimuth_thresh_deg)) << (std::fabs(elevation) < angles::from_degrees(config_.elevation_thresh_deg)) << \ // " " << angles::to_degrees(azimuth) << " " << angles::to_degrees(elevation) << endl; // } } if (valid_targets.size() > 2) { // check for zero velocity std::vector v_dopplers; for (const auto& v_pt : valid_targets) v_dopplers.emplace_back(std::fabs(v_pt[idx_.doppler])); const size_t n = v_dopplers.size() * (1.0 - config_.allowed_outlier_percentage); std::nth_element(v_dopplers.begin(), v_dopplers.begin() + n, v_dopplers.end()); const auto median = v_dopplers[n]; if (median < config_.thresh_zero_velocity) { // ROS_INFO_STREAM_THROTTLE(0.5, kPrefix << "Zero velocity detected!"); v_r = Vector3(0, 0, 0); sigma_v_r = Vector3(config_.sigma_zero_velocity_x, config_.sigma_zero_velocity_y, config_.sigma_zero_velocity_z); for (const auto& item : valid_targets) if (std::fabs(item[idx_.doppler]) < config_.thresh_zero_velocity) radar_scan_inlier->push_back(toRadarPointCloudType(item, idx_)); success = true; } else { // LSQ velocity estimation Matrix radar_data(valid_targets.size(), 4); // rx, ry, rz, v uint idx = 0, idx_o = 0; for (const auto& v_pt : valid_targets) radar_data.row(idx++) = Vector4(v_pt[idx_.normalized_x], v_pt[idx_.normalized_y], v_pt[idx_.normalized_z], v_pt[idx_.doppler]); if (config_.use_ransac) { std::vector inlier_idx_best; std::vector outlier_idx_best; success = solve3DFullRansac(radar_data, v_r, sigma_v_r, inlier_idx_best, outlier_idx_best); // Insert inlier points to the cloud for (const auto& idx : inlier_idx_best) radar_scan_inlier->push_back(toRadarPointCloudType(valid_targets.at(idx), idx_)); for (const auto& idx : outlier_idx_best) radar_scan_outlier->push_back(toRadarPointCloudType(valid_targets.at(idx), idx_)); } else { for (const auto& item : valid_targets) radar_scan_inlier->push_back(toRadarPointCloudType(item, idx_)); success = solve3DFull(radar_data, v_r, sigma_v_r); } } } else ROS_INFO("To small valid_targets (< 2) in radar_scan (%ld)", radar_scan->size()); radar_scan_inlier->height = 1; radar_scan_inlier->width = radar_scan_inlier->size(); pcl::PCLPointCloud2 tmp; pcl::toPCLPointCloud2(*radar_scan_inlier, tmp); pcl_conversions::fromPCL(tmp, inlier_radar_msg); inlier_radar_msg.header = radar_scan_msg.header; radar_scan_outlier->height = 1; radar_scan_outlier->width = radar_scan_outlier->size(); pcl::PCLPointCloud2 tmp_o; pcl::toPCLPointCloud2(*radar_scan_outlier, tmp_o); pcl_conversions::fromPCL(tmp_o, outlier_radar_msg); outlier_radar_msg.header = radar_scan_msg.header; } // if(success) // ROS_INFO("Ego Velocity estimation Successful"); // else // ROS_INFO("Ego Velocity estimation Failed"); return success; } bool RadarEgoVelocityEstimator::solve3DFullRansac(const Matrix& radar_data, Vector3& v_r, Vector3& sigma_v_r, std::vector& inlier_idx_best, std::vector& outlier_idx_best) { Matrix H_all(radar_data.rows(), 3); H_all.col(0) = radar_data.col(0); H_all.col(1) = radar_data.col(1); H_all.col(2) = radar_data.col(2); const Vector y_all = radar_data.col(3); std::vector idx(radar_data.rows()); for (uint k = 0; k < radar_data.rows(); ++k) idx[k] = k; std::random_device rd; std::mt19937 g(rd()); if (radar_data.rows() >= config_.N_ransac_points) { for (uint k = 0; k < ransac_iter_; ++k) { std::shuffle(idx.begin(), idx.end(), g); Matrix radar_data_iter; radar_data_iter.resize(config_.N_ransac_points, 4); for (uint i = 0; i < config_.N_ransac_points; ++i) radar_data_iter.row(i) = radar_data.row(idx.at(i)); bool rtn = solve3DFull(radar_data_iter, v_r, sigma_v_r, false); if (rtn == false) ROS_INFO("Failure at solve3DFullRansac() 1"); if (rtn) { const Vector err = (y_all - H_all * v_r).array().abs(); std::vector inlier_idx; std::vector outlier_idx; for (uint j = 0; j < err.rows(); ++j) { // ROS_INFO("Error: %f",err(j)); if (err(j) < config_.inlier_thresh) // find inlier points inlier_idx.emplace_back(j); else outlier_idx.emplace_back(j); } // if too small number of inlier detected, the error is too high, so regard outlier as inlier if ( float(outlier_idx.size())/(inlier_idx.size()+outlier_idx.size()) > 0.05 ) { inlier_idx.insert(inlier_idx.end(), outlier_idx.begin(), outlier_idx.end()); outlier_idx.clear(); // outlier_idx.swap(std::vector()); } // ROS_INFO("Inlier number: %ld, Outlier number: %ld, outlier Ratio: %f", // inlier_idx.size(), outlier_idx.size(), float(outlier_idx.size())/(inlier_idx.size()+outlier_idx.size())); if (inlier_idx.size() > inlier_idx_best.size()) { inlier_idx_best = inlier_idx; } if (outlier_idx.size() > outlier_idx_best.size()) { outlier_idx_best = outlier_idx; } } } } else{ROS_INFO("Warning: radar_data.rows() < config_.N_ransac_points");} if (!inlier_idx_best.empty()) { Matrix radar_data_inlier(inlier_idx_best.size(), 4); for (uint i = 0; i < inlier_idx_best.size(); ++i) radar_data_inlier.row(i) = radar_data.row(inlier_idx_best.at(i)); bool rtn = solve3DFull(radar_data_inlier, v_r, sigma_v_r, true); if (rtn == false) ROS_INFO("Failure at solve3DFullRansac() 2"); return rtn; } ROS_INFO("Failure at solve3DFullRansac() 3"); return false; } bool RadarEgoVelocityEstimator::solve3DFull(const Matrix& radar_data, Vector3& v_r, Vector3& sigma_v_r, bool estimate_sigma) { Matrix H(radar_data.rows(), 3); H.col(0) = radar_data.col(0); H.col(1) = radar_data.col(1); H.col(2) = radar_data.col(2); const Matrix HTH = H.transpose() * H; const Vector y = radar_data.col(3); Eigen::JacobiSVD svd(HTH); Real cond = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); // cond > 1000, error occurs if (1)//std::fabs(cond) < 1.0e3 { if (config_.use_cholesky_instead_of_bdcsvd) { v_r = (HTH).ldlt().solve(H.transpose() * y); } else v_r = H.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y); if (estimate_sigma) { const Vector e = H * v_r - y; const Matrix C = (e.transpose() * e).x() * (HTH).inverse() / (H.rows() - 3); sigma_v_r = Vector3(C(0, 0), C(1, 1), C(2, 2)); sigma_v_r = sigma_v_r.array(); if (sigma_v_r.x() >= 0.0 && sigma_v_r.y() >= 0.0 && sigma_v_r.z() >= 0.) { sigma_v_r = sigma_v_r.array().sqrt(); sigma_v_r += Vector3(config_.sigma_offset_radar_x, config_.sigma_offset_radar_y, config_.sigma_offset_radar_z); if (sigma_v_r.x() < config_.max_sigma_x && sigma_v_r.y() < config_.max_sigma_y && sigma_v_r.z() < config_.max_sigma_z) { return true; } } } else { return true; } } //ROS_INFO("cond too large, cond = %f", cond); return true;//return false; } ================================================ FILE: src/radar_graph_slam/Scancontext.cpp ================================================ #include "scan_context/Scancontext.h" // namespace SC2 // { using namespace radar_graph_slam; void coreImportTest (void) { cout << "scancontext lib is successfully imported." << endl; } // coreImportTest float rad2deg(float radians) { return radians * 180.0 / M_PI; } float deg2rad(float degrees) { return degrees * M_PI / 180.0; } float xy2theta( const float & _x, const float & _y ) { float theta; if ( (_x >= 0) & (_y >= 0)) theta = (180/M_PI) * atan(_y / _x); if ( (_x < 0) & (_y >= 0)) theta = 180 - ( (180/M_PI) * atan(_y / (-_x)) ); if ( (_x < 0) & (_y < 0)) theta = 180 + ( (180/M_PI) * atan(_y / _x) ); if ( (_x >= 0) & (_y < 0)) theta = 360 - ( (180/M_PI) * atan((-_y) / _x) ); return theta; } // xy2theta MatrixXd circshift( MatrixXd &_mat, int _num_shift ) { // shift columns to right direction assert(_num_shift >= 0); if( _num_shift == 0 ) { MatrixXd shifted_mat( _mat ); return shifted_mat; // Early return } MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) { int new_location = (col_idx + _num_shift) % _mat.cols(); shifted_mat.col(new_location) = _mat.col(col_idx); } return shifted_mat; } // circshift void SCManager::setScDistThresh(double thresh){ SC_DIST_THRES = thresh; } void SCManager::setAzimuthRange(double range){ PC_AZIMUTH_ANGLE_MAX = range; PC_AZIMUTH_ANGLE_MIN = - range; PC_UNIT_SECTOR_ANGLE = (PC_AZIMUTH_ANGLE_MAX - PC_AZIMUTH_ANGLE_MIN) / double(PC_NUM_SECTOR); } std::vector eig2stdvec( MatrixXd _eigmat ) { std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); return vec; } // eig2stdvec double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) { int num_eff_cols = 0; // i.e., to exclude all-nonzero sector double sum_sector_similarity = 0; for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) { VectorXd col_sc1 = _sc1.col(col_idx); VectorXd col_sc2 = _sc2.col(col_idx); if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) ) continue; // don't count this sector pair. double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); sum_sector_similarity = sum_sector_similarity + sector_similarity; num_eff_cols = num_eff_cols + 1; } double sc_sim = sum_sector_similarity / num_eff_cols; return 1.0 - sc_sim; } // distDirectSC int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) { int argmin_vkey_shift = 0; double min_veky_diff_norm = 10000000; for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) { MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); MatrixXd vkey_diff = _vkey1 - vkey2_shifted; double cur_diff_norm = vkey_diff.norm(); if( cur_diff_norm < min_veky_diff_norm ) { argmin_vkey_shift = shift_idx; min_veky_diff_norm = cur_diff_norm; } } return argmin_vkey_shift; } // fastAlignUsingVkey std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) { // 1. fast align using variant key (not in original IROS18) MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range std::vector shift_idx_search_space { argmin_vkey_shift }; for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) { shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); } std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); // 2. fast columnwise diff int argmin_shift = 0; double min_sc_dist = 10000000; for ( int num_shift: shift_idx_search_space ) { MatrixXd sc2_shifted = circshift(_sc2, num_shift); double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); if( cur_sc_dist < min_sc_dist ) { argmin_shift = num_shift; min_sc_dist = cur_sc_dist; } } return make_pair(min_sc_dist, argmin_shift); } // distanceBtnScanContext MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) { TicToc t_making_desc; int num_pts_scan_down = _scan_down.points.size(); // main const int NO_POINT = -1000; MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); SCPointType pt; float azim_angle, azim_range; // wihtin 2d plane int ring_idx, sctor_idx; for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++) { pt.x = _scan_down.points[pt_idx].x; pt.y = _scan_down.points[pt_idx].y; pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). pt.intensity = _scan_down.points[pt_idx].intensity; // xyz to ring, sector azim_range = sqrt(pt.x * pt.x + pt.y * pt.y); // azim_angle = xy2theta(pt.x, pt.y); azim_angle = (atan2f(pt.x, pt.y) - M_PI_2) * 180/M_PI; if( abs(azim_angle) > PC_AZIMUTH_ANGLE_MAX) continue; // if range is out of roi, pass if( azim_range > PC_MAX_RADIUS ) continue; ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); // sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( ((azim_angle - PC_AZIMUTH_ANGLE_MIN) / (PC_AZIMUTH_ANGLE_MAX - PC_AZIMUTH_ANGLE_MIN)) * PC_NUM_SECTOR )) ), 1 ); // taking maximum z // if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 // desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin // taking maximun intensity if ( desc(ring_idx-1, sctor_idx-1) < pt.intensity ) // -1 means cpp starts from 0 desc(ring_idx-1, sctor_idx-1) = pt.intensity; // update for taking maximum value at that bin } // reset no points to zero (for cosine dist later) for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) if( desc(row_idx, col_idx) == NO_POINT ) desc(row_idx, col_idx) = 0; t_making_desc.toc("PolarContext making"); return desc; } // SCManager::makeScancontext MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) { /* * summary: rowwise mean vector */ Eigen::MatrixXd invariant_key(_desc.rows(), 1); for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) { Eigen::MatrixXd curr_row = _desc.row(row_idx); invariant_key(row_idx, 0) = curr_row.mean(); } return invariant_key; } // SCManager::makeRingkeyFromScancontext MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) { /* * summary: columnwise mean vector */ Eigen::MatrixXd variant_key(1, _desc.cols()); for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) { Eigen::MatrixXd curr_col = _desc.col(col_idx); variant_key(0, col_idx) = curr_col.mean(); } return variant_key; } // SCManager::makeSectorkeyFromScancontext const Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void) { return polarcontexts_.back(); } void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ) { Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); polarcontexts_.push_back( sc ); polarcontext_invkeys_.push_back( ringkey ); polarcontext_vkeys_.push_back( sectorkey ); polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); // cout < SCManager::detectLoopClosureID ( const std::vector& candidate_keyframes, const KeyFrame::Ptr& new_keyframe) { int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID") // 应该比较new_keyframe和 candidate_keyframes ，所以选用的是new_keyframe对应的sc // cout << "polarcontext_invkeys_mat_.size()=" << polarcontext_invkeys_mat_.size() << " polarcontexts_.size()=" << polarcontexts_.size() << " " << endl; auto curr_key = polarcontext_invkeys_mat_[new_keyframe->index]; //.back(); // current observation (query) auto curr_desc = polarcontexts_[new_keyframe->index]; //.back(); // current observation (query) /* * step 1: candidates from ringkey tree_ */ // if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1) if( (int)new_keyframe->index < NUM_EXCLUDE_RECENT) { std::pair result {loop_id, 0.0}; return result; // Early return } // indices of candidate keyframes std::vector candidate_keyframe_indices; for (auto& ckf : candidate_keyframes) candidate_keyframe_indices.push_back(ckf->index); // tree_ reconstruction (not mandatory to make everytime) if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost { TicToc t_tree_construction; polarcontext_invkeys_to_search_.clear(); // polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; for (auto& index : candidate_keyframe_indices) if (new_keyframe->index - index >= NUM_EXCLUDE_RECENT) polarcontext_invkeys_to_search_.push_back(polarcontext_invkeys_mat_.at(index)); // error happeds here polarcontext_tree_.reset(); polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor) t_tree_construction.toc("Tree construction"); } tree_making_period_conter = tree_making_period_conter + 1; double min_dist = 10000000; // init with somthing large int nn_align = 0; int nn_idx = 0; // knn search std::vector knn_candidate_indexes( NUM_CANDIDATES_FROM_TREE ); std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); TicToc t_tree_search; nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); knnsearch_result.init( &knn_candidate_indexes[0], &out_dists_sqr[0] ); polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); t_tree_search.toc("Tree search"); /* * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) */ TicToc t_calc_dist; for ( int iter_idx = 0; iter_idx < NUM_CANDIDATES_FROM_TREE; iter_idx++ ) { if (knn_candidate_indexes[iter_idx] > candidate_keyframe_indices.size()-1){ // To avoid out-of-range knn indice ROS_INFO("To skip out-of-range knn indice: %ld",knn_candidate_indexes[iter_idx]); continue; } MatrixXd polarcontext_candidate = polarcontexts_[candidate_keyframe_indices[knn_candidate_indexes[iter_idx]]]; std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); double candidate_dist = sc_dist_result.first; int candidate_align = sc_dist_result.second; if( candidate_dist < min_dist ) { min_dist = candidate_dist; nn_align = candidate_align; nn_idx = candidate_keyframe_indices[knn_candidate_indexes[iter_idx]]; } } t_calc_dist.toc("Distance calc"); /* * loop threshold check */ if( min_dist < SC_DIST_THRES ) { loop_id = nn_idx; // std::cout.precision(3); cout << "\033[32m [Loop found] Nearest SC distance: " << min_dist << " between " << new_keyframe->index << " and " << nn_idx << ". \033[0m" << endl; cout << "\033[32m [Loop found] yaw diff: " << nn_align * PC_UNIT_SECTOR_ANGLE << " deg. \033[0m" << endl; } else { std::cout.precision(3); cout << "\033[33m [Not loop] Nearest SC distance: " << min_dist << " between " << new_keyframe->index << " and " << nn_idx << ". \033[0m" << endl; cout << "\033[33m [Not loop] yaw diff: " << nn_align * PC_UNIT_SECTOR_ANGLE << " deg. \033[0m" << endl; } // To do: return also nn_align (i.e., yaw diff) float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTOR_ANGLE); std::pair result {loop_id, yaw_diff_rad}; return result; } // SCManager::detectLoopClosureID // } // namespace SC2 ================================================ FILE: src/radar_graph_slam/__init__.py ================================================ ================================================ FILE: src/radar_graph_slam/bag_player.py ================================================ #!/usr/bin/python # SPDX-License-Identifier: BSD-2-Clause import sys import yaml import time import curses import argparse try: from StringIO import StringIO ## for Python 2 except ImportError: from io import StringIO ## for Python 3 import rospy import rosbag import roslib from std_msgs.msg import * from sensor_msgs.msg import * from rosgraph_msgs.msg import * from progressbar import ProgressBar class BagPlayer: def __init__(self, bagfile, start, duration): print('opening...',) self.bag = rosbag.Bag(bagfile, 'r') print('done') self.message_generator = self.bag.read_messages() info_dict = yaml.safe_load(self.bag._get_yaml_info()) self.duration = float(info_dict['duration']) self.endtime = float(info_dict['end']) self.progress = ProgressBar(0, self.duration, term_width=80, fd=StringIO()) self.publishers = {} for con in self.bag._get_connections(): msg_class = roslib.message.get_message_class(con.datatype) self.publishers[con.topic] = rospy.Publisher(con.topic, msg_class, queue_size=256) self.clock_pub = rospy.Publisher('/clock', Clock, queue_size=256) self.init_time = None self.start = start self.duration = duration self.fail_count = 0 self.time_subs = {} self.target_times = {} self.latest_stamps = {} self.play() def update_time_subs(self): for topic_name, msg_type in rospy.get_published_topics(): if 'read_until' in topic_name and 'std_msgs/Header' in msg_type: if topic_name not in self.time_subs: print('ADD', topic_name) self.time_subs[topic_name] = rospy.Subscriber(topic_name, Header, self.time_callback, topic_name) def time_callback(self, header_msg, topic_name): if header_msg.frame_id not in self.target_times: self.target_times[header_msg.frame_id] = {} if topic_name not in self.target_times[header_msg.frame_id] or self.target_times[header_msg.frame_id][topic_name] < header_msg.stamp: self.target_times[header_msg.frame_id][topic_name] = header_msg.stamp def play_realtime(self, duration): topic, msg, stamp = next(self.message_generator) stamp_wall = time.time() start_stamp = stamp start_stamp_wall = stamp_wall self.start_stamp = start_stamp while not rospy.is_shutdown() and (stamp - start_stamp) < duration: stamp_wall = time.time() elapsed_stamp = stamp - start_stamp if (stamp_wall - start_stamp_wall) < (elapsed_stamp.secs + elapsed_stamp.nsecs * 1e-9): time.sleep(1e-6) self.update_time_subs() continue clock_msg = Clock() clock_msg.clock = stamp if self.init_time is None: self.init_time = stamp if self.start and (stamp - self.init_time) < rospy.Duration(float(self.start)): start_stamp = stamp else: self.clock_pub.publish(clock_msg) self.publishers[topic].publish(msg) topic, msg, stamp = next(self.message_generator) return topic, msg, stamp def print_progress(self, stamp): self.stdscr.clear() self.stdscr.addstr(0, 0, 'topic') self.stdscr.addstr(0, 25, 'time') line = 1 for target in self.target_times: if target not in self.publishers: continue for sub_name in self.target_times[target]: target_time = self.target_times[target][sub_name] self.stdscr.addstr(line, 0, sub_name[:-11]) self.stdscr.addstr(line, 25, '%.6f' % (target_time.secs + target_time.nsecs * 1e-9)) residual = target_time - self.latest_stamps[target].stamp color = 1 if residual.to_sec() > 0.0 else 2 self.stdscr.addstr(line, 50, '%.5f' % residual.to_sec(), curses.color_pair(color)) line += 1 if not hasattr(self, 'prev_stamp'): self.prev_stamp = stamp self.prev_stamp_wall = time.time() self.processing_speed = 1.0 elif time.time() - self.prev_stamp_wall > 1.0: sim_duration = (stamp - self.prev_stamp).to_sec() wall_duration = time.time() - self.prev_stamp_wall self.processing_speed = sim_duration / wall_duration self.stdscr.addstr(line, 0, 'current_stamp') self.stdscr.addstr(line, 25, '%.6f' % stamp.to_sec()) self.stdscr.addstr(line, 50, '(x%.2f)' % self.processing_speed) elapsed = (stamp - self.start_stamp).to_sec() self.progress.fd = StringIO() try: self.progress.update(elapsed) except: # nothing to do pass self.stdscr.addstr(line + 1, 0, '----------') self.stdscr.addstr(line + 2, 0, self.progress.fd.getvalue()) self.stdscr.refresh() def check_stamp(self, topic, msg): if topic not in self.target_times: return True if self.fail_count > 10: self.fail_count = 0 return True target_time_map = self.target_times[topic] for sub_name in target_time_map: self.latest_stamps[topic] = msg.header if msg.header.stamp > target_time_map[sub_name]: self.fail_count += 1 return False self.fail_count = 0 return True def play(self): print('play realtime for 3.0[sec]') topic, msg, stamp = self.play_realtime(rospy.Duration(5.0)) self.update_time_subs() print('play as fast as possible') self.stdscr = curses.initscr() curses.start_color() curses.noecho() curses.init_pair(1, curses.COLOR_BLUE, curses.COLOR_WHITE) curses.init_pair(2, curses.COLOR_RED, curses.COLOR_WHITE) try: while not rospy.is_shutdown(): if not self.check_stamp(topic, msg): self.update_time_subs() self.print_progress(stamp) time.sleep(0.1) continue clock_msg = Clock() clock_msg.clock = stamp if self.duration: if (stamp - self.init_time) > rospy.Duration(float(self.duration)): break self.clock_pub.publish(clock_msg) self.publishers[topic].publish(msg) topic, msg, stamp = next(self.message_generator) except: print(sys.exc_info()[0]) clock_msg = Clock() clock_msg.clock = stamp + rospy.Duration(30.0) self.clock_pub.publish(clock_msg) time.sleep(0.5) curses.echo() curses.endwin() def main(): myargv = rospy.myargv(sys.argv) parser = argparse.ArgumentParser() parser.add_argument('input_bag', help='bag file to be played') parser.add_argument('-s', '--start', help='start sec seconds into the bag') parser.add_argument('-u', '--duration', help='play only sec seconds into the bag') args = parser.parse_args(myargv[1:]) if len(sys.argv) < 2: print('usage bag_player src_bagname') return rospy.init_node('bag_player') BagPlayer(args.input_bag, args.start, args.duration) if __name__ == '__main__': main() ================================================ FILE: src/radar_graph_slam/ford2bag.py ================================================ #!/usr/bin/python # SPDX-License-Identifier: BSD-2-Clause import re import os import sys import struct import numpy import scipy.io import rospy import rosbag import progressbar import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import NavSatFix, NavSatStatus, PointCloud2 from geographic_msgs.msg import GeoPointStamped def gps2navsat(filename, bag): with open(filename, 'r') as file: try: while True: data = struct.unpack('qddd', file.read(8*4)) time = data[0] local = data[1:] lat_lon_el_theta = struct.unpack('dddd', file.read(8*4)) gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16)) if abs(lat_lon_el_theta[0]) < 1e-1: continue navsat = NavSatFix() navsat.header.frame_id = 'gps' navsat.header.stamp = rospy.Time.from_sec(time * 1e-6) navsat.status.status = NavSatStatus.STATUS_FIX navsat.status.service = NavSatStatus.SERVICE_GPS navsat.latitude = lat_lon_el_theta[0] navsat.longitude = lat_lon_el_theta[1] navsat.altitude = lat_lon_el_theta[2] navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist() navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN bag.write('/gps/fix', navsat, navsat.header.stamp) geopoint = GeoPointStamped() geopoint.header = navsat.header geopoint.position.latitude = lat_lon_el_theta[0] geopoint.position.longitude = lat_lon_el_theta[1] geopoint.position.altitude = lat_lon_el_theta[2] bag.write('/gps/geopoint', geopoint, geopoint.header.stamp) except: print 'done' def mat2pointcloud(filename): m = scipy.io.loadmat(filename) scan = numpy.transpose(m['SCAN']['XYZ'][0][0]).astype(numpy.float32) stamp = m['SCAN']['timestamp_laser'][0][0][0][0] * 1e-6 cloud = PointCloud2() cloud.header.stamp = rospy.Time.from_sec(stamp) cloud.header.frame_id = 'velodyne' cloud = pc2.create_cloud_xyz32(cloud.header, scan) return cloud def main(): if len(sys.argv) < 2: print 'usage: ford2bag.py src_dirname output_filename' output_filename = sys.argv[1] rospy.init_node('bag') filenames = sorted(['SCANS/' + x for x in os.listdir('SCANS') if re.match('Scan[0-9]*\.mat', x)]) print filenames progress = progressbar.ProgressBar(max_value=len(filenames)) pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=32) with rosbag.Bag(output_filename, 'w') as bag: gps2navsat('GPS.log', bag) for i, filename in enumerate(filenames): if rospy.is_shutdown(): break progress.update(i) cloud = mat2pointcloud(filename) if pub.get_num_connections(): pub.publish(cloud) bag.write('/velodyne_points', cloud, cloud.header.stamp) if __name__ == '__main__': main() ================================================ FILE: src/radar_graph_slam/graph_slam.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include G2O_USE_OPTIMIZATION_LIBRARY(pcg) G2O_USE_OPTIMIZATION_LIBRARY(cholmod) // be aware of that cholmod brings GPL dependency G2O_USE_OPTIMIZATION_LIBRARY(csparse) // be aware of that csparse brings LGPL unless it is dynamically linked namespace g2o { G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane) G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY) G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ) G2O_REGISTER_TYPE(EDGE_SE3_PRIORZ, EdgeSE3PriorZ) G2O_REGISTER_TYPE(EDGE_SE3_Z, EdgeSE3Z) G2O_REGISTER_TYPE(EDGE_SE3_SE3, EdgeSE3SE3) G2O_REGISTER_TYPE(EDGE_SE3_PRIORVEC, EdgeSE3PriorVec) G2O_REGISTER_TYPE(EDGE_SE3_PRIORQUAT, EdgeSE3PriorQuat) G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_NORMAL, EdgePlanePriorNormal) G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_DISTANCE, EdgePlanePriorDistance) G2O_REGISTER_TYPE(EDGE_PLANE_IDENTITY, EdgePlaneIdentity) G2O_REGISTER_TYPE(EDGE_PLANE_PARALLEL, EdgePlaneParallel) G2O_REGISTER_TYPE(EDGE_PLANE_PAERPENDICULAR, EdgePlanePerpendicular) G2O_REGISTER_TYPE(EDGE_SE3_GTUTM, EdgeSE3GtUTM) } // namespace g2o namespace radar_graph_slam { /** * @brief constructor */ GraphSLAM::GraphSLAM(const std::string& solver_type) { graph.reset(new g2o::SparseOptimizer()); g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); std::cout << "construct solver: " << solver_type << std::endl; g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance(); g2o::OptimizationAlgorithmProperty solver_property; g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property); graph->setAlgorithm(solver); if(!graph->solver()) { std::cerr << std::endl; std::cerr << "error : failed to allocate solver!!" << std::endl; solver_factory->listSolvers(std::cerr); std::cerr << "-------------" << std::endl; std::cin.ignore(1); return; } std::cout << "done" << std::endl; robust_kernel_factory = g2o::RobustKernelFactory::instance(); } /** * @brief destructor */ GraphSLAM::~GraphSLAM() { graph.reset(); } void GraphSLAM::set_solver(const std::string& solver_type) { g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); std::cout << "construct solver: " << solver_type << std::endl; g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance(); g2o::OptimizationAlgorithmProperty solver_property; g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property); graph->setAlgorithm(solver); if(!graph->solver()) { std::cerr << std::endl; std::cerr << "error : failed to allocate solver!!" << std::endl; solver_factory->listSolvers(std::cerr); std::cerr << "-------------" << std::endl; std::cin.ignore(1); return; } std::cout << "done" << std::endl; } int GraphSLAM::num_vertices() const { return graph->vertices().size(); } int GraphSLAM::num_edges() const { return graph->edges().size(); } g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) { g2o::VertexSE3* vertex(new g2o::VertexSE3()); vertex->setId(static_cast(graph->vertices().size())); vertex->setEstimate(pose); graph->addVertex(vertex); return vertex; } g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) { g2o::VertexPlane* vertex(new g2o::VertexPlane()); vertex->setId(static_cast(graph->vertices().size())); vertex->setEstimate(plane_coeffs); graph->addVertex(vertex); return vertex; } g2o::VertexPointXYZ* GraphSLAM::add_point_xyz_node(const Eigen::Vector3d& xyz) { g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ()); vertex->setId(static_cast(graph->vertices().size())); vertex->setEstimate(xyz); graph->addVertex(vertex); return vertex; } g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3* edge(new g2o::EdgeSE3()); edge->setMeasurement(relative_pose); edge->setInformation(information_matrix); edge->vertices()[0] = v1; edge->vertices()[1] = v2; graph->addEdge(edge); return edge; } g2o::EdgeSE3Prior* GraphSLAM::add_se3_prior_edge(g2o::VertexSE3* v_se3, const Eigen::Isometry3d& pose, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3Prior* edge(new g2o::EdgeSE3Prior()); edge->setMeasurement(pose); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } // Information matrix should be 6*6 g2o::EdgeSE3SE3* GraphSLAM::add_se3_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const g2o::SE3Quat& relative_pose, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3SE3* edge(new g2o::EdgeSE3SE3()); edge->setMeasurement(relative_pose); edge->setInformation(information_matrix); edge->vertices()[0] = v1; edge->vertices()[1] = v2; graph->addEdge(edge); return edge; } g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane()); edge->setMeasurement(plane_coeffs); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; edge->vertices()[1] = v_plane; graph->addEdge(edge); return edge; } g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3PointXYZ* edge(new g2o::EdgeSE3PointXYZ()); edge->setMeasurement(xyz); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; edge->vertices()[1] = v_xyz; graph->addEdge(edge); return edge; } g2o::EdgeSE3GtUTM* GraphSLAM::add_se3_gt_utm_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& gt_xyz, const Eigen::Vector3d& utm_xyz, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3GtUTM* edge(new g2o::EdgeSE3GtUTM()); edge->setMeasurement(gt_xyz); edge->setMeasurement_utm(utm_xyz); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } g2o::EdgePlanePriorNormal* GraphSLAM::add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix) { g2o::EdgePlanePriorNormal* edge(new g2o::EdgePlanePriorNormal()); edge->setMeasurement(normal); edge->setInformation(information_matrix); edge->vertices()[0] = v; graph->addEdge(edge); return edge; } g2o::EdgePlanePriorDistance* GraphSLAM::add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix) { g2o::EdgePlanePriorDistance* edge(new g2o::EdgePlanePriorDistance()); edge->setMeasurement(distance); edge->setInformation(information_matrix); edge->vertices()[0] = v; graph->addEdge(edge); return edge; } g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY()); edge->setMeasurement(xy); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ()); edge->setMeasurement(xyz); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } g2o::EdgeSE3PriorZ* GraphSLAM::add_se3_prior_z_edge(g2o::VertexSE3* v_se3, const Eigen::Vector1d& z, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3PriorZ* edge(new g2o::EdgeSE3PriorZ()); edge->setMeasurement(z); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } g2o::EdgeSE3Z* GraphSLAM::add_se3_z_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Vector1d& z, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3Z* edge(new g2o::EdgeSE3Z()); edge->setMeasurement(z); edge->setInformation(information_matrix); edge->vertices()[0] = v1; edge->vertices()[1] = v2; graph->addEdge(edge); return edge; } g2o::EdgeSE3PriorVec* GraphSLAM::add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix) { Eigen::Matrix m; m.head<3>() = direction; m.tail<3>() = measurement; g2o::EdgeSE3PriorVec* edge(new g2o::EdgeSE3PriorVec()); edge->setMeasurement(m); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } g2o::EdgeSE3PriorQuat* GraphSLAM::add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix) { g2o::EdgeSE3PriorQuat* edge(new g2o::EdgeSE3PriorQuat()); edge->setMeasurement(quat); edge->setInformation(information_matrix); edge->vertices()[0] = v_se3; graph->addEdge(edge); return edge; } g2o::EdgePlane* GraphSLAM::add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) { g2o::EdgePlane* edge(new g2o::EdgePlane()); edge->setMeasurement(measurement); edge->setInformation(information); edge->vertices()[0] = v_plane1; edge->vertices()[1] = v_plane2; graph->addEdge(edge); return edge; } g2o::EdgePlaneIdentity* GraphSLAM::add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) { g2o::EdgePlaneIdentity* edge(new g2o::EdgePlaneIdentity()); edge->setMeasurement(measurement); edge->setInformation(information); edge->vertices()[0] = v_plane1; edge->vertices()[1] = v_plane2; graph->addEdge(edge); return edge; } g2o::EdgePlaneParallel* GraphSLAM::add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information) { g2o::EdgePlaneParallel* edge(new g2o::EdgePlaneParallel()); edge->setMeasurement(measurement); edge->setInformation(information); edge->vertices()[0] = v_plane1; edge->vertices()[1] = v_plane2; graph->addEdge(edge); return edge; } g2o::EdgePlanePerpendicular* GraphSLAM::add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information) { g2o::EdgePlanePerpendicular* edge(new g2o::EdgePlanePerpendicular()); edge->setMeasurement(measurement); edge->setInformation(information); edge->vertices()[0] = v_plane1; edge->vertices()[1] = v_plane2; graph->addEdge(edge); return edge; } void GraphSLAM::add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size) { if(kernel_type == "NONE") { return; } g2o::RobustKernel* kernel = robust_kernel_factory->construct(kernel_type); if(kernel == nullptr) { std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl; return; } kernel->setDelta(kernel_size); g2o::OptimizableGraph::Edge* edge_ = dynamic_cast(edge); edge_->setRobustKernel(kernel); } int GraphSLAM::optimize(int num_iterations) { g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); if(graph->edges().size() < 10) { return -1; } std::cout << std::endl; std::cout << "--- pose graph optimization ---" << std::endl; std::cout << "nodes: " << graph->vertices().size() << " edges: " << graph->edges().size() << std::endl; std::cout << "optimizing... " << std::flush; std::cout << "init" << std::endl; graph->initializeOptimization(); graph->setVerbose(false); // Open/Close debug output std::cout << "chi2" << std::endl; double chi2 = graph->chi2(); std::cout << "optimizing!!" << std::endl; auto t1 = ros::WallTime::now(); int iterations = graph->optimize(num_iterations); auto t2 = ros::WallTime::now(); std::cout << "done" << std::endl; std::cout << "iterations: " << iterations << " / " << num_iterations << std::endl; std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl; std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl; return iterations; } void GraphSLAM::save(const std::string& filename) { g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); std::ofstream ofs(filename); graph->save(ofs); g2o::save_robust_kernels(filename + ".kernels", graph); } bool GraphSLAM::load(const std::string& filename) { std::cout << "loading pose graph..." << std::endl; g2o::SparseOptimizer* graph = dynamic_cast(this->graph.get()); std::ifstream ifs(filename); if(!graph->load(ifs)) { return false; } std::cout << "nodes : " << graph->vertices().size() << std::endl; std::cout << "edges : " << graph->edges().size() << std::endl; if(!g2o::load_robust_kernels(filename + ".kernels", graph)) { return false; } return true; } } // namespace radar_graph_slam ================================================ FILE: src/radar_graph_slam/information_matrix_calculator.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include using namespace std; namespace radar_graph_slam { InformationMatrixCalculator::InformationMatrixCalculator(ros::NodeHandle& nh) { use_const_inf_matrix = nh.param("use_const_inf_matrix", false); const_stddev_x = nh.param("const_stddev_x", 0.5); const_stddev_q = nh.param("const_stddev_q", 0.1); var_gain_a = nh.param("var_gain_a", 20.0); min_stddev_x = nh.param("min_stddev_x", 0.1); max_stddev_x = nh.param("max_stddev_x", 5.0); min_stddev_q = nh.param("min_stddev_q", 0.05); max_stddev_q = nh.param("max_stddev_q", 0.2); fitness_score_thresh = nh.param("fitness_score_thresh", 0.5); } InformationMatrixCalculator::~InformationMatrixCalculator() {} Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const { if(use_const_inf_matrix) { Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); inf.topLeftCorner(3, 3).array() /= const_stddev_x; inf.bottomRightCorner(3, 3).array() /= const_stddev_q; return inf; } double fitness_score = calc_fitness_score(cloud1, cloud2, relpose); double min_var_x = std::pow(min_stddev_x, 2); double max_var_x = std::pow(max_stddev_x, 2); double min_var_q = std::pow(min_stddev_q, 2); double max_var_q = std::pow(max_stddev_q, 2); float w_x = weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score); float w_q = weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score); Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6); inf.topLeftCorner(3, 3).array() /= w_x; inf.bottomRightCorner(3, 3).array() /= w_q; // cout << "Information Matrix Parameters: " << inf(1,1) << inf(4,4) << endl; return inf; } double InformationMatrixCalculator::calc_fitness_score(const pcl::PointCloud::ConstPtr& cloud1, const pcl::PointCloud::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range) { pcl::search::KdTree::Ptr tree_(new pcl::search::KdTree()); tree_->setInputCloud(cloud1); double fitness_score = 0.0; // Transform the input dataset using the final transformation pcl::PointCloud input_transformed; pcl::transformPointCloud(*cloud2, input_transformed, relpose.cast()); std::vector nn_indices(1); std::vector nn_dists(1); // For each point in the source dataset int nr = 0; for(size_t i = 0; i < input_transformed.points.size(); ++i) { // Find its nearest neighbor in the target tree_->nearestKSearch(input_transformed.points[i], 1, nn_indices, nn_dists); // Deal with occlusions (incomplete targets) if(nn_dists[0] <= max_range) { // Add to the fitness score fitness_score += nn_dists[0]; nr++; } } if(nr > 0) return (fitness_score / nr); else return (std::numeric_limits::max()); } } // namespace radar_graph_slam ================================================ FILE: src/radar_graph_slam/keyframe.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include namespace radar_graph_slam { KeyFrame::KeyFrame(const size_t index, const ros::Time& stamp, const Eigen::Isometry3d& odom_scan2scan, double accum_distance, const pcl::PointCloud::ConstPtr& cloud) : index(index), stamp(stamp), odom_scan2scan(odom_scan2scan), accum_distance(accum_distance), cloud(cloud), node(nullptr) {} KeyFrame::KeyFrame(const std::string& directory, g2o::HyperGraph* graph) : stamp(), odom_scan2scan(Eigen::Isometry3d::Identity()), accum_distance(-1), cloud(nullptr), node(nullptr) { load(directory, graph); } KeyFrame::~KeyFrame() {} void KeyFrame::save(const std::string& directory) { if(!boost::filesystem::is_directory(directory)) { boost::filesystem::create_directory(directory); } std::ofstream ofs(directory + "/data"); ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n"; ofs << "estimate\n"; ofs << node->estimate().matrix() << "\n"; ofs << "odom_scan2scan\n"; ofs << odom_scan2scan.matrix() << "\n"; ofs << "accum_distance " << accum_distance << "\n"; if(floor_coeffs) { ofs << "floor_coeffs " << floor_coeffs->transpose() << "\n"; } if(utm_coord) { ofs << "utm_coord " << utm_coord->transpose() << "\n"; } if(acceleration) { ofs << "acceleration " << acceleration->transpose() << "\n"; } if(orientation) { ofs << "orientation " << orientation->w() << " " << orientation->x() << " " << orientation->y() << " " << orientation->z() << "\n"; } if(node) { ofs << "id " << node->id() << "\n"; } pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud); } bool KeyFrame::load(const std::string& directory, g2o::HyperGraph* graph) { std::ifstream ifs(directory + "/data"); if(!ifs) { return false; } long node_id = -1; boost::optional estimate; while(!ifs.eof()) { std::string token; ifs >> token; if(token == "stamp") { ifs >> stamp.sec >> stamp.nsec; } else if(token == "estimate") { Eigen::Matrix4d mat; for(int i = 0; i < 4; i++) { for(int j = 0; j < 4; j++) { ifs >> mat(i, j); } } estimate = Eigen::Isometry3d::Identity(); estimate->linear() = mat.block<3, 3>(0, 0); estimate->translation() = mat.block<3, 1>(0, 3); } else if(token == "odom_scan2scan") { Eigen::Matrix4d odom_mat = Eigen::Matrix4d::Identity(); for(int i = 0; i < 4; i++) { for(int j = 0; j < 4; j++) { ifs >> odom_mat(i, j); } } odom_scan2scan.setIdentity(); odom_scan2scan.linear() = odom_mat.block<3, 3>(0, 0); odom_scan2scan.translation() = odom_mat.block<3, 1>(0, 3); } else if(token == "accum_distance") { ifs >> accum_distance; } else if(token == "floor_coeffs") { Eigen::Vector4d coeffs; ifs >> coeffs[0] >> coeffs[1] >> coeffs[2] >> coeffs[3]; floor_coeffs = coeffs; } else if(token == "utm_coord") { Eigen::Vector3d coord; ifs >> coord[0] >> coord[1] >> coord[2]; utm_coord = coord; } else if(token == "acceleration") { Eigen::Vector3d acc; ifs >> acc[0] >> acc[1] >> acc[2]; acceleration = acc; } else if(token == "orientation") { Eigen::Quaterniond quat; ifs >> quat.w() >> quat.x() >> quat.y() >> quat.z(); orientation = quat; } else if(token == "id") { ifs >> node_id; } } if(node_id < 0) { ROS_ERROR_STREAM("invalid node id!!"); ROS_ERROR_STREAM(directory); return false; } if(graph->vertices().find(node_id) == graph->vertices().end()) { ROS_ERROR_STREAM("vertex ID=" << node_id << " does not exist!!"); return false; } node = dynamic_cast(graph->vertices()[node_id]); if(node == nullptr) { ROS_ERROR_STREAM("failed to downcast!!"); return false; } if(estimate) { node->setEstimate(*estimate); } pcl::PointCloud::Ptr cloud_(new pcl::PointCloud()); pcl::io::loadPCDFile(directory + "/cloud.pcd", *cloud_); cloud = cloud_; return true; } long KeyFrame::id() const { return node->id(); } Eigen::Isometry3d KeyFrame::estimate() const { return node->estimate(); } KeyFrameSnapshot::KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud::ConstPtr& cloud) : pose(pose), cloud(cloud) {} KeyFrameSnapshot::KeyFrameSnapshot(const KeyFrame::Ptr& key) : pose(key->node->estimate()), cloud(key->cloud) {} KeyFrameSnapshot::~KeyFrameSnapshot() {} } // namespace radar_graph_slam ================================================ FILE: src/radar_graph_slam/loop_detector.cpp ================================================ #include using namespace std; Eigen::Vector3d R2ypr_(const Eigen::Matrix3d& R) { Eigen::Vector3d n = R.col(0); Eigen::Vector3d o = R.col(1); Eigen::Vector3d a = R.col(2); Eigen::Vector3d ypr(3); double y = atan2(n(1), n(0)); double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); ypr(0) = y; ypr(1) = p; ypr(2) = r; return ypr; } double limit_value(int value, int min_, int max_){ if (value < min_) return min_; else if (value > max_) return max_; else return value; } Eigen::Vector3d monoToRainbow(int value){ double k = 4.65454545454; double blue = limit_value(- k * (value - 140), 0, 255); double green, red; if (value < 30){ green = 0; red = limit_value(- k * (value - 30), 0, 255); } else if (value < 140){ green = limit_value(k * (value - 30), 0, 255); red = 0; } else { green = limit_value(- k * (value - 250), 0, 255); red = limit_value(k * (value - 140), 0, 255); } return Eigen::Vector3d(blue, green, red); } namespace radar_graph_slam { LoopDetector::LoopDetector(ros::NodeHandle& pnh) { enable_pf = pnh.param("enable_pf", true); enable_odom_check = pnh.param("enable_odom_check", true); distance_thresh = pnh.param("distance_thresh", 5.0); accum_distance_thresh = pnh.param("accum_distance_thresh", 8.0); min_loop_interval_dist = pnh.param("min_loop_interval_dist", 5.0); fitness_score_max_range = pnh.param("fitness_score_max_range", std::numeric_limits::max()); fitness_score_thresh = pnh.param("fitness_score_thresh", 0.5); odom_drift_xy = pnh.param("odometry_drift_xy", 0.05); odom_drift_z = pnh.param("odometry_drift_z", 0.20); drift_scale_xy = pnh.param("odometry_drift_scale_xy", 2.0); drift_scale_z = pnh.param("odometry_drift_scale_z", 2.0); max_baro_difference = pnh.param("max_baro_difference", 3.0); max_yaw_difference = pnh.param("max_yaw_difference", 45.0); double sc_dist_thresh = pnh.param("sc_dist_thresh", 0.3); double sc_azimuth_range = pnh.param("sc_azimuth_range", 0.3); registration = select_registration_method(pnh); // Loop Closure Parameters pnh.param("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25); historyKeyframeFitnessScore = pnh.param("historyKeyframeFitnessScore", 6); odom_check_trans_thresh = pnh.param("odom_check_trans_thresh", 0.1); odom_check_rot_thresh = pnh.param("odom_check_rot_thresh", 0.05); pairwise_check_trans_thresh = pnh.param("pairwise_check_trans_thresh", 0.1); pairwise_check_rot_thresh = pnh.param("pairwise_check_rot_thresh", 0.05); scManager.reset(new SCManager()); inf_calclator.reset(new InformationMatrixCalculator(pnh)); image_transport.reset(new image_transport::ImageTransport(pnh)); pub_cur_sc = image_transport->advertise("/sc/cur",1); pub_pre_sc = image_transport->advertise("/sc/pre",1); scManager->setScDistThresh(sc_dist_thresh); scManager->setAzimuthRange(sc_azimuth_range); } LoopDetector::~LoopDetector() {} /** * @brief detect loops and add them to the pose graph * @param keyframes keyframes * @param new_keyframes newly registered keyframes * @param graph_slam pose graph */ std::vector LoopDetector::detect(const std::vector& keyframes, const std::deque& new_keyframes, radar_graph_slam::GraphSLAM& graph_slam) { std::vector detected_loops; for(const auto& new_keyframe : new_keyframes) { std::vector candidates; Loop::Ptr loop = nullptr; if (enable_pf){ start_ms_pf = clock(); candidates = find_candidates(keyframes, new_keyframe); end_ms_pf = clock(); } else{ for (auto kf:keyframes){ if (new_keyframe->accum_distance > kf->accum_distance + distance_thresh); candidates.push_back(kf); } } loop = performScanContextLoopClosure(keyframes, candidates, new_keyframe); // 找与new_keyframe匹配的SC // auto loop = matching(candidates, new_keyframe); if(loop) { detected_loops.push_back(loop); double time_used = double(end_ms_pf - start_ms_pf) / CLOCKS_PER_SEC; pf_time.push_back(time_used); time_used = double(end_ms_sc - start_ms_sc) / CLOCKS_PER_SEC; sc_time.push_back(time_used); time_used = double(end_ms_oc - start_ms_oc) / CLOCKS_PER_SEC; oc_time.push_back(time_used); } } return detected_loops; } /** * @brief find loop candidates. A detected loop begins at one of #keyframes and ends at #new_keyframe * @param keyframes candidate keyframes of loop start * @param new_keyframe loop end keyframe * @return loop candidates */ std::vector LoopDetector::find_candidates(const std::vector& keyframes, const KeyFrame::Ptr& new_keyframe) const { // 1. too close to the last registered loop edge double dist_btn_last_loop_edge = new_keyframe->accum_distance - last_loop_edge_accum_distance; if(dist_btn_last_loop_edge < min_loop_interval_dist) { return std::vector(); } // Pick loop candicates considering distance and barometer measurements std::vector candidates; candidates.reserve(32); for(const auto& k : keyframes) { // 2. traveled distance between keyframes is too small double accum_distance = new_keyframe->accum_distance - k->accum_distance; if(accum_distance < accum_distance_thresh) { continue; } // 3. If keyframes_ have similar barometer measurements // value if (k->altitude.is_initialized()){ if(fabs(k->altitude.value()[0] - new_keyframe->altitude.value()[0]) > max_baro_difference) continue; } // 4. yaw angle difference Eigen::Matrix4d T = k->node->estimate().matrix().inverse() * new_keyframe->node->estimate().matrix(); Eigen::Vector3d ypr_diff = R2ypr_(T.block<3,3>(0,0)); if (abs(ypr_diff(0) * 180/M_PI) > max_yaw_difference) continue; // 5. estimated distance between new-keyframe is too Large Eigen::Vector3d pos_between = T.block<3,1>(0,3); double dist = pos_between.norm(); // measure x-y distance double x_diff = pos_between(0), y_diff = pos_between(1), z_diff = pos_between(2); double rad_xy_loop = 3 + dist_btn_last_loop_edge * odom_drift_xy * drift_scale_xy; double rad_z_loop = 3 + dist_btn_last_loop_edge * odom_drift_z * drift_scale_z; double aa_lle = pow((x_diff/rad_xy_loop),2) + pow((y_diff/rad_xy_loop),2); if (aa_lle > 1) continue; // cout << "last_loop_edge_index: " << last_loop_edge_index << " new_keyframe: " << new_keyframe->index // << " dist_btn_last_loop_edge = " << dist_btn_last_loop_edge << endl; // cout << "rad_xy_loop " << rad_xy_loop << endl; // cout << "pos_btn: " << pos_between(0) << " " << pos_between(1) << " " << pos_between(2) << endl; // cout << " aa_lle = " << aa_lle << endl << endl; double rad_xy = 10.0 + odom_drift_xy * accum_distance * drift_scale_xy; double rad_z = 10.0 + odom_drift_z * accum_distance * drift_scale_z; double aa = pow((x_diff/rad_xy),2)+pow((y_diff/rad_xy),2);//+pow((z_diff/rad_z),2); if(aa > 1) // candidate keyframe pose is out of elipse (dist > distance_thresh) continue; candidates.push_back(k); } return candidates; } Loop::Ptr LoopDetector::performScanContextLoopClosure(const std::vector& keyframes, const std::vector& candidate_keyframes, const KeyFrame::Ptr& new_keyframe) { start_ms_sc = clock(); if (candidate_keyframes.empty() == true){ return nullptr; // ROS_INFO("\033[33m Cannot find Loop Candidate \033[0m"); } std::cout << "\033[36m" << " New keyframe index: " << new_keyframe->index << ". " << "Number of candidate frames: " << candidate_keyframes.size(); // << "Candidate frames index: "; // for (auto& ckf : candidate_keyframes){ // std::cout << ckf->index << " "; // } std::cout << "\033[0m" << endl; // Detect keys, first: nn index, second: yaw diff auto detectResult = scManager->detectLoopClosureID(candidate_keyframes, new_keyframe); int loopKeyCur = new_keyframe->index; int loopKeyPre = detectResult.first; float yawDiffRad = detectResult.second; // not use for v1 (because pcl icp withi initial somthing wrong...) if( loopKeyPre == -1 ) return nullptr; /* No loop found */ // extract cloud pcl::PointCloud::Ptr cureKeyframeCloud(new pcl::PointCloud()); pcl::PointCloud::Ptr prevKeyframeCloud(new pcl::PointCloud()); { *cureKeyframeCloud += *new_keyframe->cloud; // Because new_keyframe is not in keyframes *prevKeyframeCloud += *keyframes[loopKeyPre]->cloud; } registration->setInputSource(cureKeyframeCloud); registration->setInputTarget(prevKeyframeCloud); pcl::PointCloud::Ptr unused_result(new pcl::PointCloud()); registration->align(*unused_result); // TODO icp align with initial end_ms_sc = clock(); if (registration->hasConverged() == false || registration->getFitnessScore() > historyKeyframeFitnessScore) { bool converged = registration->hasConverged(); std::cout << "\033[31m" << "ICP fitness test failed. ICP convergence: " << converged << " (" << registration->getFitnessScore() << " > " << historyKeyframeFitnessScore << "). Reject this SC loop." << "\033[0m" << std::endl; return nullptr; } // Get pose transformation Eigen::Matrix4f correctionLidarFrame; correctionLidarFrame = registration->getFinalTransformation(); Eigen::Isometry3d poseFrom(Eigen::Isometry3d::Identity()); Eigen::Isometry3d poseTo(Eigen::Isometry3d::Identity()); Eigen::Vector3d vec = Eigen::Vector3d(correctionLidarFrame(0,3),correctionLidarFrame(1,3),correctionLidarFrame(2,3)); Eigen::Matrix3d mat = correctionLidarFrame.block(0,0,2,2).cast(); poseFrom.prerotate(mat); // Set rotation poseFrom.pretranslate(vec); Eigen::Isometry3d T_lc_ij = poseFrom.inverse() * poseTo; /****** Incremental Consistent Measurement Set Maximization ******/ // Odometry check (source_frame<->target_frame and source_frame<->last_loop_edge) start_ms_oc = clock(); if (enable_odom_check){ // source_frame <-> target_frame Eigen::Isometry3d T_odom_ji = new_keyframe->odom_scan2scan.inverse() * keyframes[loopKeyPre]->odom_scan2scan; Eigen::Isometry3d T_err_ij = T_lc_ij * T_odom_ji; int num_between = loopKeyCur - loopKeyPre;// the number of edges along the loop Eigen::AngleAxisd rotation_vector; rotation_vector.fromRotationMatrix(T_err_ij.rotation()); double oc_err_trans = T_err_ij.translation().norm() / num_between; double oc_err_rot = rotation_vector.angle() / num_between; // double err_rot = std::acos(Eigen::Quaterniond(T_err_ij.rotation()).w())/num_between; // cout << oc_err_rot << " " << err_rot << endl; if(oc_err_trans > odom_check_trans_thresh || oc_err_rot > odom_check_rot_thresh){ cout << "\033[31m Odometry Check invalid, Translation Error: " << oc_err_trans << ", rotation Error: " << oc_err_rot << " \033[0m" << endl; return nullptr; } // source_frame <-> last_loop_edge } end_ms_oc = clock(); if (!loopIndexQueue.empty()){ for (size_t i = loopIndexQueue.size()-1; i < loopIndexQueue.size(); i++) { // Pairwise Consistency check #if 0 // Eigen::Isometry3d T_odom_jl = Eigen::Isometry3d( keyframes[loopIndexQueue.at(i).second]->node->estimate().matrix().inverse() * new_keyframe->node->estimate().matrix()); Eigen::Isometry3d T_odom_jl = keyframes[loopIndexQueue.at(i).second]->odom_scan2scan.inverse() * new_keyframe->odom_scan2scan; Eigen::Isometry3d T_lc_lk = loopPoseQueue.at(i); // Eigen::Isometry3d T_odom_ki = Eigen::Isometry3d( keyframes[loopKeyPre]->node->estimate().matrix().inverse() * keyframes[loopIndexQueue.at(i).first]->node->estimate().matrix()); Eigen::Isometry3d T_odom_ki = keyframes[loopKeyPre]->odom_scan2scan.inverse() * keyframes[loopIndexQueue.at(i).first]->odom_scan2scan; Eigen::Isometry3d T_err_ij_kl = T_lc_ij * T_odom_jl * T_lc_lk * T_odom_ki; #endif Eigen::Isometry3d T_odom_li = keyframes[loopKeyPre]->odom_scan2scan.inverse() * keyframes[loopIndexQueue.at(i).second]->odom_scan2scan; Eigen::Isometry3d T_lc_kl = loopPoseQueue.at(i).inverse(); Eigen::Isometry3d T_odom_jk = keyframes[loopIndexQueue.at(i).first]->odom_scan2scan.inverse() * new_keyframe->odom_scan2scan; Eigen::Isometry3d T_err_ij_kl = T_lc_ij * T_odom_li * T_lc_kl * T_odom_jk; Eigen::AngleAxisd rotation_vector; rotation_vector.fromRotationMatrix(T_err_ij_kl.rotation()); // int num_between = loopKeyCur - loopKeyPre - (loopIndexQueue.at(i).first - loopIndexQueue.at(i).second); double pcc_err_trans = T_err_ij_kl.translation().norm(); cout << "T_err_ij_kl translation: " << T_err_ij_kl.translation().norm() << endl; double pcc_err_rot = rotation_vector.angle(); if(pcc_err_trans > pairwise_check_trans_thresh || pcc_err_rot > pairwise_check_rot_thresh){ ROS_INFO("\033[31m Pairwise check invalid, Translation Error: %lf, rotation Error: %lf \033[0m", pcc_err_trans, pcc_err_rot); return nullptr; } } } std::cout << "\033[32m" << " Add this ScanContext loop. ICP fitness test passed ( Fitness Score " << registration->getFitnessScore() << " < " << historyKeyframeFitnessScore << "). " << "\033[0m" << std::endl; // Publish context as image Eigen::MatrixXd& cur_sc = scManager->polarcontexts_.at(loopKeyCur); Eigen::MatrixXd& pre_sc = scManager->polarcontexts_.at(loopKeyPre); cv::Mat mat_cur_sc = makeSCImage(cur_sc); cv::Mat color_cur_sc = getColorImage(mat_cur_sc); cv::Mat mat_pre_sc = makeSCImage(pre_sc); cv::Mat color_pre_sc = getColorImage(mat_pre_sc); sensor_msgs::ImagePtr cur_img_msg=cv_bridge::CvImage(std_msgs::Header(),"bgr8",color_cur_sc).toImageMsg(); pub_cur_sc.publish(cur_img_msg); sensor_msgs::ImagePtr pre_img_msg=cv_bridge::CvImage(std_msgs::Header(),"bgr8",color_pre_sc).toImageMsg(); pub_pre_sc.publish(pre_img_msg); // robust kernel for a SC loop Eigen::MatrixXd information = inf_calclator->calc_information_matrix(cureKeyframeCloud, prevKeyframeCloud, T_lc_ij); if(new_keyframe->accum_distance > last_loop_edge_accum_distance){ last_loop_edge_accum_distance = new_keyframe->accum_distance; last_loop_edge_index = new_keyframe->index; } // Add pose constraint mtx.lock(); loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre)); loopPoseQueue.push_back(T_lc_ij); loopInfoQueue.push_back(information); mtx.unlock(); // Add loop constriant loopIndexContainer.insert(std::pair(loopKeyCur, loopKeyPre)); // giseop for multimap return std::make_shared(new_keyframe, keyframes[loopKeyPre], T_lc_ij.matrix().cast()); } cv::Mat LoopDetector::makeSCImage(Eigen::MatrixXd src_mat) { // double max_ = src_mat.rowwise().maxCoeff().colwise().maxCoeff().value(); // double min_ = src_mat.rowwise().minCoeff().colwise().minCoeff().value(); double max_ = 35; double min_ = 0; cv::Mat ssc = cv::Mat::zeros(cv::Size(scManager->PC_NUM_SECTOR, scManager->PC_NUM_RING), CV_8U); for (int i = 0; i < scManager->PC_NUM_SECTOR; i++) { // 60 for (int j = 0; j < scManager->PC_NUM_RING; j++) { // 20 double& d = src_mat(j,i); int uc = round((d - min_) / (max_ - min_) * 255); ssc.at(j, i) = uc; } } return ssc; } cv::Mat LoopDetector::getColorImage(cv::Mat &desc) { cv::Mat out = cv::Mat::zeros(desc.size(), CV_8UC3); for (int i = 0; i < desc.rows; ++i) { for (int j = 0; j < desc.cols; ++j) { int value_mono = (int)desc.at(i, j);//std::get<2>(_argmax_to_rgb[(int)desc.at(i, j)]); if (value_mono == 0){ out.at(i, j)[0] = 255; out.at(i, j)[1] = 255; out.at(i, j)[2] = 255; } else { Eigen::Vector3d bgr = monoToRainbow(value_mono); out.at(i, j)[0] = (int)bgr(0);//255 - value_mono; out.at(i, j)[1] = (int)bgr(1);//0;//std::min(std::max(128 - value_mono, 0), value_mono); out.at(i, j)[2] = (int)bgr(2);//value_mono; } } } return out; } double LoopDetector::get_distance_thresh() const { return distance_thresh; } #if 0 /** * @brief To validate a loop candidate this function applies a scan matching between keyframes consisting the loop. If they are matched well, the loop is added to the pose graph * @param candidate_keyframes candidate keyframes of loop start * @param new_keyframe loop end keyframe * @param graph_slam graph slam */ Loop::Ptr LoopDetector::matching(const std::vector& candidate_keyframes, const KeyFrame::Ptr& new_keyframe) { if(candidate_keyframes.empty()) { return nullptr; } registration->setInputTarget(new_keyframe->cloud); double best_score = std::numeric_limits::max(); KeyFrame::Ptr best_matched; Eigen::Matrix4f relative_pose; std::cout << std::endl; std::cout << "--- loop detection ---" << std::endl; std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl; std::cout << "matching" << std::flush; auto t1 = ros::Time::now(); pcl::PointCloud::Ptr aligned(new pcl::PointCloud()); for(const auto& candidate : candidate_keyframes) { registration->setInputSource(candidate->cloud); Eigen::Isometry3d new_keyframe_estimate = new_keyframe->node->estimate(); new_keyframe_estimate.linear() = Eigen::Quaterniond(new_keyframe_estimate.linear()).normalized().toRotationMatrix(); Eigen::Isometry3d candidate_estimate = candidate->node->estimate(); candidate_estimate.linear() = Eigen::Quaterniond(candidate_estimate.linear()).normalized().toRotationMatrix(); Eigen::Matrix4f guess = (new_keyframe_estimate.inverse() * candidate_estimate).matrix().cast(); guess(2, 3) = 0.0; registration->align(*aligned, guess); std::cout << "." << std::flush; double score = registration->getFitnessScore(fitness_score_max_range); if(!registration->hasConverged() || score > best_score) { continue; } best_score = score; best_matched = candidate; relative_pose = registration->getFinalTransformation(); } auto t2 = ros::Time::now(); std::cout << " done" << std::endl; std::cout << "best_score: " << boost::format("%.3f") % best_score << " time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl; if(best_score > fitness_score_thresh) { std::cout << "loop not found..." << std::endl; return nullptr; } std::cout << "loop found!!" << std::endl; std::cout << "relative pose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl; last_loop_edge_accum_distance = new_keyframe->accum_distance; return std::make_shared(new_keyframe, best_matched, relative_pose); } #endif } // namespace radar_graph_slam ================================================ FILE: src/radar_graph_slam/map2odom_publisher.py ================================================ #!/usr/bin/python # SPDX-License-Identifier: BSD-2-Clause import tf import rospy from geometry_msgs.msg import * class Map2OdomPublisher: def __init__(self): self.broadcaster = tf.TransformBroadcaster() self.subscriber = rospy.Subscriber('/radar_graph_slam/odom2pub', TransformStamped, self.callback) def callback(self, odom_msg): self.odom_msg = odom_msg def spin(self): if not hasattr(self, 'odom_msg'): self.broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), 'odom', 'map') return pose = self.odom_msg.transform pos = (pose.translation.x, pose.translation.y, pose.translation.z) quat = (pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w) map_frame_id = self.odom_msg.header.frame_id odom_frame_id = self.odom_msg.child_frame_id self.broadcaster.sendTransform(pos, quat, rospy.Time.now(), odom_frame_id, map_frame_id) def main(): rospy.init_node('map2odom_publisher') node = Map2OdomPublisher() rate = rospy.Rate(10.0) while not rospy.is_shutdown(): node.spin() rate.sleep() if __name__ == '__main__': main() ================================================ FILE: src/radar_graph_slam/map_cloud_generator.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include namespace radar_graph_slam { MapCloudGenerator::MapCloudGenerator() {} MapCloudGenerator::~MapCloudGenerator() {} pcl::PointCloud::Ptr MapCloudGenerator::generate(const std::vector& keyframes, double resolution) const { if(keyframes.empty()) { std::cerr << "warning: keyframes empty!!" << std::endl; return nullptr; } pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); cloud->reserve(keyframes.front()->cloud->size() * keyframes.size()); for(const auto& keyframe : keyframes) { Eigen::Matrix4f pose = keyframe->pose.matrix().cast(); for(const auto& src_pt : keyframe->cloud->points) { double d = src_pt.getVector3fMap().norm(); if (d > 50) continue; PointT dst_pt; dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap(); dst_pt.intensity = src_pt.intensity; cloud->push_back(dst_pt); } } cloud->width = cloud->size(); cloud->height = 1; cloud->is_dense = false; if (resolution <=0.0) return cloud; // To get unfiltered point cloud with intensity pcl::octree::OctreePointCloud octree(resolution); octree.setInputCloud(cloud); octree.addPointsFromInputCloud(); pcl::PointCloud::Ptr filtered(new pcl::PointCloud()); octree.getOccupiedVoxelCenters(filtered->points); filtered->width = filtered->size(); filtered->height = 1; filtered->is_dense = false; return filtered; } } // namespace radar_graph_slam ================================================ FILE: src/radar_graph_slam/registrations.cpp ================================================ // SPDX-License-Identifier: BSD-2-Clause #include #include #include #include #include #include #include #include #include #include #ifdef USE_VGICP_CUDA #include #endif namespace radar_graph_slam { pcl::Registration::Ptr select_registration_method(ros::NodeHandle& pnh) { using PointT = pcl::PointXYZI; // select a registration method (ICP, GICP, NDT) std::string registration_method = pnh.param("registration_method", "NDT_OMP"); if(registration_method == "FAST_GICP") { std::cout << "registration: FAST_GICP" << std::endl; fast_gicp::FastGICP::Ptr gicp(new fast_gicp::FastGICP()); gicp->setNumThreads(pnh.param("reg_num_threads", 0)); gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); gicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); gicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); return gicp; } else if(registration_method == "FAST_APDGICP") { std::cout << "registration: FAST_APDGICP" << std::endl; fast_gicp::FastAPDGICP::Ptr apdgicp(new fast_gicp::FastAPDGICP()); apdgicp->setNumThreads(pnh.param("reg_num_threads", 0)); apdgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); apdgicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); apdgicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); apdgicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); apdgicp->setDistVar(pnh.param("dist_var", 0.86)); apdgicp->setAzimuthVar(pnh.param("azimuth_var", 0.5)); apdgicp->setElevationVar(pnh.param("elevation_var", 1.0)); return apdgicp; } #ifdef USE_VGICP_CUDA else if(registration_method == "FAST_VGICP_CUDA") { std::cout << "registration: FAST_VGICP_CUDA" << std::endl; fast_gicp::FastVGICPCuda::Ptr vgicp(new fast_gicp::FastVGICPCuda()); vgicp->setResolution(pnh.param("reg_resolution", 1.0)); vgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); vgicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); vgicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); return vgicp; } #endif else if(registration_method == "FAST_VGICP") { std::cout << "registration: FAST_VGICP" << std::endl; fast_gicp::FastVGICP::Ptr vgicp(new fast_gicp::FastVGICP()); vgicp->setNumThreads(pnh.param("reg_num_threads", 0)); vgicp->setResolution(pnh.param("reg_resolution", 1.0)); vgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); vgicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); vgicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); return vgicp; } else if(registration_method == "ICP") { std::cout << "registration: ICP" << std::endl; pcl::IterativeClosestPoint::Ptr icp(new pcl::IterativeClosestPoint()); icp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); icp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); icp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); icp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); return icp; } else if(registration_method.find("GICP") != std::string::npos) { if(registration_method.find("OMP") == std::string::npos) { std::cout << "registration: GICP" << std::endl; pcl::GeneralizedIterativeClosestPoint::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint()); gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); gicp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); gicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); gicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); gicp->setMaximumOptimizerIterations(pnh.param("reg_max_optimizer_iterations", 20)); return gicp; } else { std::cout << "registration: GICP_OMP" << std::endl; pclomp::GeneralizedIterativeClosestPoint::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint()); gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); gicp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); gicp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); gicp->setCorrespondenceRandomness(pnh.param("reg_correspondence_randomness", 20)); gicp->setMaximumOptimizerIterations(pnh.param("reg_max_optimizer_iterations", 20)); return gicp; } } else { if(registration_method.find("NDT") == std::string::npos) { std::cerr << "warning: unknown registration type(" << registration_method << ")" << std::endl; std::cerr << " : use NDT" << std::endl; } double ndt_resolution = pnh.param("reg_resolution", 0.5); if(registration_method.find("OMP") == std::string::npos) { std::cout << "registration: NDT " << ndt_resolution << std::endl; pcl::NormalDistributionsTransform::Ptr ndt(new pcl::NormalDistributionsTransform()); ndt->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); ndt->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); ndt->setResolution(ndt_resolution); return ndt; } else { int num_threads = pnh.param("reg_num_threads", 0); std::string nn_search_method = pnh.param("reg_nn_search_method", "DIRECT7"); std::cout << "registration: NDT_OMP " << nn_search_method << " " << ndt_resolution << " (" << num_threads << " threads)" << std::endl; pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); if(num_threads > 0) { ndt->setNumThreads(num_threads); } ndt->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); ndt->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); ndt->setResolution(ndt_resolution); if(nn_search_method == "KDTREE") { ndt->setNeighborhoodSearchMethod(pclomp::KDTREE); } else if(nn_search_method == "DIRECT1") { ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1); } else { ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7); } return ndt; } } return nullptr; } } // namespace radar_graph_slam ================================================ FILE: srv/DumpGraph.srv ================================================ string destination --- bool success ================================================ FILE: srv/SaveMap.srv ================================================ bool utm float32 resolution string destination --- bool success