diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..65713c73 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,119 @@ +########################################################### +# # +# Copyright (c) # +# # +# The Verifiable & Control-Theoretic Robotics (VECTR) Lab # +# University of California, Los Angeles # +# # +# Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez # +# Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu # +# # +########################################################### + +cmake_minimum_required(VERSION 3.12.4) +project(direct_lidar_inertial_odometry) + +add_compile_options(-std=c++14) +set(CMAKE_BUILD_TYPE "Release") + +find_package( PCL REQUIRED ) +include_directories(${PCL_INCLUDE_DIRS}) +add_definitions(${PCL_DEFINITIONS}) +link_directories(${PCL_LIBRARY_DIRS}) + +find_package( Eigen3 REQUIRED ) +include_directories(${EIGEN3_INCLUDE_DIR}) + +include(FindOpenMP) +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +else(OPENMP_FOUND) + message("ERROR: OpenMP could not be found.") +endif(OPENMP_FOUND) + +set(CMAKE_THREAD_PREFER_PTHREAD TRUE) +set(THREADS_PREFER_PTHREAD_FLAG TRUE) +find_package(Threads REQUIRED) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + pcl_ros + message_generation +) + +add_service_files( + DIRECTORY srv + FILES + save_pcd.srv +) + +generate_messages( + DEPENDENCIES + sensor_msgs + std_msgs + geometry_msgs +) + +catkin_package( + CATKIN_DEPENDS + roscpp + std_msgs + sensor_msgs + geometry_msgs + pcl_ros + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + nano_gicp + nanoflann +) + +include_directories(include) +include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) + +# Not all machines have available +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) +file(WRITE ${CMAKE_BINARY_DIR}/test_cpuid.cpp "#include ") +try_compile(HAS_CPUID ${CMAKE_BINARY_DIR} ${CMAKE_BINARY_DIR}/test_cpuid.cpp) +file(REMOVE ${CMAKE_BINARY_DIR}/test_cpuid.cpp) +if(HAS_CPUID) + add_compile_definitions(HAS_CPUID) +endif() + +# NanoFLANN +add_library(nanoflann STATIC + src/nano_gicp/nanoflann.cc +) +target_link_libraries(nanoflann ${PCL_LIBRARIES}) + +# NanoGICP +add_library(nano_gicp STATIC + src/nano_gicp/lsq_registration.cc + src/nano_gicp/nano_gicp.cc +) +target_link_libraries(nano_gicp ${PCL_LIBRARIES} ${OpenMP_LIBS} nanoflann) + +# Odometry Node +add_executable(dlio_odom_node src/dlio/odom_node.cc src/dlio/odom.cc) +add_dependencies(dlio_odom_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) +target_compile_options(dlio_odom_node PRIVATE ${OpenMP_FLAGS}) +target_link_libraries(dlio_odom_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads nano_gicp) + +# Mapping Node +add_executable (dlio_map_node src/dlio/map_node.cc src/dlio/map.cc) +add_dependencies(dlio_map_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) +target_compile_options(dlio_map_node PRIVATE ${OpenMP_FLAGS}) +target_link_libraries(dlio_map_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenMP_LIBS} Threads::Threads) + +# Binaries +install( TARGETS dlio_odom_node dlio_map_node + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +install( DIRECTORY cfg launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/LICENSE b/LICENSE new file mode 100644 index 00000000..fc9b935d --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) Kenny J. Chen, Ryan Nemiroff, and Brett T. Lopez + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md index 14963d74..bea14a0f 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,107 @@ -# direct_lidar_inertial_odometry -[IEEE ICRA'23] A new lightweight LiDAR-inertial odometry algorithm with a novel coarse-to-fine approach in constructing continuous-time trajectories for precise motion correction. +# Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction + +#### [[Paper](https://arxiv.org/abs/2203.03749)] [[Video](https://www.youtube.com/watch?v=4-oXjG8ow10)] [[Presentation](https://www.youtube.com/watch?v=Hmiw66KZ1tU)] + +DLIO is a new lightweight LiDAR-inertial odometry algorithm with a novel coarse-to-fine approach in constructing continuous-time trajectories for precise motion correction. It features several algorithmic improvements over its predecessor, [DLO](https://github.com/vectr-ucla/direct_lidar_odometry), and was presented at the IEEE International Conference on Robotics and Automation (ICRA) in London, UK in 2023. + +
+

+ drawing +

+ +## Instructions + +### Sensor Setup +DLIO has been extensively tested using a variety of sensor configurations. DLIO requires only a point cloud input of type `sensor_msgs::PointCloud2` and a 6-axis IMU input of type `sensor_msgs::Imu`. + +For best performance, extrinsic calibration between the LiDAR/IMU sensors and the robot's center-of-gravity should be inputted into `cfg/dlio.yaml`. If the exact values of these are unavailable, a rough LiDAR-to-IMU extrinsics can also be used (note however that performance will be degraded). + +IMU intrinsics are also necessary for best performance, and there are several open-source calibration tools to get these values. These values should also go into `cfg/dlio.yaml`. In practice however, if you are just testing this work, using the default ideal values and performing the initial calibration procedure should be fine. + +Also note that the LiDAR and IMU sensors _need_ to be properly time-synchronized, otherwise DLIO will not work. We recommend using a LiDAR with an integrated IMU (such as an Ouster) for simplicity of extrinsics and synchronization. + +### Dependencies +The following has been verified to be compatible, although other configurations may work too: + +- Ubuntu 20.04 +- ROS Noetic (`roscpp`, `std_msgs`, `sensor_msgs`, `geometry_msgs`, `nav_msgs`, `pcl_ros`) +- C++ 14 +- CMake >= `3.12.4` +- OpenMP >= `4.5` +- Point Cloud Library >= `1.10.0` +- Eigen >= `3.3.7` + +```sh +sudo apt install libomp-dev libpcl-dev libeigen3-dev +``` + +DLIO currently only supports ROS1, but we welcome any contributions by the community to add ROS2 support! + +### Compiling +Compile using the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/) package via: + +```sh +mkdir ws && cd ws && mkdir src && catkin init && cd src +git clone https://github.com/vectr-ucla/direct_lidar_inertial_odometry.git +catkin build +``` + +### Execution +After compiling, source the workspace and execute via: + +```sh +roslaunch direct_lidar_inertial_odometry dlio.launch \ + rviz:={true, false} \ + pointcloud_topic:=/robot/lidar \ + imu_topic:=/robot/imu +``` + +Be sure to change the topic names to your corresponding topics. Alternatively, edit the launch file directly if desired. If successful, you should see the following output in your terminal: +
+

+ drawing +

+ +### Services +To save DLIO's generated map into `.pcd` format, call the following service: + +```sh +rosservice call /robot/dlio_map/save_pcd LEAF_SIZE SAVE_PATH +``` + +### Test Data +For your convenience, we provide test data [here](https://drive.google.com/file/d/1Sp_Mph4rekXKY2euxYxv6SD6WIzB-wVU/view?usp=sharing) (1.2GB, 1m 13s) of an aggressive motion to test our motion correction scheme. Try this data with both deskewing on and off! + +
+

+ drawing +

+ +## Citation +If you found this work useful, please cite our manuscript: + +```bibtex +@article{chen2022dlio, + title={Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction}, + author={Chen, Kenny and Nemiroff, Ryan and Lopez, Brett T}, + journal={IEEE International Conference on Robotics and Automation (ICRA)}, + year={2023} +} +``` + +## Acknowledgements + +We thank the authors of the [FastGICP](https://github.com/SMRT-AIST/fast_gicp) and [NanoFLANN](https://github.com/jlblancoc/nanoflann) open-source packages: + +- Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration,” in _IEEE International Conference on Robotics and Automation (ICRA)_, IEEE, 2021, pp. 11 054–11 059. +- Jose Luis Blanco and Pranjal Kumar Rai, “NanoFLANN: a C++ Header-Only Fork of FLANN, A Library for Nearest Neighbor (NN) with KD-Trees,” https://github.com/jlblancoc/nanoflann, 2014. + +We would also like to thank Helene Levy and David Thorne for their help with data collection. + +## License +This work is licensed under the terms of the MIT license. + +
+

+ drawing +

\ No newline at end of file diff --git a/cfg/dlio.yaml b/cfg/dlio.yaml new file mode 100644 index 00000000..0675f1b5 --- /dev/null +++ b/cfg/dlio.yaml @@ -0,0 +1,45 @@ +########################################################### +# # +# Copyright (c) # +# # +# The Verifiable & Control-Theoretic Robotics (VECTR) Lab # +# University of California, Los Angeles # +# # +# Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez # +# Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu # +# # +########################################################### + +dlio: + + version: 1.0.0 + + adaptive: true + + pointcloud: + deskew: true + voxelize: true + + imu: + calibration: true + intrinsics: + accel: + bias: [ 0.0, 0.0, 0.0 ] + sm: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] + gyro: + bias: [ 0.0, 0.0, 0.0 ] + + extrinsics: + baselink2imu: + t: [ 0.006253, -0.011775, 0.007645 ] + R: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] + baselink2lidar: + t: [ 0., 0., 0. ] + R: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1. ] + diff --git a/cfg/params.yaml b/cfg/params.yaml new file mode 100644 index 00000000..bcb3573c --- /dev/null +++ b/cfg/params.yaml @@ -0,0 +1,72 @@ +########################################################### +# # +# Copyright (c) # +# # +# The Verifiable & Control-Theoretic Robotics (VECTR) Lab # +# University of California, Los Angeles # +# # +# Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez # +# Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu # +# # +########################################################### + +dlio: + + frames: + odom: odom + baselink: base_link + lidar: lidar + imu: imu + + map: + waitUntilMove: true + dense: + filtered: true + sparse: + frequency: 1.0 + leafSize: 0.25 + + odom: + + gravity: 9.80665 + + imu: + approximateGravity: false + calibration: + gyro: true + accel: true + time: 3 + bufferSize: 5000 + + preprocessing: + cropBoxFilter: + size: 1.0 + voxelFilter: + res: 0.25 + + keyframe: + threshD: 1.0 + threshR: 45.0 + + submap: + keyframe: + knn: 10 + kcv: 10 + kcc: 10 + gicp: + minNumPoints: 64 + kCorrespondences: 16 + maxCorrespondenceDistance: 0.5 + maxIterations: 32 + transformationEpsilon: 0.01 + rotationEpsilon: 0.01 + initLambdaFactor: 1e-9 + + geo: + Kp: 4.5 + Kv: 11.25 + Kq: 4.0 + Kab: 2.25 + Kgb: 1.0 + abias_max: 5.0 + gbias_max: 0.5 diff --git a/doc/gif/aggressive.gif b/doc/gif/aggressive.gif new file mode 100644 index 00000000..d12b916b Binary files /dev/null and b/doc/gif/aggressive.gif differ diff --git a/doc/img/dlio.png b/doc/img/dlio.png new file mode 100644 index 00000000..66164945 Binary files /dev/null and b/doc/img/dlio.png differ diff --git a/doc/img/terminal.png b/doc/img/terminal.png new file mode 100644 index 00000000..a6f1413a Binary files /dev/null and b/doc/img/terminal.png differ diff --git a/doc/img/ucla.png b/doc/img/ucla.png new file mode 100644 index 00000000..33a11783 Binary files /dev/null and b/doc/img/ucla.png differ diff --git a/include/dlio/dlio.h b/include/dlio/dlio.h new file mode 100644 index 00000000..2c76835f --- /dev/null +++ b/include/dlio/dlio.h @@ -0,0 +1,106 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +// SYSTEM +#include + +#ifdef HAS_CPUID +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +template +std::string to_string_with_precision(const T a_value, const int n = 6) +{ + std::ostringstream out; + out.precision(n); + out << std::fixed << a_value; + return out.str(); +} + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include + +// BOOST +#include +#include +#include +#include +#include + +// PCL +#define PCL_NO_PRECOMPILE +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// DLIO +#include +#include + +namespace dlio { + enum class SensorType { OUSTER, VELODYNE, UNKNOWN }; + + class OdomNode; + class MapNode; + + struct Point { + Point(): data{0.f, 0.f, 0.f, 1.f} {} + + PCL_ADD_POINT4D; + float intensity; // intensity + union { + float time; // time since beginning of scan in seconds + std::uint32_t t; // time since beginning of scan in nanoseconds + }; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + } EIGEN_ALIGN16; +} + +POINT_CLOUD_REGISTER_POINT_STRUCT(dlio::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, time, time) + (std::uint32_t, t, t)) + +typedef dlio::Point PointType; diff --git a/include/dlio/map.h b/include/dlio/map.h new file mode 100644 index 00000000..a9fbd3d9 --- /dev/null +++ b/include/dlio/map.h @@ -0,0 +1,50 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +#include "dlio/dlio.h" + +class dlio::MapNode { + +public: + + MapNode(ros::NodeHandle node_handle); + ~MapNode(); + + void start(); + +private: + + void getParams(); + + void publishTimer(const ros::TimerEvent& e); + void callbackKeyframe(const sensor_msgs::PointCloud2ConstPtr& keyframe); + + bool savePcd(direct_lidar_inertial_odometry::save_pcd::Request& req, + direct_lidar_inertial_odometry::save_pcd::Response& res); + + ros::NodeHandle nh; + ros::Timer publish_timer; + + ros::Subscriber keyframe_sub; + ros::Publisher map_pub; + + ros::ServiceServer save_pcd_srv; + + pcl::PointCloud::Ptr dlio_map; + pcl::VoxelGrid voxelgrid; + + std::string odom_frame; + + double publish_freq_; + double leaf_size_; + +}; diff --git a/include/dlio/odom.h b/include/dlio/odom.h new file mode 100644 index 00000000..ae74051d --- /dev/null +++ b/include/dlio/odom.h @@ -0,0 +1,335 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +#include "dlio/dlio.h" + +class dlio::OdomNode { + +public: + + OdomNode(ros::NodeHandle node_handle); + ~OdomNode(); + + void start(); + +private: + + struct State; + struct ImuMeas; + + void getParams(); + + void callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc); + void callbackImu(const sensor_msgs::Imu::ConstPtr& imu); + + void publishPose(const ros::TimerEvent& e); + + void publishToROS(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud); + void publishCloud(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud); + void publishKeyframe(std::pair, + pcl::PointCloud::ConstPtr> kf, ros::Time timestamp); + + void getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc); + void preprocessPoints(); + void deskewPointcloud(); + void initializeInputTarget(); + void setInputSource(); + + void initializeDLIO(); + + void getNextPose(); + bool imuMeasFromTimeRange(double start_time, double end_time, + boost::circular_buffer::reverse_iterator& begin_imu_it, + boost::circular_buffer::reverse_iterator& end_imu_it); + std::vector> + integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init, + const std::vector& sorted_timestamps); + std::vector> + integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init, + const std::vector& sorted_timestamps, + boost::circular_buffer::reverse_iterator begin_imu_it, + boost::circular_buffer::reverse_iterator end_imu_it); + void propagateGICP(); + + void propagateState(); + void updateState(); + + void setAdaptiveParams(); + void setKeyframeCloud(); + + void computeMetrics(); + void computeSpaciousness(); + void computeDensity(); + + sensor_msgs::Imu::Ptr transformImu(const sensor_msgs::Imu::ConstPtr& imu); + + void updateKeyframes(); + void computeConvexHull(); + void computeConcaveHull(); + void pushSubmapIndices(std::vector dists, int k, std::vector frames); + void buildSubmap(State vehicle_state); + void buildKeyframesAndSubmap(State vehicle_state); + void pauseSubmapBuildIfNeeded(); + + void debug(); + + ros::NodeHandle nh; + ros::Timer publish_timer; + + // Subscribers + ros::Subscriber lidar_sub; + ros::Subscriber imu_sub; + + // Publishers + ros::Publisher odom_pub; + ros::Publisher pose_pub; + ros::Publisher path_pub; + ros::Publisher kf_pose_pub; + ros::Publisher kf_cloud_pub; + ros::Publisher deskewed_pub; + + // ROS Msgs + nav_msgs::Odometry odom_ros; + geometry_msgs::PoseStamped pose_ros; + nav_msgs::Path path_ros; + geometry_msgs::PoseArray kf_pose_ros; + + // Flags + std::atomic dlio_initialized; + std::atomic first_valid_scan; + std::atomic first_imu_received; + std::atomic imu_calibrated; + std::atomic submap_hasChanged; + std::atomic gicp_hasConverged; + std::atomic deskew_status; + std::atomic deskew_size; + + // Threads + std::thread publish_thread; + std::thread publish_keyframe_thread; + std::thread metrics_thread; + std::thread debug_thread; + + // Trajectory + std::vector> trajectory; + double length_traversed; + + // Keyframes + std::vector, + pcl::PointCloud::ConstPtr>> keyframes; + std::vector keyframe_timestamps; + std::vector> keyframe_normals; + std::vector> keyframe_transformations; + std::mutex keyframes_mutex; + + // Sensor Type + dlio::SensorType sensor; + + // Frames + std::string odom_frame; + std::string baselink_frame; + std::string lidar_frame; + std::string imu_frame; + + // Preprocessing + pcl::CropBox crop; + pcl::VoxelGrid voxel; + + // Point Clouds + pcl::PointCloud::ConstPtr original_scan; + pcl::PointCloud::ConstPtr deskewed_scan; + pcl::PointCloud::ConstPtr current_scan; + + // Keyframes + pcl::PointCloud::ConstPtr keyframe_cloud; + int num_processed_keyframes; + + pcl::ConvexHull convex_hull; + pcl::ConcaveHull concave_hull; + std::vector keyframe_convex; + std::vector keyframe_concave; + + // Submap + pcl::PointCloud::ConstPtr submap_cloud; + std::shared_ptr submap_normals; + std::shared_ptr> submap_kdtree; + + std::vector submap_kf_idx_curr; + std::vector submap_kf_idx_prev; + + bool new_submap_is_ready; + std::future submap_future; + std::condition_variable submap_build_cv; + bool main_loop_running; + std::mutex main_loop_running_mutex; + + // Timestamps + ros::Time scan_header_stamp; + double scan_stamp; + double prev_scan_stamp; + double scan_dt; + std::vector comp_times; + std::vector imu_rates; + std::vector lidar_rates; + + double first_scan_stamp; + double elapsed_time; + + // GICP + nano_gicp::NanoGICP gicp; + nano_gicp::NanoGICP gicp_temp; + + // Transformations + Eigen::Matrix4f T, T_prior, T_corr; + Eigen::Quaternionf q_final; + + Eigen::Vector3f origin; + + struct Extrinsics { + struct SE3 { + Eigen::Vector3f t; + Eigen::Matrix3f R; + }; + SE3 baselink2imu; + SE3 baselink2lidar; + Eigen::Matrix4f baselink2imu_T; + Eigen::Matrix4f baselink2lidar_T; + }; Extrinsics extrinsics; + + // IMU + ros::Time imu_stamp; + double first_imu_stamp; + double prev_imu_stamp; + double imu_dp, imu_dq_deg; + + struct ImuMeas { + double stamp; + double dt; // defined as the difference between the current and the previous measurement + Eigen::Vector3f ang_vel; + Eigen::Vector3f lin_accel; + }; ImuMeas imu_meas; + + boost::circular_buffer imu_buffer; + std::mutex mtx_imu; + std::condition_variable cv_imu_stamp; + + static bool comparatorImu(ImuMeas m1, ImuMeas m2) { + return (m1.stamp < m2.stamp); + }; + + // Geometric Observer + struct Geo { + bool first_opt_done; + std::mutex mtx; + double dp; + double dq_deg; + Eigen::Vector3f prev_p; + Eigen::Quaternionf prev_q; + Eigen::Vector3f prev_vel; + }; Geo geo; + + // State Vector + struct ImuBias { + Eigen::Vector3f gyro; + Eigen::Vector3f accel; + }; + + struct Frames { + Eigen::Vector3f b; + Eigen::Vector3f w; + }; + + struct Velocity { + Frames lin; + Frames ang; + }; + + struct State { + Eigen::Vector3f p; // position in world frame + Eigen::Quaternionf q; // orientation in world frame + Velocity v; + ImuBias b; // imu biases in body frame + }; State state; + + struct Pose { + Eigen::Vector3f p; // position in world frame + Eigen::Quaternionf q; // orientation in world frame + }; + Pose lidarPose; + Pose imuPose; + + // Metrics + struct Metrics { + std::vector spaciousness; + std::vector density; + }; Metrics metrics; + + std::string cpu_type; + std::vector cpu_percents; + clock_t lastCPU, lastSysCPU, lastUserCPU; + int numProcessors; + + // Parameters + std::string version_; + int num_threads_; + + bool deskew_; + + double gravity_; + + bool adaptive_params_; + + double obs_submap_thresh_; + double obs_keyframe_thresh_; + double obs_keyframe_lag_; + + double keyframe_thresh_dist_; + double keyframe_thresh_rot_; + + int submap_knn_; + int submap_kcv_; + int submap_kcc_; + double submap_concave_alpha_; + + bool densemap_filtered_; + bool wait_until_move_; + + double crop_size_; + + bool vf_use_; + double vf_res_; + + bool imu_calibrate_; + bool calibrate_gyro_; + bool calibrate_accel_; + bool gravity_align_; + double imu_calib_time_; + int imu_buffer_size_; + Eigen::Matrix3f imu_accel_sm_; + + int gicp_min_num_points_; + int gicp_k_correspondences_; + double gicp_max_corr_dist_; + int gicp_max_iter_; + double gicp_transformation_ep_; + double gicp_rotation_ep_; + double gicp_init_lambda_factor_; + + double geo_Kp_; + double geo_Kv_; + double geo_Kq_; + double geo_Kab_; + double geo_Kgb_; + double geo_abias_max_; + double geo_gbias_max_; + +}; diff --git a/include/nano_gicp/lsq_registration.h b/include/nano_gicp/lsq_registration.h new file mode 100644 index 00000000..74039f55 --- /dev/null +++ b/include/nano_gicp/lsq_registration.h @@ -0,0 +1,169 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * BSD 3-Clause License + * + * Copyright (c) 2020, SMRT-AIST + * 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. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. + *************************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace nano_gicp { + +enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt }; + +inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) { + Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); + skew(0, 1) = -x[2]; + skew(0, 2) = x[1]; + skew(1, 0) = x[2]; + skew(1, 2) = -x[0]; + skew(2, 0) = -x[1]; + skew(2, 1) = x[0]; + + return skew; +} + +inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) { + Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); + skew(0, 1) = -x[2]; + skew(0, 2) = x[1]; + skew(1, 0) = x[2]; + skew(1, 2) = -x[0]; + skew(2, 0) = -x[1]; + skew(2, 1) = x[0]; + + return skew; +} + +inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { + double theta_sq = omega.dot(omega); + + double theta; + double imag_factor; + double real_factor; + if(theta_sq < 1e-10) { + theta = 0; + double theta_quad = theta_sq * theta_sq; + imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; + real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; + } else { + theta = std::sqrt(theta_sq); + double half_theta = 0.5 * theta; + imag_factor = std::sin(half_theta) / theta; + real_factor = std::cos(half_theta); + } + + return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); +} + +template +class LsqRegistration : public pcl::Registration { +public: + using Scalar = float; + using Matrix4 = typename pcl::Registration::Matrix4; + + using PointCloudSource = typename pcl::Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename pcl::Registration::PointCloudTarget; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + +protected: + using pcl::Registration::input_; + using pcl::Registration::nr_iterations_; + using pcl::Registration::final_transformation_; + using pcl::Registration::converged_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + LsqRegistration(); + virtual ~LsqRegistration(); + + void setRotationEpsilon(double eps); + void setTransformationEpsilon(double eps); + void setMaximumIterations(int iter); + void setInitialLambdaFactor(double init_lambda_factor); + void setDebugPrint(bool lm_debug_print); + + const Eigen::Matrix& getFinalHessian() const; + double getFinalError() const; + + virtual void swapSourceAndTarget() {} + virtual void clearSource() {} + virtual void clearTarget() {} + +protected: + virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; + + bool is_converged(const Eigen::Isometry3d& delta) const; + + virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) = 0; + virtual double compute_error(const Eigen::Isometry3d& trans) = 0; + + bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); + bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); + bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); + +protected: + double rotation_epsilon_; + double transformation_epsilon_; + int max_iterations_; + + + LSQ_OPTIMIZER_TYPE lsq_optimizer_type_; + int lm_max_iterations_; + double lm_init_lambda_factor_; + double lm_lambda_; + bool lm_debug_print_; + + Eigen::Matrix final_hessian_; + double final_error_; +}; +} // namespace nano_gicp diff --git a/include/nano_gicp/nano_gicp.h b/include/nano_gicp/nano_gicp.h new file mode 100644 index 00000000..1c9b977f --- /dev/null +++ b/include/nano_gicp/nano_gicp.h @@ -0,0 +1,152 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * BSD 3-Clause License + * + * Copyright (c) 2020, SMRT-AIST + * 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. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. + *************************************************************************/ + +#pragma once + +#include +#include + +#include +#include +#include + +#include "nano_gicp/lsq_registration.h" +#include "nano_gicp/nanoflann_adaptor.h" + +namespace nano_gicp { + +typedef std::vector> CovarianceList; + +enum class RegularizationMethod { NONE, MIN_EIG, NORMALIZED_MIN_EIG, PLANE, FROBENIUS }; + +template +class NanoGICP : public LsqRegistration { +public: + using Scalar = float; + using Matrix4 = typename pcl::Registration::Matrix4; + + using PointCloudSource = typename pcl::Registration::PointCloudSource; + using PointCloudSourcePtr = typename PointCloudSource::Ptr; + using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; + + using PointCloudTarget = typename pcl::Registration::PointCloudTarget; + using PointCloudTargetPtr = typename PointCloudTarget::Ptr; + using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; + +protected: + using pcl::Registration::reg_name_; + using pcl::Registration::input_; + using pcl::Registration::target_; + using pcl::Registration::final_transformation_; + +public: + NanoGICP(); + virtual ~NanoGICP() override; + + void setNumThreads(int n); + void setCorrespondenceRandomness(int k); + void setMaxCorrespondenceDistance(double corr); + void setRegularizationMethod(RegularizationMethod method); + + virtual void swapSourceAndTarget() override; + virtual void clearSource() override; + virtual void clearTarget() override; + + virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; + virtual void setSourceCovariances(const std::shared_ptr& covs); + virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; + virtual void setTargetCovariances(const std::shared_ptr& covs); + + virtual void registerInputSource(const PointCloudSourceConstPtr& cloud); + virtual void registerInputTarget(const PointCloudTargetConstPtr& cloud); + + virtual bool calculateSourceCovariances(); + virtual bool calculateTargetCovariances(); + + std::shared_ptr getSourceCovariances() const { + return source_covs_; + } + + std::shared_ptr getTargetCovariances() const { + return target_covs_; + } + + virtual void update_correspondences(const Eigen::Isometry3d& trans); + +protected: + virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; + + virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; + + virtual double compute_error(const Eigen::Isometry3d& trans) override; + + template + bool calculate_covariances(const typename pcl::PointCloud::ConstPtr& cloud, const nanoflann::KdTreeFLANN& kdtree, CovarianceList& covariances, float& density); + +public: + std::shared_ptr> source_kdtree_; + std::shared_ptr> target_kdtree_; + + std::shared_ptr source_covs_; + std::shared_ptr target_covs_; + + float source_density_; + float target_density_; + + int num_correspondences; + +protected: + int num_threads_; + int k_correspondences_; + double corr_dist_threshold_; + + RegularizationMethod regularization_method_; + + CovarianceList mahalanobis_; + + std::vector correspondences_; + std::vector sq_distances_; +}; +} // namespace nano_gicp + diff --git a/include/nano_gicp/nanoflann.h b/include/nano_gicp/nanoflann.h new file mode 100644 index 00000000..427a6569 --- /dev/null +++ b/include/nano_gicp/nanoflann.h @@ -0,0 +1,2411 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * 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-2022 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: + * - [Online README](https://github.com/jlblancoc/nanoflann) + * - [C++ API documentation](https://jlblancoc.github.io/nanoflann/) + */ + +#pragma once + +#include +#include +#include +#include // for abs() +#include // for abs() +#include +#include +#include // std::reference_wrapper +#include +#include +#include +#include + +/** Library version: 0xMmP (M=Major,m=minor,P=patch) */ +#define NANOFLANN_VERSION 0x142 + +// 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 < + typename _DistanceType, typename _IndexType = size_t, + typename _CountType = size_t> +class KNNResultSet +{ + public: + using DistanceType = _DistanceType; + using IndexType = _IndexType; + using 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: + using DistanceType = _DistanceType; + using 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."); + using DistIt = typename std::vector< + std::pair>::const_iterator; + 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(std::ostream& stream, const T& value) +{ + stream.write(reinterpret_cast(&value), sizeof(T)); +} + +template +void save_value(std::ostream& stream, const std::vector& value) +{ + size_t size = value.size(); + stream.write(reinterpret_cast(&size), sizeof(size_t)); + stream.write(reinterpret_cast(value.data()), sizeof(T) * size); +} + +template +void load_value(std::istream& stream, T& value) +{ + stream.read(reinterpret_cast(&value), sizeof(T)); +} + +template +void load_value(std::istream& stream, std::vector& value) +{ + size_t size; + stream.read(reinterpret_cast(&size), sizeof(size_t)); + value.resize(size); + stream.read(reinterpret_cast(value.data()), sizeof(T) * size); +} +/** @} */ + +/** @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 DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) + * \tparam AccessorType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) + */ +template < + class T, class DataSource, typename _DistanceType = T, + typename AccessorType = uint32_t> +struct L1_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + L1_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric( + const T* a, const AccessorType 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 DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) + * \tparam AccessorType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) + */ +template < + class T, class DataSource, typename _DistanceType = T, + typename AccessorType = uint32_t> +struct L2_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + L2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric( + const T* a, const AccessorType 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 DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) + * \tparam AccessorType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) + */ +template < + class T, class DataSource, typename _DistanceType = T, + typename AccessorType = uint32_t> +struct L2_Simple_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + L2_Simple_Adaptor(const DataSource& _data_source) + : data_source(_data_source) + { + } + + inline DistanceType evalMetric( + const T* a, const AccessorType 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, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) orientation is constrained to be in [-pi, pi] + * \tparam AccessorType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) + */ +template < + class T, class DataSource, typename _DistanceType = T, + typename AccessorType = uint32_t> +struct SO2_Adaptor +{ + using ElementType = T; + using DistanceType = _DistanceType; + + const DataSource& data_source; + + SO2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric( + const T* a, const AccessorType 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, uint8_t) + * \tparam DataSource Source of the data, i.e. where the vectors are stored + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) + * \tparam AccessorType Type of the arguments with which the data can be + * accessed (e.g. float, double, int64_t, T*) + */ +template < + class T, class DataSource, typename _DistanceType = T, + typename AccessorType = uint32_t> +struct SO3_Adaptor +{ + using ElementType = T; + using 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 AccessorType 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 + { + using distance_t = L1_Adaptor; + }; +}; +/** Metaprogramming helper traits class for the L2 (Euclidean) metric */ +struct metric_L2 : public Metric +{ + template + struct traits + { + using distance_t = L2_Adaptor; + }; +}; +/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ +struct metric_L2_Simple : public Metric +{ + template + struct traits + { + using distance_t = L2_Simple_Adaptor; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO2 : public Metric +{ + template + struct traits + { + using distance_t = SO2_Adaptor; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO3 : public Metric +{ + template + struct traits + { + using distance_t = SO3_Adaptor; + }; +}; + +/** @} */ + +/** @addtogroup param_grp Parameter structs + * @{ */ + +enum class KDTreeSingleIndexAdaptorFlags { + None = 0, + SkipInitialBuildIndex = 1 +}; + +inline std::underlying_type::type operator&(KDTreeSingleIndexAdaptorFlags lhs, KDTreeSingleIndexAdaptorFlags rhs) { + using underlying = typename std::underlying_type::type; + return static_cast(lhs) & static_cast(rhs); +} + +/** Parameters (see README.md) */ +struct KDTreeSingleIndexAdaptorParams +{ + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, KDTreeSingleIndexAdaptorFlags _flags = KDTreeSingleIndexAdaptorFlags::None) + : leaf_max_size(_leaf_max_size), flags(_flags) + { + } + + size_t leaf_max_size; + KDTreeSingleIndexAdaptorFlags flags; +}; + +/** 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. */ + + using Offset = uint32_t; + using Size = uint32_t; + using Dimension = int32_t; + + Size 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 = nullptr; + usedMemory = 0; + wastedMemory = 0; + } + + public: + Size usedMemory; + Size 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 != nullptr) + { + 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 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 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"); + throw std::bad_alloc(); + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + Size 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 +{ + using container_t = std::array; +}; +/** Dynamic size version */ +template +struct array_or_vector_selector<-1, T> +{ + using container_t = std::vector; +}; + +/** @} */ + +/** 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 AccessorType Will be typically size_t or int + */ + +template < + class Derived, typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename AccessorType = uint32_t> +class KDTreeBaseClass +{ + public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived& obj) + { + obj.pool.free_all(); + obj.root_node = nullptr; + obj.m_size_at_index_build = 0; + } + + using ElementType = typename Distance::ElementType; + using DistanceType = typename Distance::DistanceType; + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vAcc; + + using Offset = typename decltype(vAcc)::size_type; + using Size = typename decltype(vAcc)::size_type; + using Dimension = int32_t; + + /*--------------------- 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 + { + Offset left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf + { + Dimension divfeat; //!< Dimension used for subdivision. + DistanceType divlow, + divhigh; //!< The values used for subdivision. + } sub; + } node_type; + /** Child nodes (both=nullptr mean its a leaf node) */ + Node *child1, *child2; + }; + + using NodePtr = Node*; + + struct Interval + { + ElementType low, high; + }; + + NodePtr root_node; + + Size m_leaf_max_size; + + Size m_size; //!< Number of current points in the dataset + Size m_size_at_index_build; //!< Number of points in the dataset when the + //!< index was built + Dimension dim; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container + * depending on "DIM" */ + using BoundingBox = + typename array_or_vector_selector::container_t; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + using distance_vector_t = + typename array_or_vector_selector::container_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 size(const Derived& obj) const { return obj.m_size; } + + /** Returns the length of each point in the dataset */ + Size veclen(const Derived& obj) { return DIM > 0 ? DIM : obj.dim; } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get( + const Derived& obj, AccessorType element, Dimension component) const + { + return obj.dataset.kdtree_get_pt(element, component); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + Size usedMemory(Derived& obj) + { + return obj.pool.usedMemory + obj.pool.wastedMemory + + obj.dataset.kdtree_get_point_count() * + sizeof(AccessorType); // pool memory and vind array memory + } + + void computeMinMax( + const Derived& obj, Offset ind, Size count, Dimension element, + ElementType& min_elem, ElementType& max_elem) + { + min_elem = dataset_get(obj, vAcc[ind], element); + max_elem = min_elem; + for (Offset i = 1; i < count; ++i) + { + ElementType val = dataset_get(obj, vAcc[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 Offset left, const Offset 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 = nullptr; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) + { + bbox[i].low = dataset_get(obj, obj.vAcc[left], i); + bbox[i].high = dataset_get(obj, obj.vAcc[left], i); + } + for (Offset k = left + 1; k < right; ++k) + { + for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) + { + if (bbox[i].low > dataset_get(obj, obj.vAcc[k], i)) + bbox[i].low = dataset_get(obj, obj.vAcc[k], i); + if (bbox[i].high < dataset_get(obj, obj.vAcc[k], i)) + bbox[i].high = dataset_get(obj, obj.vAcc[k], i); + } + } + } + else + { + Offset idx; + Dimension cutfeat; + DistanceType cutval; + middleSplit_(obj, 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 (Dimension 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, Offset ind, Size count, Offset& index, Dimension& cutfeat, + DistanceType& cutval, const BoundingBox& bbox) + { + const auto EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for (Dimension 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 (Dimension 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; + + Offset 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, Offset ind, const Size count, Dimension cutfeat, + DistanceType& cutval, Offset& lim1, Offset& lim2) + { + /* Move vector indices for left subtree to front of list. */ + Offset left = 0; + Offset right = count - 1; + for (;;) + { + while (left <= right && + dataset_get(obj, vAcc[ind + left], cutfeat) < cutval) + ++left; + while (right && left <= right && + dataset_get(obj, vAcc[ind + right], cutfeat) >= cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(vAcc[ind + left], vAcc[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, vAcc[ind + left], cutfeat) <= cutval) + ++left; + while (right && left <= right && + dataset_get(obj, vAcc[ind + right], cutfeat) > cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(vAcc[ind + left], vAcc[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 (Dimension 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, std::ostream& stream, NodePtr tree) + { + save_value(stream, *tree); + if (tree->child1 != nullptr) { save_tree(obj, stream, tree->child1); } + if (tree->child2 != nullptr) { save_tree(obj, stream, tree->child2); } + } + + void load_tree(Derived& obj, std::istream& stream, NodePtr& tree) + { + tree = obj.pool.template allocate(); + load_value(stream, *tree); + if (tree->child1 != nullptr) { load_tree(obj, stream, tree->child1); } + if (tree->child2 != nullptr) { 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, std::ostream& 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.vAcc); + 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, std::istream& 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.vAcc); + 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 < + typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename AccessorType = uint32_t> +class KDTreeSingleIndexAdaptor + : public KDTreeBaseClass< + KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, AccessorType> +{ + public: + /** Deleted copy constructor*/ + KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor< + Distance, DatasetAdaptor, DIM, AccessorType>&) = + delete; + + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + Distance distance; + + using BaseClassRef = typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexAdaptor< + Distance, DatasetAdaptor, DIM, AccessorType>, + Distance, DatasetAdaptor, DIM, AccessorType>; + + using Offset = typename BaseClassRef::Offset; + using Size = typename BaseClassRef::Size; + using Dimension = typename BaseClassRef::Dimension; + + using ElementType = typename BaseClassRef::ElementType; + using DistanceType = typename BaseClassRef::DistanceType; + + using Node = typename BaseClassRef::Node; + using NodePtr = Node*; + + using Interval = typename BaseClassRef::Interval; + + /** Define "BoundingBox" as a fixed-size or variable-size container + * depending on "DIM" */ + using BoundingBox = typename BaseClassRef::BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + using distance_vector_t = typename BaseClassRef::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 + * + * Note that there is a variable number of optional additional parameters + * which will be forwarded to the metric class constructor. Refer to example + * `examples/pointcloud_custom_metric.cpp` for a use case. + * + */ + template + KDTreeSingleIndexAdaptor( + const Dimension dimensionality, const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params, Args&&... args) + : dataset(inputData), + index_params(params), + distance(inputData, std::forward(args)...) + { + init(dimensionality, params); + } + + KDTreeSingleIndexAdaptor( + const Dimension dimensionality, const DatasetAdaptor& inputData, + const KDTreeSingleIndexAdaptorParams& params = {}) + : dataset(inputData), + index_params(params), + distance(inputData) + { + init(dimensionality, params); + } + + private: + void init(const Dimension dimensionality, const KDTreeSingleIndexAdaptorParams& params) + { + BaseClassRef::root_node = nullptr; + 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; + + if (!(params.flags & KDTreeSingleIndexAdaptorFlags::SkipInitialBuildIndex)) { + buildIndex(); + } + } + + public: + /** + * 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 knnSearch( + const ElementType* query_point, const Size num_closest, + AccessorType* 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 radiusSearch( + const ElementType* query_point, const DistanceType& radius, + std::vector>& IndicesDists, + const SearchParams& searchParams) const + { + RadiusResultSet resultSet( + radius, IndicesDists); + const Size 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 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::vAcc.size() != BaseClassRef::m_size) + BaseClassRef::vAcc.resize(BaseClassRef::m_size); + for (Size i = 0; i < BaseClassRef::m_size; i++) + BaseClassRef::vAcc[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 N = dataset.kdtree_get_point_count(); + if (!N) + throw std::runtime_error( + "[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) + { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, BaseClassRef::vAcc[0], i); + } + for (Offset k = 1; k < N; ++k) + { + for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); + ++i) + { + if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) < + bbox[i].low) + bbox[i].low = + this->dataset_get(*this, BaseClassRef::vAcc[k], i); + if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) > + bbox[i].high) + bbox[i].high = + this->dataset_get(*this, BaseClassRef::vAcc[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 == nullptr) && (node->child2 == nullptr)) + { + // 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 (Offset i = node->node_type.lr.left; + i < node->node_type.lr.right; ++i) + { + const AccessorType accessor = + BaseClassRef::vAcc[i]; // reorder... : i; + DistanceType dist = distance.evalMetric( + vec, accessor, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) + { + if (!result_set.addPoint(dist, BaseClassRef::vAcc[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? */ + Dimension 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(std::ostream& 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(std::istream& 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 AccessorType + * Will be typically size_t or int + */ +template < + typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename AccessorType = uint32_t> +class KDTreeSingleIndexDynamicAdaptor_ + : public KDTreeBaseClass< + KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM, AccessorType>, + Distance, DatasetAdaptor, DIM, AccessorType> +{ + public: + /** + * The dataset used by this index + */ + const DatasetAdaptor& dataset; //!< The source of our data + + KDTreeSingleIndexAdaptorParams index_params; + + std::vector& treeIndex; + + Distance distance; + + using BaseClassRef = typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM, AccessorType>, + Distance, DatasetAdaptor, DIM, AccessorType>; + + using ElementType = typename BaseClassRef::ElementType; + using DistanceType = typename BaseClassRef::DistanceType; + + using Offset = typename BaseClassRef::Offset; + using Size = typename BaseClassRef::Size; + using Dimension = typename BaseClassRef::Dimension; + + using Node = typename BaseClassRef::Node; + using NodePtr = Node*; + + using Interval = typename BaseClassRef::Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container + * depending on "DIM" */ + using BoundingBox = typename BaseClassRef::BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + using distance_vector_t = typename BaseClassRef::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 Dimension dimensionality, const DatasetAdaptor& inputData, + std::vector& treeIndex_, + const KDTreeSingleIndexAdaptorParams& params = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), + index_params(params), + treeIndex(treeIndex_), + distance(inputData) + { + BaseClassRef::root_node = nullptr; + 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; + } + + /** Explicitly default the copy constructor */ + KDTreeSingleIndexDynamicAdaptor_( + const KDTreeSingleIndexDynamicAdaptor_& rhs) = default; + + /** Assignment operator definiton */ + KDTreeSingleIndexDynamicAdaptor_ operator=( + const KDTreeSingleIndexDynamicAdaptor_& rhs) + { + KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); + std::swap(BaseClassRef::vAcc, tmp.BaseClassRef::vAcc); + 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::vAcc.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 knnSearch( + const ElementType* query_point, const Size num_closest, + AccessorType* 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 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 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 N = BaseClassRef::m_size; + if (!N) + throw std::runtime_error( + "[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) + { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, BaseClassRef::vAcc[0], i); + } + for (Offset k = 1; k < N; ++k) + { + for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); + ++i) + { + if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) < + bbox[i].low) + bbox[i].low = + this->dataset_get(*this, BaseClassRef::vAcc[k], i); + if (this->dataset_get(*this, BaseClassRef::vAcc[k], i) > + bbox[i].high) + bbox[i].high = + this->dataset_get(*this, BaseClassRef::vAcc[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 == nullptr) && (node->child2 == nullptr)) + { + // 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 (Offset i = node->node_type.lr.left; + i < node->node_type.lr.right; ++i) + { + const AccessorType index = + BaseClassRef::vAcc[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::vAcc[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? */ + Dimension 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(std::ostream& 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(std::istream& 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 AccessorType + * Will be typically size_t or int + */ +template < + typename Distance, class DatasetAdaptor, int32_t DIM = -1, + typename AccessorType = uint32_t> +class KDTreeSingleIndexDynamicAdaptor +{ + public: + using ElementType = typename Distance::ElementType; + using DistanceType = typename Distance::DistanceType; + + using Offset = typename KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM>::Offset; + using Size = typename KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM>::Size; + using Dimension = typename KDTreeSingleIndexDynamicAdaptor_< + Distance, DatasetAdaptor, DIM>::Dimension; + + protected: + Size m_leaf_max_size; + Size treeCount; + Size 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. + std::unordered_set removedPoints; + + KDTreeSingleIndexAdaptorParams index_params; + + Dimension dim; //!< Dimensionality of each data point + + using index_container_t = + KDTreeSingleIndexDynamicAdaptor_; + 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(AccessorType num) + { + int pos = 0; + while (num & 1) + { + num = num >> 1; + pos++; + } + return pos; + } + + /** Creates multiple empty trees to handle dynamic support */ + void init() + { + using my_kd_tree_t = + KDTreeSingleIndexDynamicAdaptor_; + 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& params = + KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) + : dataset(inputData), index_params(params), distance(inputData) + { + treeCount = static_cast(std::log2(maximumPointCount)) + 1; + 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< + Distance, DatasetAdaptor, DIM, AccessorType>&) = delete; + + /** Add points to the set, Inserts all points from [start, end] */ + void addPoints(AccessorType start, AccessorType end) + { + const Size count = end - start + 1; + int maxIndex = 0; + treeIndex.resize(treeIndex.size() + count); + for (AccessorType idx = start; idx <= end; idx++) + { + const int pos = First0Bit(pointCount); + maxIndex = std::max(pos, maxIndex); + treeIndex[pointCount] = pos; + + const auto it = removedPoints.find(idx); + if(it != removedPoints.end()) + { + removedPoints.erase(it); + treeIndex[idx] = pos; + } + + for (int i = 0; i < pos; i++) + { + for (int j = 0; j < static_cast(index[i].vAcc.size()); j++) + { + index[pos].vAcc.push_back(index[i].vAcc[j]); + if (treeIndex[index[i].vAcc[j]] != -1) + treeIndex[index[i].vAcc[j]] = pos; + } + index[i].vAcc.clear(); + } + index[pos].vAcc.push_back(idx); + pointCount++; + } + + for(int i = 0; i <= maxIndex; ++i) { + index[i].freeIndex(index[i]); + if(!index[i].vAcc.empty()) + index[i].buildIndex(); + } + } + + /** Remove a point from the set (Lazy Deletion) */ + void removePoint(size_t idx) + { + if (idx >= pointCount) return; + removedPoints.insert(idx); + 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. You can select whether a + * row or column in the matrix represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * + * // Fill out "mat"... + * using my_kd_tree_t = nanoflann::KDTreeEigenMatrixAdaptor< + * Eigen::Matrix>; + * + * const int max_leaf = 10; + * my_kd_tree_t mat_index(mat, max_leaf); + * 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. \tparam row_major + * If set to true the rows of the matrix are used as the points, if set to false + * the columns of the matrix are used as the points. + */ +template < + class MatrixType, int32_t DIM = -1, class Distance = nanoflann::metric_L2, + bool row_major = true> +struct KDTreeEigenMatrixAdaptor +{ + using self_t = + KDTreeEigenMatrixAdaptor; + using num_t = typename MatrixType::Scalar; + using IndexType = typename MatrixType::Index; + using metric_t = typename Distance::template traits< + num_t, self_t, IndexType>::distance_t; + + using index_t = KDTreeSingleIndexAdaptor< + metric_t, self_t, + row_major ? MatrixType::ColsAtCompileTime + : MatrixType::RowsAtCompileTime, + IndexType>; + + index_t* index; //! The kd-tree index for the user to call its methods as + //! usual with any other FLANN index. + + using Offset = typename index_t::Offset; + using Size = typename index_t::Size; + using Dimension = typename index_t::Dimension; + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor( + const Dimension dimensionality, + const std::reference_wrapper& mat, + const int leaf_max_size = 10) + : m_data_matrix(mat) + { + const auto dims = row_major ? mat.get().cols() : mat.get().rows(); + if (static_cast(dims) != dimensionality) + throw std::runtime_error( + "Error: 'dimensionality' must match column count in data " + "matrix"); + 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( + dims, *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); + } + + 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 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 kdtree_get_point_count() const + { + if (row_major) + return m_data_matrix.get().rows(); + else + return m_data_matrix.get().cols(); + } + + // 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 + { + if (row_major) + return m_data_matrix.get().coeff(idx, IndexType(dim)); + else + return m_data_matrix.get().coeff(IndexType(dim), idx); + } + + // 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 diff --git a/include/nano_gicp/nanoflann_adaptor.h b/include/nano_gicp/nanoflann_adaptor.h new file mode 100644 index 00000000..0e2c2d78 --- /dev/null +++ b/include/nano_gicp/nanoflann_adaptor.h @@ -0,0 +1,193 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * 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-2021 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. + *************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include "nano_gicp/nanoflann.h" + +namespace nanoflann +{ + +template +class KdTreeFLANN +{ +public: + + typedef typename pcl::PointCloud PointCloud; + typedef typename pcl::PointCloud::Ptr PointCloudPtr; + typedef typename pcl::PointCloud::ConstPtr PointCloudConstPtr; + + typedef boost::shared_ptr> Ptr; + typedef boost::shared_ptr> ConstPtr; + typedef boost::shared_ptr> IndicesPtr; + typedef boost::shared_ptr> IndicesConstPtr; + + KdTreeFLANN (bool sorted = false); + + void setEpsilon (float eps); + + void setSortedResults (bool sorted); + + void setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); + + inline PointCloudConstPtr getInputCloud() const { return _adaptor.pcl; } + + int nearestKSearch (const PointT &point, int k, std::vector &k_indices, + std::vector &k_sqr_distances) const; + + int radiusSearch (const PointT &point, double radius, std::vector &k_indices, + std::vector &k_sqr_distances) const; + +protected: + + nanoflann::SearchParams _params; + + struct PointCloud_Adaptor + { + inline size_t kdtree_get_point_count() const; + inline float kdtree_get_pt(const size_t idx, int dim) const; + template bool kdtree_get_bbox(BBOX&) const { return false; } + PointCloudConstPtr pcl; + IndicesConstPtr indices; + }; + + typedef nanoflann::KDTreeSingleIndexAdaptor< + nanoflann::SO3_Adaptor , + PointCloud_Adaptor, 3, int> KDTreeFlann_PCL_SO3; + + PointCloud_Adaptor _adaptor; + + KDTreeFlann_PCL_SO3 _kdtree; + +}; + +//---------- Definitions --------------------- + +template inline +KdTreeFLANN::KdTreeFLANN(bool sorted): + _kdtree(3,_adaptor, KDTreeSingleIndexAdaptorParams(25)) +{ + _params.sorted = sorted; +} + +template inline +void KdTreeFLANN::setEpsilon(float eps) +{ + _params.eps = eps; +} + +template inline +void KdTreeFLANN::setSortedResults(bool sorted) +{ + _params.sorted = sorted; +} + +template inline +void KdTreeFLANN::setInputCloud(const KdTreeFLANN::PointCloudConstPtr &cloud, + const IndicesConstPtr &indices) +{ + _adaptor.pcl = cloud; + _adaptor.indices = indices; + _kdtree.buildIndex(); +} + +template inline +int KdTreeFLANN::nearestKSearch(const PointT &point, int num_closest, + std::vector &k_indices, + std::vector &k_sqr_distances) const +{ + k_indices.resize(num_closest); + k_sqr_distances.resize(num_closest); + + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init( k_indices.data(), k_sqr_distances.data()); + _kdtree.findNeighbors(resultSet, point.data, _params ); + return resultSet.size(); +} + +template inline +int KdTreeFLANN::radiusSearch(const PointT &point, double radius, + std::vector &k_indices, + std::vector &k_sqr_distances) const +{ + static std::vector > indices_dist; + indices_dist.reserve( 128 ); + + RadiusResultSet resultSet(radius, indices_dist); + const size_t nFound = _kdtree.findNeighbors(resultSet, point.data, _params); + + if (_params.sorted) + std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter() ); + + k_indices.resize(nFound); + k_sqr_distances.resize(nFound); + for(int i=0; i inline +size_t KdTreeFLANN::PointCloud_Adaptor::kdtree_get_point_count() const { + if( indices ) return indices->size(); + if( pcl) return pcl->points.size(); + return 0; +} + +template inline +float KdTreeFLANN::PointCloud_Adaptor::kdtree_get_pt(const size_t idx, int dim) const{ + const PointT& p = ( indices ) ? pcl->points[(*indices)[idx]] : pcl->points[idx]; + if (dim==0) return p.x; + else if (dim==1) return p.y; + else if (dim==2) return p.z; + else return 0.0; +} + +} // namespace nanoflann diff --git a/launch/dlio.launch b/launch/dlio.launch new file mode 100644 index 00000000..f367dd29 --- /dev/null +++ b/launch/dlio.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/dlio.rviz b/launch/dlio.rviz new file mode 100644 index 00000000..1044de58 --- /dev/null +++ b/launch/dlio.rviz @@ -0,0 +1,315 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Frames1 + - /Point Clouds1 + - /Odometry1 + Splitter Ratio: 0.6470588445663452 + Tree Height: 865 + - 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 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5157480239868164 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Deskewed Scan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Class: rviz/Group + Displays: + - Class: rviz/TF + Enabled: false + Frame Timeout: 9999 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 5 + Name: tf tree + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: odom + Radius: 0.25 + Reference Frame: robot/odom + Show Trail: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: base_link + Radius: 0.25 + Reference Frame: robot/base_link + Show Trail: false + Value: true + Enabled: true + Name: Frames + - Class: rviz/Group + Displays: + - 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: 0; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 512 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Deskewed Scan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /robot/dlio/odom_node/pointcloud/deskewed + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: Intensity + Decay Time: 9999 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 512 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Dense Map + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: /robot/dlio/odom_node/pointcloud/deskewed + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: 0 + Value: false + 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: 512 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Sparse Map + Position Transformer: XYZ + Queue Size: 1000 + Selectable: true + Size (Pixels): 1 + Size (m): 0.05000000074505806 + Style: Points + Topic: /robot/dlio/map_node/map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: Point Clouds + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: false + Keep: 1 + Name: Pose + Position Tolerance: 0.009999999776482582 + Queue Size: 1000 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.25 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /robot/dlio/odom_node/odom + Unreliable: false + Value: false + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.5 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Keyframes + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: /robot/dlio/odom_node/keyframes + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 209; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Trajectory + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /robot/dlio/odom_node/path + Unreliable: false + Value: true + Enabled: true + Name: Odometry + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: robot/odom + 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/Orbit + Distance: 30 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 1 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1 + Target Frame: robot/base_link + Yaw: 3.141590118408203 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1016 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..b1a8fa98 --- /dev/null +++ b/package.xml @@ -0,0 +1,31 @@ + + + + + direct_lidar_inertial_odometry + 1.0.0 + Direct LiDAR-Inertial Odometry: Lightweight LIO with Continuous-Time Motion Correction + + Kenny J. Chen + Ryan Nemiroff + Brett T. Lopez + + Kenny J. Chen + Ryan Nemiroff + Brett T. Lopez + + MIT + + catkin + + roscpp + std_msgs + sensor_msgs + geometry_msgs + nav_msgs + pcl_ros + + + + + diff --git a/src/dlio/map.cc b/src/dlio/map.cc new file mode 100644 index 00000000..89ecae06 --- /dev/null +++ b/src/dlio/map.cc @@ -0,0 +1,111 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +#include "dlio/map.h" + +dlio::MapNode::MapNode(ros::NodeHandle node_handle) : nh(node_handle) { + + this->getParams(); + + this->publish_timer = this->nh.createTimer(ros::Duration(this->publish_freq_), &dlio::MapNode::publishTimer, this); + + this->keyframe_sub = this->nh.subscribe("keyframes", 10, + &dlio::MapNode::callbackKeyframe, this, ros::TransportHints().tcpNoDelay()); + this->map_pub = this->nh.advertise("map", 100); + this->save_pcd_srv = this->nh.advertiseService("save_pcd", &dlio::MapNode::savePcd, this); + + this->dlio_map = pcl::PointCloud::Ptr (boost::make_shared>()); + + pcl::console::setVerbosityLevel(pcl::console::L_ERROR); + +} + +dlio::MapNode::~MapNode() {} + +void dlio::MapNode::getParams() { + + ros::param::param("~dlio/odom/odom_frame", this->odom_frame, "odom"); + ros::param::param("~dlio/map/sparse/frequency", this->publish_freq_, 1.0); + ros::param::param("~dlio/map/sparse/leafSize", this->leaf_size_, 0.5); + + // Get Node NS and Remove Leading Character + std::string ns = ros::this_node::getNamespace(); + ns.erase(0,1); + + // Concatenate Frame Name Strings + this->odom_frame = ns + "/" + this->odom_frame; + +} + +void dlio::MapNode::start() { +} + +void dlio::MapNode::publishTimer(const ros::TimerEvent& e) { + + if (this->dlio_map->points.size() == this->dlio_map->width * this->dlio_map->height) { + sensor_msgs::PointCloud2 map_ros; + pcl::toROSMsg(*this->dlio_map, map_ros); + map_ros.header.stamp = ros::Time::now(); + map_ros.header.frame_id = this->odom_frame; + this->map_pub.publish(map_ros); + } + +} + +void dlio::MapNode::callbackKeyframe(const sensor_msgs::PointCloud2ConstPtr& keyframe) { + + // convert scan to pcl format + pcl::PointCloud::Ptr keyframe_pcl = + pcl::PointCloud::Ptr (boost::make_shared>()); + pcl::fromROSMsg(*keyframe, *keyframe_pcl); + + // voxel filter + this->voxelgrid.setLeafSize(this->leaf_size_, this->leaf_size_, this->leaf_size_); + this->voxelgrid.setInputCloud(keyframe_pcl); + this->voxelgrid.filter(*keyframe_pcl); + + // save filtered keyframe to map for rviz + *this->dlio_map += *keyframe_pcl; + +} + +bool dlio::MapNode::savePcd(direct_lidar_inertial_odometry::save_pcd::Request& req, + direct_lidar_inertial_odometry::save_pcd::Response& res) { + + pcl::PointCloud::Ptr m = + pcl::PointCloud::Ptr (boost::make_shared>(*this->dlio_map)); + + float leaf_size = req.leaf_size; + std::string p = req.save_path; + + std::cout << std::setprecision(2) << "Saving map to " << p + "/dlio_map.pcd" + << " with leaf size " << to_string_with_precision(leaf_size, 2) << "... "; std::cout.flush(); + + // voxelize map + pcl::VoxelGrid vg; + vg.setLeafSize(leaf_size, leaf_size, leaf_size); + vg.setInputCloud(m); + vg.filter(*m); + + // save map + int ret = pcl::io::savePCDFileBinary(p + "/dlio_map.pcd", *m); + res.success = ret == 0; + + if (res.success) { + std::cout << "done" << std::endl; + } else { + std::cout << "failed" << std::endl; + } + + return res.success; + +} diff --git a/src/dlio/map_node.cc b/src/dlio/map_node.cc new file mode 100644 index 00000000..6f070bc1 --- /dev/null +++ b/src/dlio/map_node.cc @@ -0,0 +1,28 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +#include "dlio/map.h" + +int main(int argc, char** argv) { + + ros::init(argc, argv, "dlio_map_node"); + ros::NodeHandle nh("~"); + + dlio::MapNode node(nh); + ros::AsyncSpinner spinner(0); + spinner.start(); + node.start(); + ros::waitForShutdown(); + + return 0; + +} diff --git a/src/dlio/odom.cc b/src/dlio/odom.cc new file mode 100644 index 00000000..340b1da0 --- /dev/null +++ b/src/dlio/odom.cc @@ -0,0 +1,1973 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +#include "dlio/odom.h" + +dlio::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { + + this->getParams(); + + this->num_threads_ = omp_get_max_threads(); + + this->dlio_initialized = false; + this->first_valid_scan = false; + this->first_imu_received = false; + if (this->imu_calibrate_) {this->imu_calibrated = false;} + else {this->imu_calibrated = true;} + this->deskew_status = false; + this->deskew_size = 0; + + this->lidar_sub = this->nh.subscribe("pointcloud", 1, + &dlio::OdomNode::callbackPointCloud, this, ros::TransportHints().tcpNoDelay()); + this->imu_sub = this->nh.subscribe("imu", 1000, + &dlio::OdomNode::callbackImu, this, ros::TransportHints().tcpNoDelay()); + + this->odom_pub = this->nh.advertise("odom", 1, true); + this->pose_pub = this->nh.advertise("pose", 1, true); + this->path_pub = this->nh.advertise("path", 1, true); + this->kf_pose_pub = this->nh.advertise("kf_pose", 1, true); + this->kf_cloud_pub = this->nh.advertise("kf_cloud", 1, true); + this->deskewed_pub = this->nh.advertise("deskewed", 1, true); + + this->publish_timer = this->nh.createTimer(ros::Duration(0.01), &dlio::OdomNode::publishPose, this); + + this->T = Eigen::Matrix4f::Identity(); + this->T_prior = Eigen::Matrix4f::Identity(); + this->T_corr = Eigen::Matrix4f::Identity(); + + this->origin = Eigen::Vector3f(0., 0., 0.); + this->state.p = Eigen::Vector3f(0., 0., 0.); + this->state.q = Eigen::Quaternionf(1., 0., 0., 0.); + this->state.v.lin.b = Eigen::Vector3f(0., 0., 0.); + this->state.v.lin.w = Eigen::Vector3f(0., 0., 0.); + this->state.v.ang.b = Eigen::Vector3f(0., 0., 0.); + this->state.v.ang.w = Eigen::Vector3f(0., 0., 0.); + + this->lidarPose.p = Eigen::Vector3f(0., 0., 0.); + this->lidarPose.q = Eigen::Quaternionf(1., 0., 0., 0.); + + this->imu_meas.stamp = 0.; + this->imu_meas.ang_vel[0] = 0.; + this->imu_meas.ang_vel[1] = 0.; + this->imu_meas.ang_vel[2] = 0.; + this->imu_meas.lin_accel[0] = 0.; + this->imu_meas.lin_accel[1] = 0.; + this->imu_meas.lin_accel[2] = 0.; + + this->imu_buffer.set_capacity(this->imu_buffer_size_); + this->first_imu_stamp = 0.; + this->prev_imu_stamp = 0.; + + this->original_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); + this->deskewed_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); + this->current_scan = pcl::PointCloud::ConstPtr (boost::make_shared>()); + this->submap_cloud = pcl::PointCloud::ConstPtr (boost::make_shared>()); + + this->num_processed_keyframes = 0; + + this->submap_hasChanged = true; + this->submap_kf_idx_prev.clear(); + + this->first_scan_stamp = 0.; + this->elapsed_time = 0.; + this->length_traversed; + + this->convex_hull.setDimension(3); + this->concave_hull.setDimension(3); + this->concave_hull.setAlpha(this->keyframe_thresh_dist_); + this->concave_hull.setKeepInformation(true); + + this->gicp.setCorrespondenceRandomness(this->gicp_k_correspondences_); + this->gicp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_); + this->gicp.setMaximumIterations(this->gicp_max_iter_); + this->gicp.setTransformationEpsilon(this->gicp_transformation_ep_); + this->gicp.setRotationEpsilon(this->gicp_rotation_ep_); + this->gicp.setInitialLambdaFactor(this->gicp_init_lambda_factor_); + + this->gicp_temp.setCorrespondenceRandomness(this->gicp_k_correspondences_); + this->gicp_temp.setMaxCorrespondenceDistance(this->gicp_max_corr_dist_); + this->gicp_temp.setMaximumIterations(this->gicp_max_iter_); + this->gicp_temp.setTransformationEpsilon(this->gicp_transformation_ep_); + this->gicp_temp.setRotationEpsilon(this->gicp_rotation_ep_); + this->gicp_temp.setInitialLambdaFactor(this->gicp_init_lambda_factor_); + + pcl::Registration::KdTreeReciprocalPtr temp; + this->gicp.setSearchMethodSource(temp, true); + this->gicp.setSearchMethodTarget(temp, true); + this->gicp_temp.setSearchMethodSource(temp, true); + this->gicp_temp.setSearchMethodTarget(temp, true); + + this->geo.first_opt_done = false; + this->geo.prev_vel = Eigen::Vector3f(0., 0., 0.); + + pcl::console::setVerbosityLevel(pcl::console::L_ERROR); + + this->crop.setNegative(true); + this->crop.setMin(Eigen::Vector4f(-this->crop_size_, -this->crop_size_, -this->crop_size_, 1.0)); + this->crop.setMax(Eigen::Vector4f(this->crop_size_, this->crop_size_, this->crop_size_, 1.0)); + + this->voxel.setLeafSize(this->vf_res_, this->vf_res_, this->vf_res_); + + this->metrics.spaciousness.push_back(0.); + this->metrics.density.push_back(this->gicp_max_corr_dist_); + + // CPU Specs + char CPUBrandString[0x40]; + memset(CPUBrandString, 0, sizeof(CPUBrandString)); + + this->cpu_type = ""; + + #ifdef HAS_CPUID + unsigned int CPUInfo[4] = {0,0,0,0}; + __cpuid(0x80000000, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); + unsigned int nExIds = CPUInfo[0]; + for (unsigned int i = 0x80000000; i <= nExIds; ++i) { + __cpuid(i, CPUInfo[0], CPUInfo[1], CPUInfo[2], CPUInfo[3]); + if (i == 0x80000002) + memcpy(CPUBrandString, CPUInfo, sizeof(CPUInfo)); + else if (i == 0x80000003) + memcpy(CPUBrandString + 16, CPUInfo, sizeof(CPUInfo)); + else if (i == 0x80000004) + memcpy(CPUBrandString + 32, CPUInfo, sizeof(CPUInfo)); + } + this->cpu_type = CPUBrandString; + boost::trim(this->cpu_type); + #endif + + FILE* file; + struct tms timeSample; + char line[128]; + + this->lastCPU = times(&timeSample); + this->lastSysCPU = timeSample.tms_stime; + this->lastUserCPU = timeSample.tms_utime; + + file = fopen("/proc/cpuinfo", "r"); + this->numProcessors = 0; + while(fgets(line, 128, file) != nullptr) { + if (strncmp(line, "processor", 9) == 0) this->numProcessors++; + } + fclose(file); + +} + +dlio::OdomNode::~OdomNode() {} + +void dlio::OdomNode::getParams() { + + // Version + ros::param::param("~dlio/version", this->version_, "0.0.0"); + + // Frames + ros::param::param("~dlio/frames/odom", this->odom_frame, "odom"); + ros::param::param("~dlio/frames/baselink", this->baselink_frame, "base_link"); + ros::param::param("~dlio/frames/lidar", this->lidar_frame, "lidar"); + ros::param::param("~dlio/frames/imu", this->imu_frame, "imu"); + + // Get Node NS and Remove Leading Character + std::string ns = ros::this_node::getNamespace(); + ns.erase(0,1); + + // Concatenate Frame Name Strings + this->odom_frame = ns + "/" + this->odom_frame; + this->baselink_frame = ns + "/" + this->baselink_frame; + this->lidar_frame = ns + "/" + this->lidar_frame; + this->imu_frame = ns + "/" + this->imu_frame; + + // Deskew FLag + ros::param::param("~dlio/pointcloud/deskew", this->deskew_, true); + + // Gravity + ros::param::param("~dlio/odom/gravity", this->gravity_, 9.80665); + + // Keyframe Threshold + ros::param::param("~dlio/odom/keyframe/threshD", this->keyframe_thresh_dist_, 0.1); + ros::param::param("~dlio/odom/keyframe/threshR", this->keyframe_thresh_rot_, 1.0); + + // Submap + ros::param::param("~dlio/odom/submap/keyframe/knn", this->submap_knn_, 10); + ros::param::param("~dlio/odom/submap/keyframe/kcv", this->submap_kcv_, 10); + ros::param::param("~dlio/odom/submap/keyframe/kcc", this->submap_kcc_, 10); + + // Dense map resolution + ros::param::param("~dlio/map/dense/filtered", this->densemap_filtered_, true); + + // Wait until movement to publish map + ros::param::param("~dlio/map/waitUntilMove", this->wait_until_move_, false); + + // Crop Box Filter + ros::param::param("~dlio/odom/preprocessing/cropBoxFilter/size", this->crop_size_, 1.0); + + // Voxel Grid Filter + ros::param::param("~dlio/pointcloud/voxelize", this->vf_use_, true); + ros::param::param("~dlio/odom/preprocessing/voxelFilter/res", this->vf_res_, 0.05); + + // Adaptive Parameters + ros::param::param("~dlio/adaptive", this->adaptive_params_, true); + + // Extrinsics + std::vector t_default{0., 0., 0.}; + std::vector R_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; + + // center of gravity to imu + std::vector baselink2imu_t, baselink2imu_R; + ros::param::param>("~dlio/extrinsics/baselink2imu/t", baselink2imu_t, t_default); + ros::param::param>("~dlio/extrinsics/baselink2imu/R", baselink2imu_R, R_default); + this->extrinsics.baselink2imu.t = + Eigen::Vector3f(baselink2imu_t[0], baselink2imu_t[1], baselink2imu_t[2]); + this->extrinsics.baselink2imu.R = + Eigen::Map>(baselink2imu_R.data(), 3, 3); + + this->extrinsics.baselink2imu_T = Eigen::Matrix4f::Identity(); + this->extrinsics.baselink2imu_T.block(0, 3, 3, 1) = this->extrinsics.baselink2imu.t; + this->extrinsics.baselink2imu_T.block(0, 0, 3, 3) = this->extrinsics.baselink2imu.R; + + // center of gravity to lidar + std::vector baselink2lidar_t, baselink2lidar_R; + ros::param::param>("~dlio/extrinsics/baselink2lidar/t", baselink2lidar_t, t_default); + ros::param::param>("~dlio/extrinsics/baselink2lidar/R", baselink2lidar_R, R_default); + + this->extrinsics.baselink2lidar.t = + Eigen::Vector3f(baselink2lidar_t[0], baselink2lidar_t[1], baselink2lidar_t[2]); + this->extrinsics.baselink2lidar.R = + Eigen::Map>(baselink2lidar_R.data(), 3, 3); + + this->extrinsics.baselink2lidar_T = Eigen::Matrix4f::Identity(); + this->extrinsics.baselink2lidar_T.block(0, 3, 3, 1) = this->extrinsics.baselink2lidar.t; + this->extrinsics.baselink2lidar_T.block(0, 0, 3, 3) = this->extrinsics.baselink2lidar.R; + + // IMU + ros::param::param("~dlio/odom/imu/calibration/accel", this->calibrate_accel_, true); + ros::param::param("~dlio/odom/imu/calibration/gyro", this->calibrate_gyro_, true); + ros::param::param("~dlio/odom/imu/calibration/time", this->imu_calib_time_, 3.0); + ros::param::param("~dlio/odom/imu/bufferSize", this->imu_buffer_size_, 2000); + + std::vector accel_default{0., 0., 0.}; std::vector prior_accel_bias; + std::vector gyro_default{0., 0., 0.}; std::vector prior_gyro_bias; + + ros::param::param("~dlio/odom/imu/approximateGravity", this->gravity_align_, true); + ros::param::param("~dlio/imu/calibration", this->imu_calibrate_, true); + ros::param::param>("~dlio/imu/intrinsics/accel/bias", prior_accel_bias, accel_default); + ros::param::param>("~dlio/imu/intrinsics/gyro/bias", prior_gyro_bias, gyro_default); + + // scale-misalignment matrix + std::vector imu_sm_default{1., 0., 0., 0., 1., 0., 0., 0., 1.}; + std::vector imu_sm; + + ros::param::param>("~dlio/imu/intrinsics/accel/sm", imu_sm, imu_sm_default); + + if (!this->imu_calibrate_) { + this->state.b.accel[0] = prior_accel_bias[0]; + this->state.b.accel[1] = prior_accel_bias[1]; + this->state.b.accel[2] = prior_accel_bias[2]; + this->state.b.gyro[0] = prior_gyro_bias[0]; + this->state.b.gyro[1] = prior_gyro_bias[1]; + this->state.b.gyro[2] = prior_gyro_bias[2]; + this->imu_accel_sm_ = Eigen::Map>(imu_sm.data(), 3, 3); + } else { + this->state.b.accel = Eigen::Vector3f(0., 0., 0.); + this->state.b.gyro = Eigen::Vector3f(0., 0., 0.); + this->imu_accel_sm_ = Eigen::Matrix3f::Identity(); + } + + // GICP + ros::param::param("~dlio/odom/gicp/minNumPoints", this->gicp_min_num_points_, 100); + ros::param::param("~dlio/odom/gicp/kCorrespondences", this->gicp_k_correspondences_, 20); + ros::param::param("~dlio/odom/gicp/maxCorrespondenceDistance", this->gicp_max_corr_dist_, + std::sqrt(std::numeric_limits::max())); + ros::param::param("~dlio/odom/gicp/maxIterations", this->gicp_max_iter_, 64); + ros::param::param("~dlio/odom/gicp/transformationEpsilon", this->gicp_transformation_ep_, 0.0005); + ros::param::param("~dlio/odom/gicp/rotationEpsilon", this->gicp_rotation_ep_, 0.0005); + ros::param::param("~dlio/odom/gicp/initLambdaFactor", this->gicp_init_lambda_factor_, 1e-9); + + // Geometric Observer + ros::param::param("~dlio/odom/geo/Kp", this->geo_Kp_, 1.0); + ros::param::param("~dlio/odom/geo/Kv", this->geo_Kv_, 1.0); + ros::param::param("~dlio/odom/geo/Kq", this->geo_Kq_, 1.0); + ros::param::param("~dlio/odom/geo/Kab", this->geo_Kab_, 1.0); + ros::param::param("~dlio/odom/geo/Kgb", this->geo_Kgb_, 1.0); + ros::param::param("~dlio/odom/geo/abias_max", this->geo_abias_max_, 1.0); + ros::param::param("~dlio/odom/geo/gbias_max", this->geo_gbias_max_, 1.0); + + +} + +void dlio::OdomNode::start() { + + printf("\033[2J\033[1;1H"); + std::cout << std::endl + << "+-------------------------------------------------------------------+" << std::endl; + std::cout << "| Direct LiDAR-Inertial Odometry v" << this->version_ << " |" + << std::endl; + std::cout << "+-------------------------------------------------------------------+" << std::endl; + +} + +void dlio::OdomNode::publishPose(const ros::TimerEvent& e) { + + // nav_msgs::Odometry + this->odom_ros.header.stamp = this->imu_stamp; + this->odom_ros.header.frame_id = this->odom_frame; + this->odom_ros.child_frame_id = this->baselink_frame; + + this->odom_ros.pose.pose.position.x = this->state.p[0]; + this->odom_ros.pose.pose.position.y = this->state.p[1]; + this->odom_ros.pose.pose.position.z = this->state.p[2]; + + this->odom_ros.pose.pose.orientation.w = this->state.q.w(); + this->odom_ros.pose.pose.orientation.x = this->state.q.x(); + this->odom_ros.pose.pose.orientation.y = this->state.q.y(); + this->odom_ros.pose.pose.orientation.z = this->state.q.z(); + + this->odom_ros.twist.twist.linear.x = this->state.v.lin.w[0]; + this->odom_ros.twist.twist.linear.y = this->state.v.lin.w[1]; + this->odom_ros.twist.twist.linear.z = this->state.v.lin.w[2]; + + this->odom_ros.twist.twist.angular.x = this->state.v.ang.b[0]; + this->odom_ros.twist.twist.angular.y = this->state.v.ang.b[1]; + this->odom_ros.twist.twist.angular.z = this->state.v.ang.b[2]; + + this->odom_pub.publish(this->odom_ros); + + // geometry_msgs::PoseStamped + this->pose_ros.header.stamp = this->imu_stamp; + this->pose_ros.header.frame_id = this->odom_frame; + + this->pose_ros.pose.position.x = this->state.p[0]; + this->pose_ros.pose.position.y = this->state.p[1]; + this->pose_ros.pose.position.z = this->state.p[2]; + + this->pose_ros.pose.orientation.w = this->state.q.w(); + this->pose_ros.pose.orientation.x = this->state.q.x(); + this->pose_ros.pose.orientation.y = this->state.q.y(); + this->pose_ros.pose.orientation.z = this->state.q.z(); + + this->pose_pub.publish(this->pose_ros); + +} + +void dlio::OdomNode::publishToROS(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud) { + this->publishCloud(published_cloud, T_cloud); + + // nav_msgs::Path + this->path_ros.header.stamp = this->imu_stamp; + this->path_ros.header.frame_id = this->odom_frame; + + geometry_msgs::PoseStamped p; + p.header.stamp = this->imu_stamp; + p.header.frame_id = this->odom_frame; + p.pose.position.x = this->state.p[0]; + p.pose.position.y = this->state.p[1]; + p.pose.position.z = this->state.p[2]; + p.pose.orientation.w = this->state.q.w(); + p.pose.orientation.x = this->state.q.x(); + p.pose.orientation.y = this->state.q.y(); + p.pose.orientation.z = this->state.q.z(); + + this->path_ros.poses.push_back(p); + this->path_pub.publish(this->path_ros); + + // transform: odom to baselink + static tf2_ros::TransformBroadcaster br; + geometry_msgs::TransformStamped transformStamped; + + transformStamped.header.stamp = this->imu_stamp; + transformStamped.header.frame_id = this->odom_frame; + transformStamped.child_frame_id = this->baselink_frame; + + transformStamped.transform.translation.x = this->state.p[0]; + transformStamped.transform.translation.y = this->state.p[1]; + transformStamped.transform.translation.z = this->state.p[2]; + + transformStamped.transform.rotation.w = this->state.q.w(); + transformStamped.transform.rotation.x = this->state.q.x(); + transformStamped.transform.rotation.y = this->state.q.y(); + transformStamped.transform.rotation.z = this->state.q.z(); + + br.sendTransform(transformStamped); + + // transform: baselink to imu + transformStamped.header.stamp = this->imu_stamp; + transformStamped.header.frame_id = this->baselink_frame; + transformStamped.child_frame_id = this->imu_frame; + + transformStamped.transform.translation.x = this->extrinsics.baselink2imu.t[0]; + transformStamped.transform.translation.y = this->extrinsics.baselink2imu.t[1]; + transformStamped.transform.translation.z = this->extrinsics.baselink2imu.t[2]; + + Eigen::Quaternionf q(this->extrinsics.baselink2imu.R); + transformStamped.transform.rotation.w = q.w(); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + + br.sendTransform(transformStamped); + + // transform: baselink to lidar + transformStamped.header.stamp = this->imu_stamp; + transformStamped.header.frame_id = this->baselink_frame; + transformStamped.child_frame_id = this->lidar_frame; + + transformStamped.transform.translation.x = this->extrinsics.baselink2lidar.t[0]; + transformStamped.transform.translation.y = this->extrinsics.baselink2lidar.t[1]; + transformStamped.transform.translation.z = this->extrinsics.baselink2lidar.t[2]; + + Eigen::Quaternionf qq(this->extrinsics.baselink2lidar.R); + transformStamped.transform.rotation.w = qq.w(); + transformStamped.transform.rotation.x = qq.x(); + transformStamped.transform.rotation.y = qq.y(); + transformStamped.transform.rotation.z = qq.z(); + + br.sendTransform(transformStamped); + +} + +void dlio::OdomNode::publishCloud(pcl::PointCloud::ConstPtr published_cloud, Eigen::Matrix4f T_cloud) { + + if (this->wait_until_move_) { + if (this->length_traversed < 0.1) { return; } + } + + pcl::PointCloud::Ptr deskewed_scan_t_ (boost::make_shared>()); + + pcl::transformPointCloud (*published_cloud, *deskewed_scan_t_, T_cloud); + + // published deskewed cloud + sensor_msgs::PointCloud2 deskewed_ros; + pcl::toROSMsg(*deskewed_scan_t_, deskewed_ros); + deskewed_ros.header.stamp = this->scan_header_stamp; + deskewed_ros.header.frame_id = this->odom_frame; + this->deskewed_pub.publish(deskewed_ros); + +} + +void dlio::OdomNode::publishKeyframe(std::pair, pcl::PointCloud::ConstPtr> kf, ros::Time timestamp) { + + // Push back + geometry_msgs::Pose p; + p.position.x = kf.first.first[0]; + p.position.y = kf.first.first[1]; + p.position.z = kf.first.first[2]; + p.orientation.w = kf.first.second.w(); + p.orientation.x = kf.first.second.x(); + p.orientation.y = kf.first.second.y(); + p.orientation.z = kf.first.second.z(); + this->kf_pose_ros.poses.push_back(p); + + // Publish + this->kf_pose_ros.header.stamp = timestamp; + this->kf_pose_ros.header.frame_id = this->odom_frame; + this->kf_pose_pub.publish(this->kf_pose_ros); + + // publish keyframe scan for map + if (this->vf_use_) { + if (kf.second->points.size() == kf.second->width * kf.second->height) { + sensor_msgs::PointCloud2 keyframe_cloud_ros; + pcl::toROSMsg(*kf.second, keyframe_cloud_ros); + keyframe_cloud_ros.header.stamp = timestamp; + keyframe_cloud_ros.header.frame_id = this->odom_frame; + this->kf_cloud_pub.publish(keyframe_cloud_ros); + } + } else { + sensor_msgs::PointCloud2 keyframe_cloud_ros; + pcl::toROSMsg(*kf.second, keyframe_cloud_ros); + keyframe_cloud_ros.header.stamp = timestamp; + keyframe_cloud_ros.header.frame_id = this->odom_frame; + this->kf_cloud_pub.publish(keyframe_cloud_ros); + } + +} + +void dlio::OdomNode::getScanFromROS(const sensor_msgs::PointCloud2ConstPtr& pc) { + + pcl::PointCloud::Ptr original_scan_ (boost::make_shared>()); + pcl::fromROSMsg(*pc, *original_scan_); + + // Remove NaNs + std::vector idx; + original_scan_->is_dense = false; + pcl::removeNaNFromPointCloud(*original_scan_, *original_scan_, idx); + + // Crop Box Filter + this->crop.setInputCloud(original_scan_); + this->crop.filter(*original_scan_); + + // automatically detect sensor type + this->sensor = dlio::SensorType::UNKNOWN; + for (auto &field : pc->fields) { + if (field.name == "t") { + this->sensor = dlio::SensorType::OUSTER; + break; + } else if (field.name == "time") { + this->sensor = dlio::SensorType::VELODYNE; + break; + } + } + + if (this->sensor == dlio::SensorType::UNKNOWN) { + this->deskew_ = false; + } + + this->scan_header_stamp = pc->header.stamp; + this->original_scan = original_scan_; + +} + +void dlio::OdomNode::preprocessPoints() { + + // Deskew the original dlio-type scan + if (this->deskew_) { + + this->deskewPointcloud(); + + if (!this->first_valid_scan) { + return; + } + + } else { + + this->scan_stamp = this->scan_header_stamp.toSec(); + + // don't process scans until IMU data is present + if (!this->first_valid_scan) { + + if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) { + return; + } + + this->first_valid_scan = true; + this->T_prior = this->T; // assume no motion for the first scan + + } else { + + // IMU prior for second scan onwards + std::vector> frames; + frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p, + this->geo.prev_vel.cast(), {this->scan_stamp}); + + if (frames.size() > 0) { + this->T_prior = frames.back(); + } else { + this->T_prior = this->T; + } + + } + + pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); + pcl::transformPointCloud (*this->original_scan, *deskewed_scan_, + this->T_prior * this->extrinsics.baselink2lidar_T); + this->deskewed_scan = deskewed_scan_; + this->deskew_status = false; + } + + // Voxel Grid Filter + if (this->vf_use_) { + pcl::PointCloud::Ptr current_scan_ + (boost::make_shared>(*this->deskewed_scan)); + this->voxel.setInputCloud(current_scan_); + this->voxel.filter(*current_scan_); + this->current_scan = current_scan_; + } else { + this->current_scan = this->deskewed_scan; + } + +} + +void dlio::OdomNode::deskewPointcloud() { + + pcl::PointCloud::Ptr deskewed_scan_ (boost::make_shared>()); + deskewed_scan_->points.resize(this->original_scan->points.size()); + + // individual point timestamps should be relative to this time + double sweep_ref_time = this->scan_header_stamp.toSec(); + + // sort points by timestamp and build list of timestamps + std::function point_time_cmp; + std::function, + boost::range::index_value)> point_time_neq; + std::function)> extract_point_time; + + if (this->sensor == dlio::SensorType::OUSTER) { + point_time_cmp = [](const PointType& p1, const PointType& p2) + { return p1.t < p2.t; }; + point_time_neq = [](boost::range::index_value p1, + boost::range::index_value p2) + { return p1.value().t != p2.value().t; }; + extract_point_time = [&sweep_ref_time](boost::range::index_value pt) + { return sweep_ref_time + pt.value().t * 1e-9f; }; + } else { + point_time_cmp = [](const PointType& p1, const PointType& p2) + { return p1.time < p2.time; }; + point_time_neq = [](boost::range::index_value p1, + boost::range::index_value p2) + { return p1.value().time != p2.value().time; }; + extract_point_time = [&sweep_ref_time](boost::range::index_value pt) + { return sweep_ref_time + pt.value().time; }; + } + + // copy points into deskewed_scan_ in order of timestamp + std::partial_sort_copy(this->original_scan->points.begin(), this->original_scan->points.end(), + deskewed_scan_->points.begin(), deskewed_scan_->points.end(), point_time_cmp); + + // filter unique timestamps + auto points_unique_timestamps = deskewed_scan_->points + | boost::adaptors::indexed() + | boost::adaptors::adjacent_filtered(point_time_neq); + + // extract timestamps from points and put them in their own list + std::vector timestamps; + std::vector unique_time_indices; + for (auto it = points_unique_timestamps.begin(); it != points_unique_timestamps.end(); it++) { + timestamps.push_back(extract_point_time(*it)); + unique_time_indices.push_back(it->index()); + } + unique_time_indices.push_back(deskewed_scan_->points.size()); + + int median_pt_index = timestamps.size() / 2; + this->scan_stamp = timestamps[median_pt_index]; // set this->scan_stamp to the timestamp of the median point + + // don't process scans until IMU data is present + if (!this->first_valid_scan) { + if (this->imu_buffer.empty() || this->scan_stamp <= this->imu_buffer.back().stamp) { + return; + } + + this->first_valid_scan = true; + this->T_prior = this->T; // assume no motion for the first scan + pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); + this->deskewed_scan = deskewed_scan_; + this->deskew_status = true; + return; + } + + // IMU prior & deskewing for second scan onwards + std::vector> frames; + frames = this->integrateImu(this->prev_scan_stamp, this->lidarPose.q, this->lidarPose.p, + this->geo.prev_vel.cast(), timestamps); + this->deskew_size = frames.size(); // if integration successful, equal to timestamps.size() + + // if there are no frames between the start and end of the sweep + // that probably means that there's a sync issue + if (frames.size() != timestamps.size()) { + ROS_FATAL("Bad time sync between LiDAR and IMU!"); + + this->T_prior = this->T; + pcl::transformPointCloud (*deskewed_scan_, *deskewed_scan_, this->T_prior * this->extrinsics.baselink2lidar_T); + this->deskewed_scan = deskewed_scan_; + this->deskew_status = false; + return; + } + + // update prior to be the estimated pose at the median time of the scan (corresponds to this->scan_stamp) + this->T_prior = frames[median_pt_index]; + +#pragma omp parallel for num_threads(this->num_threads_) + for (int i = 0; i < timestamps.size(); i++) { + + Eigen::Matrix4f T = frames[i] * this->extrinsics.baselink2lidar_T; + + // transform point to world frame + for (int k = unique_time_indices[i]; k < unique_time_indices[i+1]; k++) { + auto &pt = deskewed_scan_->points[k]; + pt.getVector4fMap()[3] = 1.; + pt.getVector4fMap() = T * pt.getVector4fMap(); + } + } + + this->deskewed_scan = deskewed_scan_; + this->deskew_status = true; + +} + +void dlio::OdomNode::initializeInputTarget() { + + this->prev_scan_stamp = this->scan_stamp; + + // keep history of keyframes + this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan)); + this->keyframe_timestamps.push_back(this->scan_header_stamp); + this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); + this->keyframe_transformations.push_back(this->T_corr); + +} + +void dlio::OdomNode::setInputSource() { + this->gicp.setInputSource(this->current_scan); + this->gicp.calculateSourceCovariances(); +} + +void dlio::OdomNode::initializeDLIO() { + + // Wait for IMU + if (!this->first_imu_received || !this->imu_calibrated) { + return; + } + + this->dlio_initialized = true; + std::cout << std::endl << " DLIO initialized!" << std::endl; + +} + +void dlio::OdomNode::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& pc) { + + std::unique_lockmain_loop_running_mutex)> lock(main_loop_running_mutex); + this->main_loop_running = true; + lock.unlock(); + + double then = ros::Time::now().toSec(); + + if (this->first_scan_stamp == 0.) { + this->first_scan_stamp = pc->header.stamp.toSec(); + } + + // DLIO Initialization procedures (IMU calib, gravity align) + if (!this->dlio_initialized) { + this->initializeDLIO(); + } + + // Convert incoming scan into DLIO format + this->getScanFromROS(pc); + + // Preprocess points + this->preprocessPoints(); + + if (!this->first_valid_scan) { + return; + } + + if (this->current_scan->points.size() <= this->gicp_min_num_points_) { + ROS_FATAL("Low number of points in the cloud!"); + return; + } + + // Compute Metrics + this->metrics_thread = std::thread( &dlio::OdomNode::computeMetrics, this ); + this->metrics_thread.detach(); + + // Set Adaptive Parameters + if (this->adaptive_params_) { + this->setAdaptiveParams(); + } + + // Set new frame as input source + this->setInputSource(); + + // Set initial frame as first keyframe + if (this->keyframes.size() == 0) { + this->initializeInputTarget(); + this->main_loop_running = false; + this->submap_future = + std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state ); + this->submap_future.wait(); // wait until completion + return; + } + + // Get the next pose via IMU + S2M + GEO + this->getNextPose(); + + // Update current keyframe poses and map + this->updateKeyframes(); + + // Build keyframe normals and submap if needed (and if we're not already waiting) + if (this->new_submap_is_ready) { + this->main_loop_running = false; + this->submap_future = + std::async( std::launch::async, &dlio::OdomNode::buildKeyframesAndSubmap, this, this->state ); + } else { + lock.lock(); + this->main_loop_running = false; + lock.unlock(); + this->submap_build_cv.notify_one(); + } + + // Update trajectory + this->trajectory.push_back( std::make_pair(this->state.p, this->state.q) ); + + // Update time stamps + this->lidar_rates.push_back( 1. / (this->scan_stamp - this->prev_scan_stamp) ); + this->prev_scan_stamp = this->scan_stamp; + this->elapsed_time = this->scan_stamp - this->first_scan_stamp; + + // Publish stuff to ROS + pcl::PointCloud::ConstPtr published_cloud; + if (this->densemap_filtered_) { + published_cloud = this->current_scan; + } else { + published_cloud = this->deskewed_scan; + } + this->publish_thread = std::thread( &dlio::OdomNode::publishToROS, this, published_cloud, this->T_corr ); + this->publish_thread.detach(); + + // Update some statistics + this->comp_times.push_back(ros::Time::now().toSec() - then); + this->gicp_hasConverged = this->gicp.hasConverged(); + + // Debug statements and publish custom DLIO message + this->debug_thread = std::thread( &dlio::OdomNode::debug, this ); + this->debug_thread.detach(); + + this->geo.first_opt_done = true; + +} + +void dlio::OdomNode::callbackImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + + this->first_imu_received = true; + + sensor_msgs::Imu::Ptr imu = this->transformImu( imu_raw ); + this->imu_stamp = imu->header.stamp; + + Eigen::Vector3f lin_accel; + Eigen::Vector3f ang_vel; + + // Get IMU samples + ang_vel[0] = imu->angular_velocity.x; + ang_vel[1] = imu->angular_velocity.y; + ang_vel[2] = imu->angular_velocity.z; + + lin_accel[0] = imu->linear_acceleration.x; + lin_accel[1] = imu->linear_acceleration.y; + lin_accel[2] = imu->linear_acceleration.z; + + if (this->first_imu_stamp == 0.) { + this->first_imu_stamp = imu->header.stamp.toSec(); + } + + // IMU calibration procedure - do for three seconds + if (!this->imu_calibrated) { + + static int num_samples = 0; + static Eigen::Vector3f gyro_avg (0., 0., 0.); + static Eigen::Vector3f accel_avg (0., 0., 0.); + static bool print = true; + + if ((imu->header.stamp.toSec() - this->first_imu_stamp) < this->imu_calib_time_) { + + num_samples++; + + gyro_avg[0] += ang_vel[0]; + gyro_avg[1] += ang_vel[1]; + gyro_avg[2] += ang_vel[2]; + + accel_avg[0] += lin_accel[0]; + accel_avg[1] += lin_accel[1]; + accel_avg[2] += lin_accel[2]; + + if(print) { + std::cout << std::endl << " Calibrating IMU for " << this->imu_calib_time_ << " seconds... "; + std::cout.flush(); + print = false; + } + + } else { + + std::cout << "done" << std::endl << std::endl; + + gyro_avg /= num_samples; + accel_avg /= num_samples; + + Eigen::Vector3f grav_vec (0., 0., this->gravity_); + + if (this->gravity_align_) { + + // Estimate gravity vector - Only approximate if biases have not been pre-calibrated + grav_vec = (accel_avg - this->state.b.accel).normalized() * abs(this->gravity_); + Eigen::Quaternionf grav_q = Eigen::Quaternionf::FromTwoVectors(grav_vec, Eigen::Vector3f(0., 0., this->gravity_)); + + // set gravity aligned orientation + this->state.q = grav_q; + this->T.block(0,0,3,3) = this->state.q.toRotationMatrix(); + this->lidarPose.q = this->state.q; + + // rpy + auto euler = grav_q.toRotationMatrix().eulerAngles(2, 1, 0); + double yaw = euler[0] * (180.0/M_PI); + double pitch = euler[1] * (180.0/M_PI); + double roll = euler[2] * (180.0/M_PI); + + // use alternate representation if the yaw is smaller + if (abs(remainder(yaw + 180.0, 360.0)) < abs(yaw)) { + yaw = remainder(yaw + 180.0, 360.0); + pitch = remainder(180.0 - pitch, 360.0); + roll = remainder(roll + 180.0, 360.0); + } + std::cout << " Estimated initial attitude:" << std::endl; + std::cout << " Roll [deg]: " << to_string_with_precision(roll, 4) << std::endl; + std::cout << " Pitch [deg]: " << to_string_with_precision(pitch, 4) << std::endl; + std::cout << " Yaw [deg]: " << to_string_with_precision(yaw, 4) << std::endl; + std::cout << std::endl; + } + + if (this->calibrate_accel_) { + + // subtract gravity from avg accel to get bias + this->state.b.accel = accel_avg - grav_vec; + + std::cout << " Accel biases [xyz]: " << to_string_with_precision(this->state.b.accel[0], 8) << ", " + << to_string_with_precision(this->state.b.accel[1], 8) << ", " + << to_string_with_precision(this->state.b.accel[2], 8) << std::endl; + } + + if (this->calibrate_gyro_) { + + this->state.b.gyro = gyro_avg; + + std::cout << " Gyro biases [xyz]: " << to_string_with_precision(this->state.b.gyro[0], 8) << ", " + << to_string_with_precision(this->state.b.gyro[1], 8) << ", " + << to_string_with_precision(this->state.b.gyro[2], 8) << std::endl; + } + + this->imu_calibrated = true; + + } + + } else { + + double dt = imu->header.stamp.toSec() - this->prev_imu_stamp; + this->imu_rates.push_back( 1./dt ); + if (dt == 0) { return; } + + // Apply the calibrated bias to the new IMU measurements + this->imu_meas.stamp = imu->header.stamp.toSec(); + this->imu_meas.dt = dt; + this->prev_imu_stamp = this->imu_meas.stamp; + + Eigen::Vector3f lin_accel_corrected = (this->imu_accel_sm_ * lin_accel) - this->state.b.accel; + Eigen::Vector3f ang_vel_corrected = ang_vel - this->state.b.gyro; + + this->imu_meas.lin_accel = lin_accel_corrected; + this->imu_meas.ang_vel = ang_vel_corrected; + + // Store calibrated IMU measurements into imu buffer for manual integration later. + this->mtx_imu.lock(); + this->imu_buffer.push_front(this->imu_meas); + this->mtx_imu.unlock(); + + // Notify the callbackPointCloud thread that IMU data exists for this time + this->cv_imu_stamp.notify_one(); + + if (this->geo.first_opt_done) { + // Geometric Observer: Propagate State + this->propagateState(); + } + + } + +} + +void dlio::OdomNode::getNextPose() { + + // Check if the new submap is ready to be used + this->new_submap_is_ready = (this->submap_future.wait_for(std::chrono::seconds(0)) == std::future_status::ready); + + if (this->new_submap_is_ready && this->submap_hasChanged) { + + // Set the current global submap as the target cloud + this->gicp.registerInputTarget(this->submap_cloud); + + // Set submap kdtree + this->gicp.target_kdtree_ = this->submap_kdtree; + + // Set target cloud's normals as submap normals + this->gicp.setTargetCovariances(this->submap_normals); + + this->submap_hasChanged = false; + } + + // Align with current submap with global IMU transformation as initial guess + pcl::PointCloud::Ptr aligned (boost::make_shared>()); + this->gicp.align(*aligned); + + // Get final transformation in global frame + this->T_corr = this->gicp.getFinalTransformation(); // "correction" transformation + this->T = this->T_corr * this->T_prior; + + // Update next global pose + // Both source and target clouds are in the global frame now, so tranformation is global + this->propagateGICP(); + + // Geometric observer update + this->updateState(); + +} + +bool dlio::OdomNode::imuMeasFromTimeRange(double start_time, double end_time, + boost::circular_buffer::reverse_iterator& begin_imu_it, + boost::circular_buffer::reverse_iterator& end_imu_it) { + + if (this->imu_buffer.empty() || this->imu_buffer.front().stamp < end_time) { + // Wait for the latest IMU data + std::unique_lockmtx_imu)> lock(this->mtx_imu); + this->cv_imu_stamp.wait(lock, [this, &end_time]{ return this->imu_buffer.front().stamp >= end_time; }); + } + + auto imu_it = this->imu_buffer.begin(); + + auto last_imu_it = imu_it; + imu_it++; + while (imu_it != this->imu_buffer.end() && imu_it->stamp >= end_time) { + last_imu_it = imu_it; + imu_it++; + } + + while (imu_it != this->imu_buffer.end() && imu_it->stamp >= start_time) { + imu_it++; + } + + if (imu_it == this->imu_buffer.end()) { + // not enough IMU measurements, return false + return false; + } + imu_it++; + + // Set reverse iterators (to iterate forward in time) + end_imu_it = boost::circular_buffer::reverse_iterator(last_imu_it); + begin_imu_it = boost::circular_buffer::reverse_iterator(imu_it); + + return true; +} + +std::vector> +dlio::OdomNode::integrateImu(double start_time, Eigen::Quaternionf q_init, Eigen::Vector3f p_init, + Eigen::Vector3f v_init, const std::vector& sorted_timestamps) { + + const std::vector> empty; + + if (sorted_timestamps.empty() || start_time > sorted_timestamps.front()) { + // invalid input, return empty vector + return empty; + } + + boost::circular_buffer::reverse_iterator begin_imu_it; + boost::circular_buffer::reverse_iterator end_imu_it; + if (this->imuMeasFromTimeRange(start_time, sorted_timestamps.back(), begin_imu_it, end_imu_it) == false) { + // not enough IMU measurements, return empty vector + return empty; + } + + // Backwards integration to find pose at first IMU sample + const ImuMeas& f1 = *begin_imu_it; + const ImuMeas& f2 = *(begin_imu_it+1); + + // Time between first two IMU samples + double dt = f2.dt; + + // Time between first IMU sample and start_time + double idt = start_time - f1.stamp; + + // Angular acceleration between first two IMU samples + Eigen::Vector3f alpha_dt = f2.ang_vel - f1.ang_vel; + Eigen::Vector3f alpha = alpha_dt / dt; + + // Average angular velocity (reversed) between first IMU sample and start_time + Eigen::Vector3f omega_i = -(f1.ang_vel + 0.5*alpha*idt); + + // Set q_init to orientation at first IMU sample + q_init = Eigen::Quaternionf ( + q_init.w() - 0.5*( q_init.x()*omega_i[0] + q_init.y()*omega_i[1] + q_init.z()*omega_i[2] ) * idt, + q_init.x() + 0.5*( q_init.w()*omega_i[0] - q_init.z()*omega_i[1] + q_init.y()*omega_i[2] ) * idt, + q_init.y() + 0.5*( q_init.z()*omega_i[0] + q_init.w()*omega_i[1] - q_init.x()*omega_i[2] ) * idt, + q_init.z() + 0.5*( q_init.x()*omega_i[1] - q_init.y()*omega_i[0] + q_init.w()*omega_i[2] ) * idt + ); + q_init.normalize(); + + // Average angular velocity between first two IMU samples + Eigen::Vector3f omega = f1.ang_vel + 0.5*alpha_dt; + + // Orientation at second IMU sample + Eigen::Quaternionf q2 ( + q_init.w() - 0.5*( q_init.x()*omega[0] + q_init.y()*omega[1] + q_init.z()*omega[2] ) * dt, + q_init.x() + 0.5*( q_init.w()*omega[0] - q_init.z()*omega[1] + q_init.y()*omega[2] ) * dt, + q_init.y() + 0.5*( q_init.z()*omega[0] + q_init.w()*omega[1] - q_init.x()*omega[2] ) * dt, + q_init.z() + 0.5*( q_init.x()*omega[1] - q_init.y()*omega[0] + q_init.w()*omega[2] ) * dt + ); + q2.normalize(); + + // Acceleration at first IMU sample + Eigen::Vector3f a1 = q_init._transformVector(f1.lin_accel); + a1[2] -= this->gravity_; + + // Acceleration at second IMU sample + Eigen::Vector3f a2 = q2._transformVector(f2.lin_accel); + a2[2] -= this->gravity_; + + // Jerk between first two IMU samples + Eigen::Vector3f j = (a2 - a1) / dt; + + // Set v_init to velocity at first IMU sample (go backwards from start_time) + v_init -= a1*idt + 0.5*j*idt*idt; + + // Set p_init to position at first IMU sample (go backwards from start_time) + p_init -= v_init*idt + 0.5*a1*idt*idt + (1/6.)*j*idt*idt*idt; + + return this->integrateImuInternal(q_init, p_init, v_init, sorted_timestamps, begin_imu_it, end_imu_it); +} + +std::vector> +dlio::OdomNode::integrateImuInternal(Eigen::Quaternionf q_init, Eigen::Vector3f p_init, Eigen::Vector3f v_init, + const std::vector& sorted_timestamps, + boost::circular_buffer::reverse_iterator begin_imu_it, + boost::circular_buffer::reverse_iterator end_imu_it) { + + std::vector> imu_se3; + + // Initialization + Eigen::Quaternionf q = q_init; + Eigen::Vector3f p = p_init; + Eigen::Vector3f v = v_init; + Eigen::Vector3f a = q._transformVector(begin_imu_it->lin_accel); + a[2] -= this->gravity_; + + // Iterate over IMU measurements and timestamps + auto prev_imu_it = begin_imu_it; + auto imu_it = prev_imu_it + 1; + + auto stamp_it = sorted_timestamps.begin(); + + for (; imu_it != end_imu_it; imu_it++) { + + const ImuMeas& f0 = *prev_imu_it; + const ImuMeas& f = *imu_it; + + // Time between IMU samples + double dt = f.dt; + + // Angular acceleration + Eigen::Vector3f alpha_dt = f.ang_vel - f0.ang_vel; + Eigen::Vector3f alpha = alpha_dt / dt; + + // Average angular velocity + Eigen::Vector3f omega = f0.ang_vel + 0.5*alpha_dt; + + // Orientation + q = Eigen::Quaternionf ( + q.w() - 0.5*( q.x()*omega[0] + q.y()*omega[1] + q.z()*omega[2] ) * dt, + q.x() + 0.5*( q.w()*omega[0] - q.z()*omega[1] + q.y()*omega[2] ) * dt, + q.y() + 0.5*( q.z()*omega[0] + q.w()*omega[1] - q.x()*omega[2] ) * dt, + q.z() + 0.5*( q.x()*omega[1] - q.y()*omega[0] + q.w()*omega[2] ) * dt + ); + q.normalize(); + + // Acceleration + Eigen::Vector3f a0 = a; + a = q._transformVector(f.lin_accel); + a[2] -= this->gravity_; + + // Jerk + Eigen::Vector3f j_dt = a - a0; + Eigen::Vector3f j = j_dt / dt; + + // Interpolate for given timestamps + while (stamp_it != sorted_timestamps.end() && *stamp_it <= f.stamp) { + // Time between previous IMU sample and given timestamp + double idt = *stamp_it - f0.stamp; + + // Average angular velocity + Eigen::Vector3f omega_i = f0.ang_vel + 0.5*alpha*idt; + + // Orientation + Eigen::Quaternionf q_i ( + q.w() - 0.5*( q.x()*omega_i[0] + q.y()*omega_i[1] + q.z()*omega_i[2] ) * idt, + q.x() + 0.5*( q.w()*omega_i[0] - q.z()*omega_i[1] + q.y()*omega_i[2] ) * idt, + q.y() + 0.5*( q.z()*omega_i[0] + q.w()*omega_i[1] - q.x()*omega_i[2] ) * idt, + q.z() + 0.5*( q.x()*omega_i[1] - q.y()*omega_i[0] + q.w()*omega_i[2] ) * idt + ); + q_i.normalize(); + + // Position + Eigen::Vector3f p_i = p + v*idt + 0.5*a0*idt*idt + (1/6.)*j*idt*idt*idt; + + // Transformation + Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); + T.block(0, 0, 3, 3) = q_i.toRotationMatrix(); + T.block(0, 3, 3, 1) = p_i; + + imu_se3.push_back(T); + + stamp_it++; + } + + // Position + p += v*dt + 0.5*a0*dt*dt + (1/6.)*j_dt*dt*dt; + + // Velocity + v += a0*dt + 0.5*j_dt*dt; + + prev_imu_it = imu_it; + + } + + return imu_se3; + +} + +void dlio::OdomNode::propagateGICP() { + + this->lidarPose.p << this->T(0,3), this->T(1,3), this->T(2,3); + + Eigen::Matrix3f rotSO3; + rotSO3 << this->T(0,0), this->T(0,1), this->T(0,2), + this->T(1,0), this->T(1,1), this->T(1,2), + this->T(2,0), this->T(2,1), this->T(2,2); + + Eigen::Quaternionf q(rotSO3); + + // Normalize quaternion + double norm = sqrt(q.w()*q.w() + q.x()*q.x() + q.y()*q.y() + q.z()*q.z()); + q.w() /= norm; q.x() /= norm; q.y() /= norm; q.z() /= norm; + this->lidarPose.q = q; + +} + +void dlio::OdomNode::propagateState() { + + // Lock thread to prevent state from being accessed by UpdateState + std::lock_guard lock( this->geo.mtx ); + + double dt = this->imu_meas.dt; + + Eigen::Quaternionf qhat = this->state.q, omega; + Eigen::Vector3f world_accel; + + // Transform accel from body to world frame + world_accel = qhat._transformVector(this->imu_meas.lin_accel); + + // Accel propogation + this->state.p[0] += this->state.v.lin.w[0]*dt + 0.5*dt*dt*world_accel[0]; + this->state.p[1] += this->state.v.lin.w[1]*dt + 0.5*dt*dt*world_accel[1]; + this->state.p[2] += this->state.v.lin.w[2]*dt + 0.5*dt*dt*(world_accel[2] - this->gravity_); + + this->state.v.lin.w[0] += world_accel[0]*dt; + this->state.v.lin.w[1] += world_accel[1]*dt; + this->state.v.lin.w[2] += (world_accel[2] - this->gravity_)*dt; + this->state.v.lin.b = this->state.q.toRotationMatrix().inverse() * this->state.v.lin.w; + + // Gyro propogation + omega.w() = 0; + omega.vec() = this->imu_meas.ang_vel; + Eigen::Quaternionf tmp = qhat * omega; + this->state.q.w() += 0.5 * dt * tmp.w(); + this->state.q.vec() += 0.5 * dt * tmp.vec(); + + // Ensure quaternion is properly normalized + this->state.q.normalize(); + + this->state.v.ang.b = this->imu_meas.ang_vel; + this->state.v.ang.w = this->state.q.toRotationMatrix() * this->state.v.ang.b; + +} + +void dlio::OdomNode::updateState() { + + // Lock thread to prevent state from being accessed by PropagateState + std::lock_guard lock( this->geo.mtx ); + + Eigen::Vector3f pin = this->lidarPose.p; + Eigen::Quaternionf qin = this->lidarPose.q; + double dt = this->scan_stamp - this->prev_scan_stamp; + + Eigen::Quaternionf qe, qhat, qcorr; + qhat = this->state.q; + + // Constuct error quaternion + qe = qhat.conjugate()*qin; + + double sgn = 1.; + if (qe.w() < 0) { + sgn = -1; + } + + // Construct quaternion correction + qcorr.w() = 1 - abs(qe.w()); + qcorr.vec() = sgn*qe.vec(); + qcorr = qhat * qcorr; + + Eigen::Vector3f err = pin - this->state.p; + Eigen::Vector3f err_body; + + err_body = qhat.conjugate()._transformVector(err); + + double abias_max = this->geo_abias_max_; + double gbias_max = this->geo_gbias_max_; + + // Update accel bias + this->state.b.accel -= dt * this->geo_Kab_ * err_body; + this->state.b.accel = this->state.b.accel.array().min(abias_max).max(-abias_max); + + // Update gyro bias + this->state.b.gyro[0] -= dt * this->geo_Kgb_ * qe.w() * qe.x(); + this->state.b.gyro[1] -= dt * this->geo_Kgb_ * qe.w() * qe.y(); + this->state.b.gyro[2] -= dt * this->geo_Kgb_ * qe.w() * qe.z(); + this->state.b.gyro = this->state.b.gyro.array().min(gbias_max).max(-gbias_max); + + // Update state + this->state.p += dt * this->geo_Kp_ * err; + this->state.v.lin.w += dt * this->geo_Kv_ * err; + + this->state.q.w() += dt * this->geo_Kq_ * qcorr.w(); + this->state.q.x() += dt * this->geo_Kq_ * qcorr.x(); + this->state.q.y() += dt * this->geo_Kq_ * qcorr.y(); + this->state.q.z() += dt * this->geo_Kq_ * qcorr.z(); + this->state.q.normalize(); + + // store previous pose, orientation, and velocity + this->geo.prev_p = this->state.p; + this->geo.prev_q = this->state.q; + this->geo.prev_vel = this->state.v.lin.w; + +} + +sensor_msgs::Imu::Ptr dlio::OdomNode::transformImu(const sensor_msgs::Imu::ConstPtr& imu_raw) { + + sensor_msgs::Imu::Ptr imu (new sensor_msgs::Imu); + + // Copy header + imu->header = imu_raw->header; + + static double prev_stamp = imu->header.stamp.toSec(); + double dt = imu->header.stamp.toSec() - prev_stamp; + prev_stamp = imu->header.stamp.toSec(); + + if (dt == 0) { dt = 1.0/200.0; } + + // Transform angular velocity (will be the same on a rigid body, so just rotate to ROS convention) + Eigen::Vector3f ang_vel(imu_raw->angular_velocity.x, + imu_raw->angular_velocity.y, + imu_raw->angular_velocity.z); + + Eigen::Vector3f ang_vel_cg = this->extrinsics.baselink2imu.R * ang_vel; + + imu->angular_velocity.x = ang_vel_cg[0]; + imu->angular_velocity.y = ang_vel_cg[1]; + imu->angular_velocity.z = ang_vel_cg[2]; + + static Eigen::Vector3f ang_vel_cg_prev = ang_vel_cg; + + // Transform linear acceleration (need to account for component due to translational difference) + Eigen::Vector3f lin_accel(imu_raw->linear_acceleration.x, + imu_raw->linear_acceleration.y, + imu_raw->linear_acceleration.z); + + Eigen::Vector3f lin_accel_cg = this->extrinsics.baselink2imu.R * lin_accel; + + lin_accel_cg = lin_accel_cg + + ((ang_vel_cg - ang_vel_cg_prev) / dt).cross(-this->extrinsics.baselink2imu.t) + + ang_vel_cg.cross(ang_vel_cg.cross(-this->extrinsics.baselink2imu.t)); + + ang_vel_cg_prev = ang_vel_cg; + + imu->linear_acceleration.x = lin_accel_cg[0]; + imu->linear_acceleration.y = lin_accel_cg[1]; + imu->linear_acceleration.z = lin_accel_cg[2]; + + return imu; + +} + +void dlio::OdomNode::computeMetrics() { + this->computeSpaciousness(); + this->computeDensity(); +} + +void dlio::OdomNode::computeSpaciousness() { + + // compute range of points + std::vector ds; + + for (int i = 0; i <= this->original_scan->points.size(); i++) { + float d = std::sqrt(pow(this->original_scan->points[i].x, 2) + + pow(this->original_scan->points[i].y, 2)); + ds.push_back(d); + } + + // median + std::nth_element(ds.begin(), ds.begin() + ds.size()/2, ds.end()); + float median_curr = ds[ds.size()/2]; + static float median_prev = median_curr; + float median_lpf = 0.95*median_prev + 0.05*median_curr; + median_prev = median_lpf; + + // push + this->metrics.spaciousness.push_back( median_lpf ); + +} + +void dlio::OdomNode::computeDensity() { + + float density; + + if (!this->geo.first_opt_done) { + density = 0.; + } else { + density = this->gicp.source_density_; + } + + static float density_prev = density; + float density_lpf = 0.95*density_prev + 0.05*density; + density_prev = density_lpf; + + this->metrics.density.push_back( density_lpf ); + +} + +void dlio::OdomNode::computeConvexHull() { + + // at least 4 keyframes for convex hull + if (this->num_processed_keyframes < 4) { + return; + } + + // create a pointcloud with points at keyframes + pcl::PointCloud::Ptr cloud = + pcl::PointCloud::Ptr (boost::make_shared>()); + + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + for (int i = 0; i < this->num_processed_keyframes; i++) { + PointType pt; + pt.x = this->keyframes[i].first.first[0]; + pt.y = this->keyframes[i].first.first[1]; + pt.z = this->keyframes[i].first.first[2]; + cloud->push_back(pt); + } + lock.unlock(); + + // calculate the convex hull of the point cloud + this->convex_hull.setInputCloud(cloud); + + // get the indices of the keyframes on the convex hull + pcl::PointCloud::Ptr convex_points = + pcl::PointCloud::Ptr (boost::make_shared>()); + this->convex_hull.reconstruct(*convex_points); + + pcl::PointIndices::Ptr convex_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); + this->convex_hull.getHullPointIndices(*convex_hull_point_idx); + + this->keyframe_convex.clear(); + for (int i=0; iindices.size(); ++i) { + this->keyframe_convex.push_back(convex_hull_point_idx->indices[i]); + } + +} + +void dlio::OdomNode::computeConcaveHull() { + + // at least 5 keyframes for concave hull + if (this->num_processed_keyframes < 5) { + return; + } + + // create a pointcloud with points at keyframes + pcl::PointCloud::Ptr cloud = + pcl::PointCloud::Ptr (boost::make_shared>()); + + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + for (int i = 0; i < this->num_processed_keyframes; i++) { + PointType pt; + pt.x = this->keyframes[i].first.first[0]; + pt.y = this->keyframes[i].first.first[1]; + pt.z = this->keyframes[i].first.first[2]; + cloud->push_back(pt); + } + lock.unlock(); + + // calculate the concave hull of the point cloud + this->concave_hull.setInputCloud(cloud); + + // get the indices of the keyframes on the concave hull + pcl::PointCloud::Ptr concave_points = + pcl::PointCloud::Ptr (boost::make_shared>()); + this->concave_hull.reconstruct(*concave_points); + + pcl::PointIndices::Ptr concave_hull_point_idx = pcl::PointIndices::Ptr (boost::make_shared()); + this->concave_hull.getHullPointIndices(*concave_hull_point_idx); + + this->keyframe_concave.clear(); + for (int i=0; iindices.size(); ++i) { + this->keyframe_concave.push_back(concave_hull_point_idx->indices[i]); + } + +} + +void dlio::OdomNode::updateKeyframes() { + + // calculate difference in pose and rotation to all poses in trajectory + float closest_d = std::numeric_limits::infinity(); + int closest_idx = 0; + int keyframes_idx = 0; + + int num_nearby = 0; + + for (const auto& k : this->keyframes) { + + // calculate distance between current pose and pose in keyframes + float delta_d = sqrt( pow(this->state.p[0] - k.first.first[0], 2) + + pow(this->state.p[1] - k.first.first[1], 2) + + pow(this->state.p[2] - k.first.first[2], 2) ); + + // count the number nearby current pose + if (delta_d <= this->keyframe_thresh_dist_ * 1.5){ + ++num_nearby; + } + + // store into variable + if (delta_d < closest_d) { + closest_d = delta_d; + closest_idx = keyframes_idx; + } + + keyframes_idx++; + + } + + // get closest pose and corresponding rotation + Eigen::Vector3f closest_pose = this->keyframes[closest_idx].first.first; + Eigen::Quaternionf closest_pose_r = this->keyframes[closest_idx].first.second; + + // calculate distance between current pose and closest pose from above + float dd = sqrt( pow(this->state.p[0] - closest_pose[0], 2) + + pow(this->state.p[1] - closest_pose[1], 2) + + pow(this->state.p[2] - closest_pose[2], 2) ); + + // calculate difference in orientation using SLERP + Eigen::Quaternionf dq; + + if (this->state.q.dot(closest_pose_r) < 0.) { + Eigen::Quaternionf lq = closest_pose_r; + lq.w() *= -1.; lq.x() *= -1.; lq.y() *= -1.; lq.z() *= -1.; + dq = this->state.q * lq.inverse(); + } else { + dq = this->state.q * closest_pose_r.inverse(); + } + + double theta_rad = 2. * atan2(sqrt( pow(dq.x(), 2) + pow(dq.y(), 2) + pow(dq.z(), 2) ), dq.w()); + double theta_deg = theta_rad * (180.0/M_PI); + + // update keyframes + bool newKeyframe = false; + + // spaciousness keyframing + if (abs(dd) > this->keyframe_thresh_dist_ || abs(theta_deg) > this->keyframe_thresh_rot_) { + newKeyframe = true; + } + + // rotational exploration keyframing + if (abs(dd) <= this->keyframe_thresh_dist_ && abs(theta_deg) > this->keyframe_thresh_rot_ && num_nearby <= 1) { + newKeyframe = true; + } + + // check for nearby keyframes + if (abs(dd) <= this->keyframe_thresh_dist_) { + newKeyframe = false; + } else if (abs(dd) <= 0.5) { + newKeyframe = false; + } + + if (newKeyframe) { + + // update keyframe vector + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan)); + this->keyframe_timestamps.push_back(this->scan_header_stamp); + this->keyframe_normals.push_back(this->gicp.getSourceCovariances()); + this->keyframe_transformations.push_back(this->T_corr); + lock.unlock(); + + } + +} + +void dlio::OdomNode::setAdaptiveParams() { + + // Spaciousness + float sp = this->metrics.spaciousness.back(); + + if (sp < 0.5) { sp = 0.5; } + if (sp > 5.0) { sp = 5.0; } + + this->keyframe_thresh_dist_ = sp; + + // Density + float den = this->metrics.density.back(); + + if (den < 0.5*this->gicp_max_corr_dist_) { den = 0.5*this->gicp_max_corr_dist_; } + if (den > 2.0*this->gicp_max_corr_dist_) { den = 2.0*this->gicp_max_corr_dist_; } + + if (sp < 5.0) { den = 0.5*this->gicp_max_corr_dist_; }; + if (sp > 5.0) { den = 2.0*this->gicp_max_corr_dist_; }; + + this->gicp.setMaxCorrespondenceDistance(den); + + // Concave hull alpha + this->concave_hull.setAlpha(this->keyframe_thresh_dist_); + +} + +void dlio::OdomNode::pushSubmapIndices(std::vector dists, int k, std::vector frames) { + + // make sure dists is not empty + if (!dists.size()) { return; } + + // maintain max heap of at most k elements + std::priority_queue pq; + + for (auto d : dists) { + if (pq.size() >= k && pq.top() > d) { + pq.push(d); + pq.pop(); + } else if (pq.size() < k) { + pq.push(d); + } + } + + // get the kth smallest element, which should be at the top of the heap + float kth_element = pq.top(); + + // get all elements smaller or equal to the kth smallest element + for (int i = 0; i < dists.size(); ++i) { + if (dists[i] <= kth_element) + this->submap_kf_idx_curr.push_back(frames[i]); + } + +} + +void dlio::OdomNode::buildSubmap(State vehicle_state) { + + // clear vector of keyframe indices to use for submap + this->submap_kf_idx_curr.clear(); + + // calculate distance between current pose and poses in keyframe set + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + std::vector ds; + std::vector keyframe_nn; + for (int i = 0; i < this->num_processed_keyframes; i++) { + float d = sqrt( pow(vehicle_state.p[0] - this->keyframes[i].first.first[0], 2) + + pow(vehicle_state.p[1] - this->keyframes[i].first.first[1], 2) + + pow(vehicle_state.p[2] - this->keyframes[i].first.first[2], 2) ); + ds.push_back(d); + keyframe_nn.push_back(i); + } + lock.unlock(); + + // get indices for top K nearest neighbor keyframe poses + this->pushSubmapIndices(ds, this->submap_knn_, keyframe_nn); + + // get convex hull indices + this->computeConvexHull(); + + // get distances for each keyframe on convex hull + std::vector convex_ds; + for (const auto& c : this->keyframe_convex) { + convex_ds.push_back(ds[c]); + } + + // get indices for top kNN for convex hull + this->pushSubmapIndices(convex_ds, this->submap_kcv_, this->keyframe_convex); + + // get concave hull indices + this->computeConcaveHull(); + + // get distances for each keyframe on concave hull + std::vector concave_ds; + for (const auto& c : this->keyframe_concave) { + concave_ds.push_back(ds[c]); + } + + // get indices for top kNN for concave hull + this->pushSubmapIndices(concave_ds, this->submap_kcc_, this->keyframe_concave); + + // concatenate all submap clouds and normals + std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); + auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); + this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end()); + + // sort current and previous submap kf list of indices + std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); + std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end()); + + // check if submap has changed from previous iteration + if (this->submap_kf_idx_curr != this->submap_kf_idx_prev){ + + this->submap_hasChanged = true; + + // Pause to prevent stealing resources from the main loop if it is running. + this->pauseSubmapBuildIfNeeded(); + + // reinitialize submap cloud and normals + pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); + std::shared_ptr submap_normals_ (std::make_shared()); + + for (auto k : this->submap_kf_idx_curr) { + + // create current submap cloud + lock.lock(); + *submap_cloud_ += *this->keyframes[k].second; + lock.unlock(); + + // grab corresponding submap cloud's normals + submap_normals_->insert( std::end(*submap_normals_), + std::begin(*(this->keyframe_normals[k])), std::end(*(this->keyframe_normals[k])) ); + } + + this->submap_cloud = submap_cloud_; + this->submap_normals = submap_normals_; + + // Pause to prevent stealing resources from the main loop if it is running. + this->pauseSubmapBuildIfNeeded(); + + this->gicp_temp.setInputTarget(this->submap_cloud); + this->submap_kdtree = this->gicp_temp.target_kdtree_; + + this->submap_kf_idx_prev = this->submap_kf_idx_curr; + } +} + +void dlio::OdomNode::buildKeyframesAndSubmap(State vehicle_state) { + + // transform the new keyframe(s) and associated covariance list(s) + std::unique_lockkeyframes_mutex)> lock(this->keyframes_mutex); + + for (int i = this->num_processed_keyframes; i < this->keyframes.size(); i++) { + pcl::PointCloud::ConstPtr raw_keyframe = this->keyframes[i].second; + std::shared_ptr raw_covariances = this->keyframe_normals[i]; + Eigen::Matrix4f T = this->keyframe_transformations[i]; + lock.unlock(); + + Eigen::Matrix4d Td = T.cast(); + + pcl::PointCloud::Ptr transformed_keyframe (boost::make_shared>()); + pcl::transformPointCloud (*raw_keyframe, *transformed_keyframe, T); + + std::shared_ptr transformed_covariances (std::make_shared(raw_covariances->size())); + std::transform(raw_covariances->begin(), raw_covariances->end(), transformed_covariances->begin(), + [&Td](Eigen::Matrix4d cov) { return Td * cov * Td.transpose(); }); + + ++this->num_processed_keyframes; + + lock.lock(); + this->keyframes[i].second = transformed_keyframe; + this->keyframe_normals[i] = transformed_covariances; + + this->publish_keyframe_thread = std::thread( &dlio::OdomNode::publishKeyframe, this, this->keyframes[i], this->keyframe_timestamps[i] ); + this->publish_keyframe_thread.detach(); + } + + lock.unlock(); + + // Pause to prevent stealing resources from the main loop if it is running. + this->pauseSubmapBuildIfNeeded(); + + this->buildSubmap(vehicle_state); +} + +void dlio::OdomNode::pauseSubmapBuildIfNeeded() { + std::unique_lockmain_loop_running_mutex)> lock(this->main_loop_running_mutex); + this->submap_build_cv.wait(lock, [this]{ return !this->main_loop_running; }); +} + +void dlio::OdomNode::debug() { + + // Total length traversed + double length_traversed = 0.; + Eigen::Vector3f p_curr = Eigen::Vector3f(0., 0., 0.); + Eigen::Vector3f p_prev = Eigen::Vector3f(0., 0., 0.); + for (const auto& t : this->trajectory) { + if (p_prev == Eigen::Vector3f(0., 0., 0.)) { + p_prev = t.first; + continue; + } + p_curr = t.first; + double l = sqrt(pow(p_curr[0] - p_prev[0], 2) + pow(p_curr[1] - p_prev[1], 2) + pow(p_curr[2] - p_prev[2], 2)); + + if (l >= 0.1) { + length_traversed += l; + p_prev = p_curr; + } + } + this->length_traversed = length_traversed; + + // Average computation time + double avg_comp_time = + std::accumulate(this->comp_times.begin(), this->comp_times.end(), 0.0) / this->comp_times.size(); + + // Average sensor rates + int win_size = 100; + double avg_imu_rate; + double avg_lidar_rate; + if (this->imu_rates.size() < win_size) { + avg_imu_rate = + std::accumulate(this->imu_rates.begin(), this->imu_rates.end(), 0.0) / this->imu_rates.size(); + } else { + avg_imu_rate = + std::accumulate(this->imu_rates.end()-win_size, this->imu_rates.end(), 0.0) / win_size; + } + if (this->lidar_rates.size() < win_size) { + avg_lidar_rate = + std::accumulate(this->lidar_rates.begin(), this->lidar_rates.end(), 0.0) / this->lidar_rates.size(); + } else { + avg_lidar_rate = + std::accumulate(this->lidar_rates.end()-win_size, this->lidar_rates.end(), 0.0) / win_size; + } + + // RAM Usage + double vm_usage = 0.0; + double resident_set = 0.0; + std::ifstream stat_stream("/proc/self/stat", std::ios_base::in); //get info from proc directory + std::string pid, comm, state, ppid, pgrp, session, tty_nr; + std::string tpgid, flags, minflt, cminflt, majflt, cmajflt; + std::string utime, stime, cutime, cstime, priority, nice; + std::string num_threads, itrealvalue, starttime; + unsigned long vsize; + long rss; + stat_stream >> pid >> comm >> state >> ppid >> pgrp >> session >> tty_nr + >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt + >> utime >> stime >> cutime >> cstime >> priority >> nice + >> num_threads >> itrealvalue >> starttime >> vsize >> rss; // don't care about the rest + stat_stream.close(); + long page_size_kb = sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages + vm_usage = vsize / 1024.0; + resident_set = rss * page_size_kb; + + // CPU Usage + struct tms timeSample; + clock_t now; + double cpu_percent; + now = times(&timeSample); + if (now <= this->lastCPU || timeSample.tms_stime < this->lastSysCPU || + timeSample.tms_utime < this->lastUserCPU) { + cpu_percent = -1.0; + } else { + cpu_percent = (timeSample.tms_stime - this->lastSysCPU) + (timeSample.tms_utime - this->lastUserCPU); + cpu_percent /= (now - this->lastCPU); + cpu_percent /= this->numProcessors; + cpu_percent *= 100.; + } + this->lastCPU = now; + this->lastSysCPU = timeSample.tms_stime; + this->lastUserCPU = timeSample.tms_utime; + this->cpu_percents.push_back(cpu_percent); + double avg_cpu_usage = + std::accumulate(this->cpu_percents.begin(), this->cpu_percents.end(), 0.0) / this->cpu_percents.size(); + + // Print to terminal + printf("\033[2J\033[1;1H"); + + std::cout << std::endl + << "+-------------------------------------------------------------------+" << std::endl; + std::cout << "| Direct LiDAR-Inertial Odometry v" << this->version_ << " |" + << std::endl; + std::cout << "+-------------------------------------------------------------------+" << std::endl; + + std::time_t curr_time = this->scan_stamp; + std::string asc_time = std::asctime(std::localtime(&curr_time)); asc_time.pop_back(); + std::cout << "| " << std::left << asc_time; + std::cout << std::right << std::setfill(' ') << std::setw(42) + << "Elapsed Time: " + to_string_with_precision(this->elapsed_time, 2) + " seconds " + << "|" << std::endl; + + if ( !this->cpu_type.empty() ) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << this->cpu_type + " x " + std::to_string(this->numProcessors) + << "|" << std::endl; + } + + if (this->sensor == dlio::SensorType::OUSTER) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Sensor Rates: Ouster @ " + to_string_with_precision(avg_lidar_rate, 2) + + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" + << "|" << std::endl; + } else if (this->sensor == dlio::SensorType::VELODYNE) { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Sensor Rates: Velodyne @ " + to_string_with_precision(avg_lidar_rate, 2) + + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" + << "|" << std::endl; + } else { + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Sensor Rates: LiDAR @ " + to_string_with_precision(avg_lidar_rate, 2) + + " Hz, IMU @ " + to_string_with_precision(avg_imu_rate, 2) + " Hz" + << "|" << std::endl; + } + + std::cout << "|===================================================================|" << std::endl; + + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Position {W} [xyz] :: " + to_string_with_precision(this->state.p[0], 4) + " " + + to_string_with_precision(this->state.p[1], 4) + " " + + to_string_with_precision(this->state.p[2], 4) + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Orientation {W} [wxyz] :: " + to_string_with_precision(this->state.q.w(), 4) + " " + + to_string_with_precision(this->state.q.x(), 4) + " " + + to_string_with_precision(this->state.q.y(), 4) + " " + + to_string_with_precision(this->state.q.z(), 4) + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Lin Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.lin.b[0], 4) + " " + + to_string_with_precision(this->state.v.lin.b[1], 4) + " " + + to_string_with_precision(this->state.v.lin.b[2], 4) + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Ang Velocity {B} [xyz] :: " + to_string_with_precision(this->state.v.ang.b[0], 4) + " " + + to_string_with_precision(this->state.v.ang.b[1], 4) + " " + + to_string_with_precision(this->state.v.ang.b[2], 4) + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Accel Bias [xyz] :: " + to_string_with_precision(this->state.b.accel[0], 8) + " " + + to_string_with_precision(this->state.b.accel[1], 8) + " " + + to_string_with_precision(this->state.b.accel[2], 8) + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Gyro Bias [xyz] :: " + to_string_with_precision(this->state.b.gyro[0], 8) + " " + + to_string_with_precision(this->state.b.gyro[1], 8) + " " + + to_string_with_precision(this->state.b.gyro[2], 8) + << "|" << std::endl; + + std::cout << "| |" << std::endl; + + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Distance Traveled :: " + to_string_with_precision(length_traversed, 4) + " meters" + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Distance to Origin :: " + + to_string_with_precision( sqrt(pow(this->state.p[0]-this->origin[0],2) + + pow(this->state.p[1]-this->origin[1],2) + + pow(this->state.p[2]-this->origin[2],2)), 4) + " meters" + << "|" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "Registration :: keyframes: " + std::to_string(this->keyframes.size()) + ", " + + "deskewed points: " + std::to_string(this->deskew_size) + << "|" << std::endl; + std::cout << "| |" << std::endl; + + std::cout << std::right << std::setprecision(2) << std::fixed; + std::cout << "| Computation Time :: " + << std::setfill(' ') << std::setw(6) << this->comp_times.back()*1000. << " ms // Avg: " + << std::setw(6) << avg_comp_time*1000. << " / Max: " + << std::setw(6) << *std::max_element(this->comp_times.begin(), this->comp_times.end())*1000. + << " |" << std::endl; + std::cout << "| Cores Utilized :: " + << std::setfill(' ') << std::setw(6) << (cpu_percent/100.) * this->numProcessors << " cores // Avg: " + << std::setw(6) << (avg_cpu_usage/100.) * this->numProcessors << " / Max: " + << std::setw(6) << (*std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) / 100.) + * this->numProcessors + << " |" << std::endl; + std::cout << "| CPU Load :: " + << std::setfill(' ') << std::setw(6) << cpu_percent << " % // Avg: " + << std::setw(6) << avg_cpu_usage << " / Max: " + << std::setw(6) << *std::max_element(this->cpu_percents.begin(), this->cpu_percents.end()) + << " |" << std::endl; + std::cout << "| " << std::left << std::setfill(' ') << std::setw(66) + << "RAM Allocation :: " + to_string_with_precision(resident_set/1000., 2) + " MB" + << "|" << std::endl; + + std::cout << "+-------------------------------------------------------------------+" << std::endl; + +} diff --git a/src/dlio/odom_node.cc b/src/dlio/odom_node.cc new file mode 100644 index 00000000..74014984 --- /dev/null +++ b/src/dlio/odom_node.cc @@ -0,0 +1,28 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +#include "dlio/odom.h" + +int main(int argc, char** argv) { + + ros::init(argc, argv, "dlio_odom_node"); + ros::NodeHandle nh("~"); + + dlio::OdomNode node(nh); + ros::AsyncSpinner spinner(0); + spinner.start(); + node.start(); + ros::waitForShutdown(); + + return 0; + +} diff --git a/src/nano_gicp/lsq_registration.cc b/src/nano_gicp/lsq_registration.cc new file mode 100644 index 00000000..271cb03f --- /dev/null +++ b/src/nano_gicp/lsq_registration.cc @@ -0,0 +1,231 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * BSD 3-Clause License + * + * Copyright (c) 2020, SMRT-AIST + * 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. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. + *************************************************************************/ + +#include "dlio/dlio.h" +#include "nano_gicp/lsq_registration.h" + +template class nano_gicp::LsqRegistration; + +namespace nano_gicp { + +template +LsqRegistration::LsqRegistration() { + this->reg_name_ = "LsqRegistration"; + max_iterations_ = 64; + rotation_epsilon_ = 2e-3; + transformation_epsilon_ = 5e-4; + + lsq_optimizer_type_ = LSQ_OPTIMIZER_TYPE::LevenbergMarquardt; + lm_debug_print_ = false; + lm_max_iterations_ = 10; + lm_init_lambda_factor_ = 1e-9; + lm_lambda_ = -1.0; + + final_hessian_.setIdentity(); + final_error_ = 0.; +} + +template +LsqRegistration::~LsqRegistration() {} + +template +void LsqRegistration::setRotationEpsilon(double eps) { + rotation_epsilon_ = eps; +} + +template +void LsqRegistration::setTransformationEpsilon(double eps) { + transformation_epsilon_ = eps; +} + +template +void LsqRegistration::setMaximumIterations(int iter) { + max_iterations_ = iter; +} + +template +void LsqRegistration::setInitialLambdaFactor(double init_lambda_factor) { + lm_init_lambda_factor_ = init_lambda_factor; +} + +template +void LsqRegistration::setDebugPrint(bool lm_debug_print) { + lm_debug_print_ = lm_debug_print; +} + +template +const Eigen::Matrix& LsqRegistration::getFinalHessian() const { + return final_hessian_; +} + +template +double LsqRegistration::getFinalError() const { + return final_error_; +} + +template +void LsqRegistration::computeTransformation(PointCloudSource& output, const Matrix4& guess) { + Eigen::Isometry3d x0 = Eigen::Isometry3d(guess.template cast()); + + lm_lambda_ = -1.0; + converged_ = false; + + if (lm_debug_print_) { + std::cout << "********************************************" << std::endl; + std::cout << "***************** optimize *****************" << std::endl; + std::cout << "********************************************" << std::endl; + } + + for (int i = 0; i < max_iterations_ && !converged_; i++) { + nr_iterations_ = i; + + Eigen::Isometry3d delta; + if (!step_optimize(x0, delta)) { + std::cerr << "lm not converged!!" << std::endl; + break; + } + + converged_ = is_converged(delta); + } + + final_transformation_ = x0.cast().matrix(); + pcl::transformPointCloud(*input_, output, final_transformation_); +} + +template +bool LsqRegistration::is_converged(const Eigen::Isometry3d& delta) const { + double accum = 0.0; + Eigen::Matrix3d R = delta.linear() - Eigen::Matrix3d::Identity(); + Eigen::Vector3d t = delta.translation(); + + Eigen::Matrix3d r_delta = 1.0 / rotation_epsilon_ * R.array().abs(); + Eigen::Vector3d t_delta = 1.0 / transformation_epsilon_ * t.array().abs(); + + return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1; +} + +template +bool LsqRegistration::step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { + switch (lsq_optimizer_type_) { + case LSQ_OPTIMIZER_TYPE::LevenbergMarquardt: + return step_lm(x0, delta); + case LSQ_OPTIMIZER_TYPE::GaussNewton: + return step_gn(x0, delta); + } + + return step_lm(x0, delta); +} + +template +bool LsqRegistration::step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { + Eigen::Matrix H; + Eigen::Matrix b; + double y0 = linearize(x0, &H, &b); + + Eigen::LDLT> solver(H); + Eigen::Matrix d = solver.solve(-b); + + delta.setIdentity(); + delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); + delta.translation() = d.tail<3>(); + + x0 = delta * x0; + final_hessian_ = H; + final_error_ = y0; + + return true; +} + +template +bool LsqRegistration::step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { + Eigen::Matrix H; + Eigen::Matrix b; + double y0 = linearize(x0, &H, &b); + + if (lm_lambda_ < 0.0) { + lm_lambda_ = lm_init_lambda_factor_ * H.diagonal().array().abs().maxCoeff(); + } + + double nu = 2.0; + for (int i = 0; i < lm_max_iterations_; i++) { + Eigen::LDLT> solver(H + lm_lambda_ * Eigen::Matrix::Identity()); + Eigen::Matrix d = solver.solve(-b); + + delta.setIdentity(); + delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); + delta.translation() = d.tail<3>(); + + Eigen::Isometry3d xi = delta * x0; + double yi = compute_error(xi); + double rho = (y0 - yi) / (d.dot(lm_lambda_ * d - b)); + + if (lm_debug_print_) { + if (i == 0) { + std::cout << boost::format("--- LM optimization ---\n%5s %15s %15s %15s %15s %15s %5s\n") % "i" % "y0" % "yi" % "rho" % "lambda" % "|delta|" % "dec"; + } + char dec = rho > 0.0 ? 'x' : ' '; + std::cout << boost::format("%5d %15g %15g %15g %15g %15g %5c") % i % y0 % yi % rho % lm_lambda_ % d.norm() % dec << std::endl; + } + + if (rho < 0) { + if (is_converged(delta)) { + return true; + } + + lm_lambda_ = nu * lm_lambda_; + nu = 2 * nu; + continue; + } + + x0 = xi; + lm_lambda_ = lm_lambda_ * std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3)); + final_hessian_ = H; + final_error_ = yi; + return true; + } + + return false; +} + +} // namespace nano_gicp diff --git a/src/nano_gicp/nano_gicp.cc b/src/nano_gicp/nano_gicp.cc new file mode 100644 index 00000000..906e3c33 --- /dev/null +++ b/src/nano_gicp/nano_gicp.cc @@ -0,0 +1,394 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * BSD 3-Clause License + * + * Copyright (c) 2020, SMRT-AIST + * 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. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. + *************************************************************************/ + +#include "dlio/dlio.h" +#include "nano_gicp/nano_gicp.h" + +template class nano_gicp::NanoGICP; + +namespace nano_gicp { + +template +NanoGICP::NanoGICP() { +#ifdef _OPENMP + num_threads_ = omp_get_max_threads(); +#else + num_threads_ = 1; +#endif + + k_correspondences_ = 20; + reg_name_ = "NanoGICP"; + corr_dist_threshold_ = std::numeric_limits::max(); + + regularization_method_ = RegularizationMethod::PLANE; +} + +template +NanoGICP::~NanoGICP() {} + +template +void NanoGICP::setNumThreads(int n) { + num_threads_ = n; + +#ifdef _OPENMP + if (n == 0) { + num_threads_ = omp_get_max_threads(); + } +#endif +} + +template +void NanoGICP::setCorrespondenceRandomness(int k) { + k_correspondences_ = k; +} + +template +void NanoGICP::setMaxCorrespondenceDistance(double corr) { + corr_dist_threshold_ = corr; +} + +template +void NanoGICP::setRegularizationMethod(RegularizationMethod method) { + regularization_method_ = method; +} + +template +void NanoGICP::swapSourceAndTarget() { + input_.swap(target_); + source_kdtree_.swap(target_kdtree_); + source_covs_.swap(target_covs_); + + correspondences_.clear(); + sq_distances_.clear(); +} + +template +void NanoGICP::clearSource() { + input_.reset(); + source_covs_.reset(); +} + +template +void NanoGICP::clearTarget() { + target_.reset(); + target_covs_.reset(); +} + +template +void NanoGICP::registerInputSource(const PointCloudSourceConstPtr& cloud) { + if (input_ == cloud) { + return; + } + pcl::Registration::setInputSource(cloud); +} + +template +void NanoGICP::registerInputTarget(const PointCloudTargetConstPtr& cloud) { + if (target_ == cloud) { + return; + } + pcl::Registration::setInputTarget(cloud); +} + +template +void NanoGICP::setInputSource(const PointCloudSourceConstPtr& cloud) { + if (input_ == cloud) { + return; + } + + pcl::Registration::setInputSource(cloud); + + std::shared_ptr> source_kdtree = std::make_shared>(); + source_kdtree->setInputCloud(cloud); + source_kdtree_ = source_kdtree; + + source_covs_.reset(); +} + +template +void NanoGICP::setInputTarget(const PointCloudTargetConstPtr& cloud) { + if (target_ == cloud) { + return; + } + pcl::Registration::setInputTarget(cloud); + + std::shared_ptr> target_kdtree = std::make_shared>(); + target_kdtree->setInputCloud(cloud); + target_kdtree_ = target_kdtree; + + target_covs_.reset(); +} + +template +void NanoGICP::setSourceCovariances(const std::shared_ptr& covs) { + source_covs_ = covs; +} + +template +void NanoGICP::setTargetCovariances(const std::shared_ptr& covs) { + target_covs_ = covs; +} + +template +bool NanoGICP::calculateSourceCovariances() { + std::shared_ptr source_covs = std::make_shared(); + std::shared_ptr source_density = std::make_shared(); + bool ret = calculate_covariances(input_, *source_kdtree_, *source_covs, *source_density); + source_covs_ = source_covs; + source_density_ = *source_density; + return ret; +} + +template +bool NanoGICP::calculateTargetCovariances() { + std::shared_ptr target_covs = std::make_shared(); + std::shared_ptr target_density = std::make_shared(); + bool ret = calculate_covariances(target_, *target_kdtree_, *target_covs, *target_density); + target_covs_ = target_covs; + target_density_ = *target_density; + return ret; +} + +template +void NanoGICP::computeTransformation(PointCloudSource& output, const Matrix4& guess) { + if (source_covs_ == nullptr || source_covs_->size() != input_->size()) { + calculateSourceCovariances(); + } + if (target_covs_ == nullptr || target_covs_->size() != target_->size()) { + calculateTargetCovariances(); + } + + LsqRegistration::computeTransformation(output, guess); +} + +template +void NanoGICP::update_correspondences(const Eigen::Isometry3d& trans) { + assert(source_covs_ != nullptr && source_covs_->size() == input_->size()); + assert(target_covs_ != nullptr && target_covs_->size() == target_->size()); + + Eigen::Isometry3f trans_f = trans.cast(); + + correspondences_.resize(input_->size()); + sq_distances_.resize(input_->size()); + mahalanobis_.resize(input_->size()); + + std::vector k_indices(1); + std::vector k_sq_dists(1); + +#pragma omp parallel for num_threads(num_threads_) firstprivate(k_indices, k_sq_dists) schedule(guided, 8) + for (int i = 0; i < input_->size(); i++) { + PointTarget pt; + pt.getVector4fMap() = trans_f * input_->at(i).getVector4fMap(); + + target_kdtree_->nearestKSearch(pt, 1, k_indices, k_sq_dists); + + sq_distances_[i] = k_sq_dists[0]; + correspondences_[i] = k_sq_dists[0] < corr_dist_threshold_ * corr_dist_threshold_ ? k_indices[0] : -1; + + if (correspondences_[i] < 0) { + continue; + } + + const int target_index = correspondences_[i]; + const auto& cov_A = (*source_covs_)[i]; + const auto& cov_B = (*target_covs_)[target_index]; + + Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose(); + RCR(3, 3) = 1.0; + + mahalanobis_[i] = RCR.inverse(); + mahalanobis_[i](3, 3) = 0.0f; + } + + num_correspondences = std::count_if(correspondences_.begin(), correspondences_.end(), [](int c){return c > 0;}); +} + +template +double NanoGICP::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { + update_correspondences(trans); + + double sum_errors = 0.0; + std::vector, Eigen::aligned_allocator>> Hs(num_threads_); + std::vector, Eigen::aligned_allocator>> bs(num_threads_); + for (int i = 0; i < num_threads_; i++) { + Hs[i].setZero(); + bs[i].setZero(); + } + +#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8) + for (int i = 0; i < input_->size(); i++) { + int target_index = correspondences_[i]; + if (target_index < 0) { + continue; + } + + const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); + + const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); + + const Eigen::Vector4d transed_mean_A = trans * mean_A; + const Eigen::Vector4d error = mean_B - transed_mean_A; + + sum_errors += error.transpose() * mahalanobis_[i] * error; + + if (H == nullptr || b == nullptr) { + continue; + } + + Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); + dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); + dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); + + Eigen::Matrix jlossexp = dtdx0; + + Eigen::Matrix Hi = jlossexp.transpose() * mahalanobis_[i] * jlossexp; + Eigen::Matrix bi = jlossexp.transpose() * mahalanobis_[i] * error; + + Hs[omp_get_thread_num()] += Hi; + bs[omp_get_thread_num()] += bi; + } + + if (H && b) { + H->setZero(); + b->setZero(); + for (int i = 0; i < num_threads_; i++) { + (*H) += Hs[i]; + (*b) += bs[i]; + } + } + + return sum_errors; +} + +template +double NanoGICP::compute_error(const Eigen::Isometry3d& trans) { + double sum_errors = 0.0; + +#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8) + for (int i = 0; i < input_->size(); i++) { + int target_index = correspondences_[i]; + if (target_index < 0) { + continue; + } + + const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); + + const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); + + const Eigen::Vector4d transed_mean_A = trans * mean_A; + const Eigen::Vector4d error = mean_B - transed_mean_A; + + sum_errors += error.transpose() * mahalanobis_[i] * error; + } + + return sum_errors; +} + +template +template +bool NanoGICP::calculate_covariances( + const typename pcl::PointCloud::ConstPtr& cloud, + const nanoflann::KdTreeFLANN& kdtree, + CovarianceList& covariances, + float& density) { + + covariances.resize(cloud->size()); + float sum_k_sq_distances = 0.0; + +#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) reduction(+:sum_k_sq_distances) + for (int i = 0; i < cloud->size(); i++) { + std::vector k_indices; + std::vector k_sq_distances; + kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances); + + const int normalization = ((k_correspondences_ - 1) * (2 + k_correspondences_)) / 2; + sum_k_sq_distances += std::accumulate(k_sq_distances.begin()+1, k_sq_distances.end(), 0.0) / normalization; + + Eigen::Matrix neighbors(4, k_correspondences_); + for (int j = 0; j < k_indices.size(); j++) { + neighbors.col(j) = cloud->at(k_indices[j]).getVector4fMap().template cast(); + } + + neighbors.colwise() -= neighbors.rowwise().mean().eval(); + Eigen::Matrix4d cov = neighbors * neighbors.transpose() / k_correspondences_; + + if (regularization_method_ == RegularizationMethod::NONE) { + covariances[i] = cov; + } else if (regularization_method_ == RegularizationMethod::FROBENIUS) { + double lambda = 1e-3; + Eigen::Matrix3d C = cov.block<3, 3>(0, 0).cast() + lambda * Eigen::Matrix3d::Identity(); + Eigen::Matrix3d C_inv = C.inverse(); + covariances[i].setZero(); + covariances[i].template block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse(); + } else { + Eigen::JacobiSVD svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Vector3d values; + + switch (regularization_method_) { + default: + std::cerr << "here must not be reached" << std::endl; + abort(); + case RegularizationMethod::PLANE: + values = Eigen::Vector3d(1, 1, 1e-3); + break; + case RegularizationMethod::MIN_EIG: + values = svd.singularValues().array().max(1e-3); + break; + case RegularizationMethod::NORMALIZED_MIN_EIG: + values = svd.singularValues() / svd.singularValues().maxCoeff(); + values = values.array().max(1e-3); + break; + } + + covariances[i].setZero(); + covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); + } + } + + density = sum_k_sq_distances / cloud->size(); + + return true; +} + +} // namespace nano_gicp diff --git a/src/nano_gicp/nanoflann.cc b/src/nano_gicp/nanoflann.cc new file mode 100644 index 00000000..bf25a4a5 --- /dev/null +++ b/src/nano_gicp/nanoflann.cc @@ -0,0 +1,48 @@ +/*********************************************************** + * * + * Copyright (c) * + * * + * The Verifiable & Control-Theoretic Robotics (VECTR) Lab * + * University of California, Los Angeles * + * * + * Authors: Kenny J. Chen, Ryan Nemiroff, Brett T. Lopez * + * Contact: {kennyjchen, ryguyn, btlopez}@ucla.edu * + * * + ***********************************************************/ + +/*********************************************************************** + * 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-2021 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. + *************************************************************************/ + +#include "dlio/dlio.h" +#include "nano_gicp/nanoflann_adaptor.h" + +template class nanoflann::KdTreeFLANN; diff --git a/srv/save_pcd.srv b/srv/save_pcd.srv new file mode 100644 index 00000000..7225fc15 --- /dev/null +++ b/srv/save_pcd.srv @@ -0,0 +1,4 @@ +float32 leaf_size +string save_path +--- +bool success