├── src ├── lidar_localization │ ├── srv │ │ └── saveMap.srv │ ├── cmake │ │ ├── eigen3.cmake │ │ ├── ceres.cmake │ │ ├── sophus.cmake │ │ ├── geographic.cmake │ │ ├── PCL.cmake │ │ ├── YAML.cmake │ │ ├── global_defination.cmake │ │ └── glog.cmake │ ├── include │ │ └── lidar_localization │ │ │ ├── global_defination │ │ │ └── global_defination.h.in │ │ │ ├── tools │ │ │ └── file_manager.hpp │ │ │ ├── models │ │ │ ├── cloud_filter │ │ │ │ ├── cloud_filter_interface.hpp │ │ │ │ └── voxel_filter.hpp │ │ │ ├── registration │ │ │ │ ├── registration_interface.hpp │ │ │ │ ├── ndt_registration.hpp │ │ │ │ ├── icp_registration.hpp │ │ │ │ └── icp_svd_registration.hpp │ │ │ └── loam │ │ │ │ └── aloam_factor.hpp │ │ │ ├── sensor_data │ │ │ ├── cloud_data.hpp │ │ │ ├── pose_data.hpp │ │ │ ├── velocity_data.hpp │ │ │ ├── gnss_data.hpp │ │ │ └── imu_data.hpp │ │ │ ├── utils │ │ │ ├── tic_toc.h │ │ │ └── common.h │ │ │ ├── subscriber │ │ │ ├── imu_subscriber.hpp │ │ │ ├── gnss_subscriber.hpp │ │ │ ├── velocity_subscriber.hpp │ │ │ ├── odometry_subscriber.hpp │ │ │ └── cloud_subscriber.hpp │ │ │ ├── tf_listener │ │ │ └── tf_listener.hpp │ │ │ ├── publisher │ │ │ ├── odometry_publisher.hpp │ │ │ └── cloud_publisher.hpp │ │ │ └── evaluation │ │ │ └── evaluation_flow.hpp │ ├── config │ │ ├── dataset │ │ │ └── config.yaml │ │ └── front_end │ │ │ └── config.yaml │ ├── .gitignore │ ├── src │ │ ├── sensor_data │ │ │ ├── pose_data.cpp │ │ │ ├── velocity_data.cpp │ │ │ ├── gnss_data.cpp │ │ │ └── imu_data.cpp │ │ ├── tools │ │ │ └── file_manager.cpp │ │ ├── publisher │ │ │ ├── cloud_publisher.cpp │ │ │ └── odometry_publisher.cpp │ │ ├── evaluation_node.cpp │ │ ├── subscriber │ │ │ ├── cloud_subscriber.cpp │ │ │ ├── gnss_subscriber.cpp │ │ │ ├── velocity_subscriber.cpp │ │ │ ├── imu_subscriber.cpp │ │ │ └── odometry_subscriber.cpp │ │ ├── models │ │ │ ├── cloud_filter │ │ │ │ └── voxel_filter.cpp │ │ │ └── registration │ │ │ │ ├── ndt_registration.cpp │ │ │ │ ├── icp_registration.cpp │ │ │ │ └── icp_svd_registration.cpp │ │ ├── tf_listener │ │ │ └── tf_lisener.cpp │ │ ├── evaluation │ │ │ └── evaluation_flow.cpp │ │ ├── aloam_scan_registration_node.cpp │ │ ├── aloam_laser_odometry_node.cpp │ │ └── aloam_mapping_node.cpp │ ├── launch │ │ └── front_end.launch │ ├── package.xml │ ├── CMakeLists.txt │ └── rviz │ │ └── front_end.rviz └── CMakeLists.txt ├── doc ├── traj.png ├── 公式推导.pdf ├── ape_map.png ├── ape_raw.png ├── rpe_map.png ├── rpe_raw.png ├── irpy_view.png ├── xyz_view.png └── 公式与代码对应关系.png ├── README.md └── CMakeLists.txt /src/lidar_localization/srv/saveMap.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | bool succeed -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /doc/traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/traj.png -------------------------------------------------------------------------------- /doc/公式推导.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/公式推导.pdf -------------------------------------------------------------------------------- /doc/ape_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/ape_map.png -------------------------------------------------------------------------------- /doc/ape_raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/ape_raw.png -------------------------------------------------------------------------------- /doc/rpe_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/rpe_map.png -------------------------------------------------------------------------------- /doc/rpe_raw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/rpe_raw.png -------------------------------------------------------------------------------- /doc/irpy_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/irpy_view.png -------------------------------------------------------------------------------- /doc/xyz_view.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/xyz_view.png -------------------------------------------------------------------------------- /doc/公式与代码对应关系.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cuifeng1993/SensorFusionChapter3/HEAD/doc/公式与代码对应关系.png -------------------------------------------------------------------------------- /src/lidar_localization/cmake/eigen3.cmake: -------------------------------------------------------------------------------- 1 | find_package(Eigen3 REQUIRED) 2 | 3 | include_directories(${EIGEN3_INCLUDE_DIRS}) -------------------------------------------------------------------------------- /src/lidar_localization/cmake/ceres.cmake: -------------------------------------------------------------------------------- 1 | find_package (Ceres REQUIRED) 2 | 3 | include_directories(${CERES_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${CERES_LIBRARIES}) -------------------------------------------------------------------------------- /src/lidar_localization/cmake/sophus.cmake: -------------------------------------------------------------------------------- 1 | find_package (Sophus REQUIRED) 2 | 3 | include_directories(${Sophus_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${Sophus_LIBRARIES}) -------------------------------------------------------------------------------- /src/lidar_localization/cmake/geographic.cmake: -------------------------------------------------------------------------------- 1 | find_package (GeographicLib REQUIRED) 2 | 3 | include_directories(${GeographicLib_INCLUDE_DIRS}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${GeographicLib_LIBRARIES}) -------------------------------------------------------------------------------- /src/lidar_localization/cmake/PCL.cmake: -------------------------------------------------------------------------------- 1 | find_package(PCL REQUIRED) 2 | list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") 3 | 4 | include_directories(${PCL_INCLUDE_DIRS}) 5 | list(APPEND ALL_TARGET_LIBRARIES ${PCL_LIBRARIES}) -------------------------------------------------------------------------------- /src/lidar_localization/cmake/YAML.cmake: -------------------------------------------------------------------------------- 1 | find_package(PkgConfig REQUIRED) 2 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 3 | include_directories(${YAML_CPP_INCLUDEDIR}) 4 | list(APPEND ALL_TARGET_LIBRARIES ${YAML_CPP_LIBRARIES}) -------------------------------------------------------------------------------- /src/lidar_localization/cmake/global_defination.cmake: -------------------------------------------------------------------------------- 1 | set(WORK_SPACE_PATH ${PROJECT_SOURCE_DIR}) 2 | configure_file ( 3 | ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h.in 4 | ${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h) 5 | include_directories(${PROJECT_BINARY_DIR}/include) -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/global_defination/global_defination.h.in: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_GLOBAL_DEFINATION_H_IN_ 7 | #define LIDAR_LOCALIZATION_GLOBAL_DEFINATION_H_IN_ 8 | 9 | #include 10 | 11 | namespace lidar_localization { 12 | const std::string WORK_SPACE_PATH = "@WORK_SPACE_PATH@"; 13 | } 14 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/config/dataset/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # ROS bag dataset config 3 | # 4 | measurements: 5 | lidar: 6 | topic_name: /kitti/velo/pointcloud 7 | frame_id: velo_link 8 | queue_size: 100000 9 | imu: 10 | topic_name: /kitti/oxts/imu 11 | frame_id: imu_link 12 | queue_size: 1000000 13 | gnss: 14 | topic_name: /kitti/oxts/gps/fix 15 | frame_id: imu_link 16 | queue_size: 1000000 17 | 18 | -------------------------------------------------------------------------------- /src/lidar_localization/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | 3 | *.INFO 4 | *.INFO.* 5 | 6 | # Prerequisites 7 | *.d 8 | 9 | # Compiled Object files 10 | *.slo 11 | *.lo 12 | *.o 13 | *.obj 14 | 15 | # Precompiled Headers 16 | *.gch 17 | *.pch 18 | 19 | # Compiled Dynamic libraries 20 | *.so 21 | *.dylib 22 | *.dll 23 | 24 | # Fortran module files 25 | *.mod 26 | *.smod 27 | 28 | # Compiled Static libraries 29 | *.lai 30 | *.la 31 | *.a 32 | *.lib 33 | 34 | # Executables 35 | *.exe 36 | *.out 37 | *.app 38 | -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/tools/file_manager.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 读写文件管理 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-24 19:22:53 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_TOOLS_FILE_MANAGER_HPP_ 7 | #define LIDAR_LOCALIZATION_TOOLS_FILE_MANAGER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace lidar_localization { 14 | class FileManager{ 15 | public: 16 | static bool CreateFile(std::ofstream& ofs, std::string file_path); 17 | static bool CreateDirectory(std::string directory_path); 18 | }; 19 | } 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/cloud_filter/cloud_filter_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云滤波模块的接口 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:29:50 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_CLOUD_FILTER_INTERFACE_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/sensor_data/cloud_data.hpp" 11 | 12 | namespace lidar_localization { 13 | class CloudFilterInterface { 14 | public: 15 | virtual ~CloudFilterInterface() = default; 16 | 17 | virtual bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) = 0; 18 | }; 19 | } 20 | 21 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/sensor_data/cloud_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:17:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | namespace lidar_localization { 13 | class CloudData { 14 | public: 15 | using POINT = pcl::PointXYZ; 16 | using CLOUD = pcl::PointCloud; 17 | using CLOUD_PTR = CLOUD::Ptr; 18 | 19 | public: 20 | CloudData() 21 | :cloud_ptr(new CLOUD()) { 22 | } 23 | 24 | public: 25 | double time = 0.0; 26 | CLOUD_PTR cloud_ptr; 27 | }; 28 | } 29 | 30 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/utils/tic_toc.h: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | class TicToc 11 | { 12 | public: 13 | TicToc() 14 | { 15 | tic(); 16 | } 17 | 18 | void tic() 19 | { 20 | start = std::chrono::system_clock::now(); 21 | } 22 | 23 | double toc() 24 | { 25 | end = std::chrono::system_clock::now(); 26 | std::chrono::duration elapsed_seconds = end - start; 27 | return elapsed_seconds.count() * 1000; 28 | } 29 | 30 | private: 31 | std::chrono::time_point start, end; 32 | }; 33 | -------------------------------------------------------------------------------- /src/lidar_localization/src/sensor_data/pose_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-28 18:50:16 5 | */ 6 | #include "lidar_localization/sensor_data/pose_data.hpp" 7 | 8 | namespace lidar_localization { 9 | 10 | Eigen::Quaternionf PoseData::GetQuaternion() { 11 | Eigen::Quaternionf q; 12 | q = pose.block<3,3>(0,0); 13 | return q; 14 | } 15 | 16 | void PoseData::GetVelocityData(VelocityData &velocity_data) const { 17 | velocity_data.time = time; 18 | 19 | velocity_data.linear_velocity.x = vel.v.x(); 20 | velocity_data.linear_velocity.y = vel.v.y(); 21 | velocity_data.linear_velocity.z = vel.v.z(); 22 | 23 | velocity_data.angular_velocity.x = vel.w.x(); 24 | velocity_data.angular_velocity.y = vel.w.y(); 25 | velocity_data.angular_velocity.z = vel.w.z(); 26 | } 27 | 28 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/sensor_data/pose_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 存放处理后的IMU姿态以及GNSS位置 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-27 23:10:56 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_POSE_DATA_HPP_ 8 | 9 | #include 10 | 11 | #include "lidar_localization/sensor_data/velocity_data.hpp" 12 | 13 | namespace lidar_localization { 14 | 15 | class PoseData { 16 | public: 17 | double time = 0.0; 18 | Eigen::Matrix4f pose = Eigen::Matrix4f::Identity(); 19 | 20 | struct { 21 | Eigen::Vector3f v = Eigen::Vector3f::Zero(); 22 | Eigen::Vector3f w = Eigen::Vector3f::Zero(); 23 | } vel; 24 | 25 | public: 26 | Eigen::Quaternionf GetQuaternion(); 27 | 28 | void GetVelocityData(VelocityData &velocity_data) const; 29 | }; 30 | 31 | } // namespace lidar_localization 32 | 33 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 深蓝学院《多传感器融合定位》第二期作业 2 | 3 | 深蓝学院, 多传感器融合定位与建图, 基于线面特征的激光里程计，第3章Lidar Odometry using LOAM代码框架. 4 | 5 | --- 6 | 7 | ## Overview 8 | 9 | 本作业在课程所给代码框架下，按照作业要求改写了F-LOAM中与残差雅可比矩阵的解析实现相关的部分，实现基于线面特征的激光前端里程计算法. 10 | 11 | --- 12 | 13 | ## homework 14 | 15 | ### 推导残差模型的雅可比 16 | 见文件夹[点击链接进入](doc/)中的PDF文件《公式推导》 17 | 18 | ### 公式与代码对应关系 19 | 由于F-LOAM代码中的面特征公式与作业要求相同，所以只需要改写线特征公式相关的代码，见图公式与代码对应关系 20 | ### 给出新模型基于evo的精度评测结果 21 | 见评测结果图: 22 | 23 | ape_map 24 | 25 | ape_raw 26 | 27 | rpy_view 28 | 29 | ape_map 30 | 31 | ape_raw 32 | 33 | trajectory 34 | 35 | axyz_view 36 | -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/subscriber/imu_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_IMU_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include "sensor_msgs/Imu.h" 12 | #include "lidar_localization/sensor_data/imu_data.hpp" 13 | 14 | namespace lidar_localization { 15 | class IMUSubscriber { 16 | public: 17 | IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 18 | IMUSubscriber() = default; 19 | void ParseData(std::deque& deque_imu_data); 20 | 21 | private: 22 | void msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr); 23 | 24 | private: 25 | ros::NodeHandle nh_; 26 | ros::Subscriber subscriber_; 27 | 28 | std::deque new_imu_data_; 29 | }; 30 | } 31 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/sensor_data/velocity_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: velocity 数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:27:40 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_VELOCITY_DATA_HPP_ 8 | 9 | #include 10 | #include 11 | 12 | namespace lidar_localization { 13 | class VelocityData { 14 | public: 15 | struct LinearVelocity { 16 | double x = 0.0; 17 | double y = 0.0; 18 | double z = 0.0; 19 | }; 20 | 21 | struct AngularVelocity { 22 | double x = 0.0; 23 | double y = 0.0; 24 | double z = 0.0; 25 | }; 26 | 27 | double time = 0.0; 28 | LinearVelocity linear_velocity; 29 | AngularVelocity angular_velocity; 30 | 31 | public: 32 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 33 | }; 34 | } 35 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/subscriber/gnss_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-03-31 12:58:10 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include "sensor_msgs/NavSatFix.h" 12 | #include "lidar_localization/sensor_data/gnss_data.hpp" 13 | 14 | namespace lidar_localization { 15 | class GNSSSubscriber { 16 | public: 17 | GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 18 | GNSSSubscriber() = default; 19 | void ParseData(std::deque& deque_gnss_data); 20 | 21 | private: 22 | void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr); 23 | 24 | private: 25 | ros::NodeHandle nh_; 26 | ros::Subscriber subscriber_; 27 | 28 | std::deque new_gnss_data_; 29 | }; 30 | } 31 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/registration/registration_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 点云匹配模块的基类 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-08 21:25:11 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_INTERFACE_HPP_ 8 | 9 | #include 10 | #include 11 | #include "lidar_localization/sensor_data/cloud_data.hpp" 12 | 13 | namespace lidar_localization { 14 | class RegistrationInterface { 15 | public: 16 | virtual ~RegistrationInterface() = default; 17 | 18 | virtual bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) = 0; 19 | virtual bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 20 | const Eigen::Matrix4f& predict_pose, 21 | CloudData::CLOUD_PTR& result_cloud_ptr, 22 | Eigen::Matrix4f& result_pose) = 0; 23 | }; 24 | } 25 | 26 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/tf_listener/tf_listener.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: tf监听模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 16:01:21 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_TF_LISTENER_HPP_ 7 | #define LIDAR_LOCALIZATION_TF_LISTENER_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace lidar_localization { 16 | class TFListener { 17 | public: 18 | TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id); 19 | TFListener() = default; 20 | 21 | bool LookupData(Eigen::Matrix4f& transform_matrix); 22 | 23 | private: 24 | bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix); 25 | 26 | private: 27 | ros::NodeHandle nh_; 28 | tf::TransformListener listener_; 29 | std::string base_frame_id_; 30 | std::string child_frame_id_; 31 | }; 32 | } 33 | 34 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/src/tools/file_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 一些文件读写的方法 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-24 20:09:32 5 | */ 6 | #include "lidar_localization/tools/file_manager.hpp" 7 | 8 | #include 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | bool FileManager::CreateFile(std::ofstream& ofs, std::string file_path) { 13 | ofs.open(file_path.c_str(), std::ios::app); 14 | if (!ofs) { 15 | LOG(WARNING) << "无法生成文件: " << file_path; 16 | return false; 17 | } 18 | 19 | return true; 20 | } 21 | 22 | bool FileManager::CreateDirectory(std::string directory_path) { 23 | if (!boost::filesystem::is_directory(directory_path)) { 24 | boost::filesystem::create_directory(directory_path); 25 | } 26 | if (!boost::filesystem::is_directory(directory_path)) { 27 | LOG(WARNING) << "无法建立文件夹: " << directory_path; 28 | return false; 29 | } 30 | return true; 31 | } 32 | } -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/cloud_filter/voxel_filter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel filter 模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:37:49 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_CLOUD_FILTER_VOXEL_FILTER_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp" 11 | 12 | namespace lidar_localization { 13 | class VoxelFilter: public CloudFilterInterface { 14 | public: 15 | VoxelFilter(const YAML::Node& node); 16 | VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z); 17 | 18 | bool Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) override; 19 | 20 | private: 21 | bool SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z); 22 | 23 | private: 24 | pcl::VoxelGrid voxel_filter_; 25 | }; 26 | } 27 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/publisher/odometry_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: odometry 信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:05:47 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 7 | #define LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace lidar_localization { 16 | class OdometryPublisher { 17 | public: 18 | OdometryPublisher(ros::NodeHandle& nh, 19 | std::string topic_name, 20 | std::string base_frame_id, 21 | std::string child_frame_id, 22 | int buff_size); 23 | OdometryPublisher() = default; 24 | 25 | void Publish(const Eigen::Matrix4f& transform_matrix); 26 | 27 | private: 28 | ros::NodeHandle nh_; 29 | ros::Publisher publisher_; 30 | nav_msgs::Odometry odometry_; 31 | }; 32 | } 33 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/publisher/cloud_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 在ros中发布点云 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 8 | #define LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "lidar_localization/sensor_data/cloud_data.hpp" 16 | 17 | namespace lidar_localization { 18 | class CloudPublisher { 19 | public: 20 | CloudPublisher(ros::NodeHandle& nh, 21 | std::string topic_name, 22 | size_t buff_size, 23 | std::string frame_id); 24 | CloudPublisher() = default; 25 | void Publish(CloudData::CLOUD_PTR cloud_ptr_input); 26 | 27 | private: 28 | ros::NodeHandle nh_; 29 | ros::Publisher publisher_; 30 | std::string frame_id_; 31 | }; 32 | } 33 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/subscriber/velocity_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅velocity数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_VELOCITY_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include "geometry_msgs/TwistStamped.h" 12 | #include "lidar_localization/sensor_data/velocity_data.hpp" 13 | 14 | namespace lidar_localization { 15 | class VelocitySubscriber { 16 | public: 17 | VelocitySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 18 | VelocitySubscriber() = default; 19 | void ParseData(std::deque& deque_velocity_data); 20 | 21 | private: 22 | void msg_callback(const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr); 23 | 24 | private: 25 | ros::NodeHandle nh_; 26 | ros::Subscriber subscriber_; 27 | 28 | std::deque new_velocity_data_; 29 | }; 30 | } 31 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/src/publisher/cloud_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 通过ros发布点云 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #include "lidar_localization/publisher/cloud_publisher.hpp" 8 | 9 | namespace lidar_localization { 10 | CloudPublisher::CloudPublisher(ros::NodeHandle& nh, 11 | std::string topic_name, 12 | size_t buff_size, 13 | std::string frame_id) 14 | :nh_(nh), frame_id_(frame_id) { 15 | publisher_ = nh_.advertise(topic_name, buff_size); 16 | } 17 | 18 | void CloudPublisher::Publish(CloudData::CLOUD_PTR cloud_ptr_input) { 19 | sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2()); 20 | pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output); 21 | cloud_ptr_output->header.stamp = ros::Time::now(); 22 | cloud_ptr_output->header.frame_id = frame_id_; 23 | publisher_.publish(*cloud_ptr_output); 24 | } 25 | 26 | } // namespace data_output -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/sensor_data/gnss_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:25:13 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | namespace lidar_localization { 14 | class GNSSData { 15 | public: 16 | double time = 0.0; 17 | double longitude = 0.0; 18 | double latitude = 0.0; 19 | double altitude = 0.0; 20 | double local_E = 0.0; 21 | double local_N = 0.0; 22 | double local_U = 0.0; 23 | int status = 0; 24 | int service = 0; 25 | 26 | private: 27 | static GeographicLib::LocalCartesian geo_converter; 28 | static bool origin_position_inited; 29 | 30 | public: 31 | void InitOriginPosition(); 32 | void UpdateXYZ(); 33 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 34 | }; 35 | } 36 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/config/front_end/config.yaml: -------------------------------------------------------------------------------- 1 | data_path: /workspace/assignments/01-lidar-odometry/src/lidar_localization # 数据存放路径 2 | 3 | # 匹配 4 | registration_method: NDT # 选择点云匹配方法，目前支持：ICP, NDT, ICP_SVD 5 | 6 | # 局部地图 7 | key_frame_distance: 2.0 # 关键帧距离 8 | local_frame_num: 20 9 | local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter 10 | 11 | # rviz显示 12 | display_filter: voxel_filter # rviz 实时显示点云时滤波方法，目前支持：voxel_filter 13 | 14 | # 当前帧 15 | frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter 16 | 17 | # 各配置选项对应参数 18 | ## 匹配相关参数 19 | ICP: 20 | max_corr_dist : 1.2 21 | trans_eps : 0.01 22 | euc_fitness_eps : 0.36 23 | max_iter : 30 24 | NDT: 25 | res : 1.0 26 | step_size : 0.1 27 | trans_eps : 0.01 28 | max_iter : 30 29 | ICP_SVD: 30 | max_corr_dist : 1.2 31 | trans_eps : 0.01 32 | euc_fitness_eps : 0.36 33 | max_iter : 10 34 | ## 滤波相关参数 35 | voxel_filter: 36 | local_map: 37 | leaf_size: [0.6, 0.6, 0.6] 38 | frame: 39 | leaf_size: [1.3, 1.3, 1.3] 40 | display: 41 | leaf_size: [0.5, 0.5, 0.5] -------------------------------------------------------------------------------- /src/lidar_localization/src/evaluation_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 前端里程计的node文件 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:56:27 5 | */ 6 | #include 7 | 8 | #include 9 | #include "glog/logging.h" 10 | 11 | #include 12 | #include "lidar_localization/global_defination/global_defination.h" 13 | #include "lidar_localization/evaluation/evaluation_flow.hpp" 14 | 15 | using namespace lidar_localization; 16 | 17 | int main(int argc, char *argv[]) { 18 | google::InitGoogleLogging(argv[0]); 19 | FLAGS_log_dir = WORK_SPACE_PATH + "/Log"; 20 | FLAGS_alsologtostderr = 1; 21 | 22 | ros::init(argc, argv, "evaluation_node"); 23 | ros::NodeHandle nh; 24 | 25 | // register front end processing workflow: 26 | std::shared_ptr evaluation_flow_ptr = std::make_shared(nh); 27 | 28 | // process rate: 10Hz 29 | ros::Rate rate(10); 30 | 31 | while (ros::ok()) { 32 | ros::spinOnce(); 33 | 34 | evaluation_flow_ptr->Run(); 35 | 36 | rate.sleep(); 37 | } 38 | 39 | return EXIT_SUCCESS; 40 | } -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/subscriber/odometry_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅odometry数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-08-19 19:22:17 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_ 7 | #define LIDAR_LOCALIZATION_SUBSCRIBER_ODOMETRY_SUBSCRIBER_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "lidar_localization/sensor_data/pose_data.hpp" 17 | 18 | namespace lidar_localization { 19 | 20 | class OdometrySubscriber { 21 | public: 22 | OdometrySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 23 | OdometrySubscriber() = default; 24 | void ParseData(std::deque& deque_pose_data); 25 | 26 | private: 27 | void msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr); 28 | 29 | private: 30 | ros::NodeHandle nh_; 31 | ros::Subscriber subscriber_; 32 | std::deque new_pose_data_; 33 | 34 | std::mutex buff_mutex_; 35 | }; 36 | 37 | } // namespace lidar_localization 38 | 39 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/subscriber/cloud_subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #ifndef LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 8 | #define LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_ 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include "lidar_localization/sensor_data/cloud_data.hpp" 19 | 20 | namespace lidar_localization { 21 | class CloudSubscriber { 22 | public: 23 | CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size); 24 | CloudSubscriber() = default; 25 | void ParseData(std::deque& deque_cloud_data); 26 | 27 | private: 28 | void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr); 29 | 30 | private: 31 | ros::NodeHandle nh_; 32 | ros::Subscriber subscriber_; 33 | 34 | std::deque new_cloud_data_; 35 | }; 36 | } 37 | 38 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/src/subscriber/cloud_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅激光点云信息，并解析数据 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-05 02:27:30 5 | */ 6 | 7 | #include "lidar_localization/subscriber/cloud_subscriber.hpp" 8 | 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 13 | :nh_(nh) { 14 | subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this); 15 | } 16 | 17 | void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) { 18 | CloudData cloud_data; 19 | cloud_data.time = cloud_msg_ptr->header.stamp.toSec(); 20 | pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr)); 21 | 22 | new_cloud_data_.push_back(cloud_data); 23 | } 24 | 25 | void CloudSubscriber::ParseData(std::deque& cloud_data_buff) { 26 | if (new_cloud_data_.size() > 0) { 27 | cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end()); 28 | new_cloud_data_.clear(); 29 | } 30 | } 31 | } // namespace data_input -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/registration/ndt_registration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: NDT 匹配模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-08 21:46:57 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/models/registration/registration_interface.hpp" 11 | 12 | namespace lidar_localization { 13 | class NDTRegistration: public RegistrationInterface { 14 | public: 15 | NDTRegistration(const YAML::Node& node); 16 | NDTRegistration(float res, float step_size, float trans_eps, int max_iter); 17 | 18 | bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override; 19 | bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 20 | const Eigen::Matrix4f& predict_pose, 21 | CloudData::CLOUD_PTR& result_cloud_ptr, 22 | Eigen::Matrix4f& result_pose) override; 23 | 24 | private: 25 | bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter); 26 | 27 | private: 28 | pcl::NormalDistributionsTransform::Ptr ndt_ptr_; 29 | }; 30 | } 31 | 32 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/src/subscriber/gnss_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-03-31 13:10:51 5 | */ 6 | #include "lidar_localization/subscriber/gnss_subscriber.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 12 | :nh_(nh) { 13 | subscriber_ = nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this); 14 | } 15 | 16 | void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) { 17 | GNSSData gnss_data; 18 | gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec(); 19 | gnss_data.latitude = nav_sat_fix_ptr->latitude; 20 | gnss_data.longitude = nav_sat_fix_ptr->longitude; 21 | gnss_data.altitude = nav_sat_fix_ptr->altitude; 22 | gnss_data.status = nav_sat_fix_ptr->status.status; 23 | gnss_data.service = nav_sat_fix_ptr->status.service; 24 | 25 | new_gnss_data_.push_back(gnss_data); 26 | } 27 | 28 | void GNSSSubscriber::ParseData(std::deque& gnss_data_buff) { 29 | if (new_gnss_data_.size() > 0) { 30 | gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end()); 31 | new_gnss_data_.clear(); 32 | } 33 | } 34 | } -------------------------------------------------------------------------------- /src/lidar_localization/launch/front_end.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/registration/icp_registration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: ICP 匹配模块 3 | * @Author: Ge Yao 4 | * @Date: 2020-10-24 21:46:57 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_ 8 | 9 | #include 10 | #include "lidar_localization/models/registration/registration_interface.hpp" 11 | 12 | namespace lidar_localization { 13 | class ICPRegistration: public RegistrationInterface { 14 | public: 15 | ICPRegistration(const YAML::Node& node); 16 | ICPRegistration( 17 | float max_corr_dist, 18 | float trans_eps, 19 | float euc_fitness_eps, 20 | int max_iter 21 | ); 22 | 23 | bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override; 24 | bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 25 | const Eigen::Matrix4f& predict_pose, 26 | CloudData::CLOUD_PTR& result_cloud_ptr, 27 | Eigen::Matrix4f& result_pose) override; 28 | 29 | private: 30 | bool SetRegistrationParam( 31 | float max_corr_dist, 32 | float trans_eps, 33 | float euc_fitness_eps, 34 | int max_iter 35 | ); 36 | 37 | private: 38 | pcl::IterativeClosestPoint::Ptr icp_ptr_; 39 | }; 40 | } 41 | 42 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/cmake/glog.cmake: -------------------------------------------------------------------------------- 1 | include(FindPackageHandleStandardArgs) 2 | 3 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 4 | 5 | if(WIN32) 6 | find_path(GLOG_INCLUDE_DIR glog/logging.h 7 | PATHS ${GLOG_ROOT_DIR}/src/windows) 8 | else() 9 | find_path(GLOG_INCLUDE_DIR glog/logging.h 10 | PATHS ${GLOG_ROOT_DIR}) 11 | endif() 12 | 13 | if(MSVC) 14 | find_library(GLOG_LIBRARY_RELEASE libglog_static 15 | PATHS ${GLOG_ROOT_DIR} 16 | PATH_SUFFIXES Release) 17 | 18 | find_library(GLOG_LIBRARY_DEBUG libglog_static 19 | PATHS ${GLOG_ROOT_DIR} 20 | PATH_SUFFIXES Debug) 21 | 22 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 23 | else() 24 | find_library(GLOG_LIBRARY glog 25 | PATHS ${GLOG_ROOT_DIR} 26 | PATH_SUFFIXES lib lib64) 27 | endif() 28 | 29 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 30 | 31 | if(GLOG_FOUND) 32 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 33 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 34 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 35 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 36 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 37 | endif() 38 | 39 | include_directories(${GLOG_INCLUDE_DIRS}) 40 | list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES}) 41 | -------------------------------------------------------------------------------- /src/lidar_localization/src/publisher/odometry_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 里程计信息发布 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 21:11:44 5 | */ 6 | #include "lidar_localization/publisher/odometry_publisher.hpp" 7 | 8 | namespace lidar_localization { 9 | OdometryPublisher::OdometryPublisher(ros::NodeHandle& nh, 10 | std::string topic_name, 11 | std::string base_frame_id, 12 | std::string child_frame_id, 13 | int buff_size) 14 | :nh_(nh) { 15 | 16 | publisher_ = nh_.advertise(topic_name, buff_size); 17 | odometry_.header.frame_id = base_frame_id; 18 | odometry_.child_frame_id = child_frame_id; 19 | } 20 | 21 | void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix) { 22 | odometry_.header.stamp = ros::Time::now(); 23 | 24 | //set the position 25 | odometry_.pose.pose.position.x = transform_matrix(0,3); 26 | odometry_.pose.pose.position.y = transform_matrix(1,3); 27 | odometry_.pose.pose.position.z = transform_matrix(2,3); 28 | 29 | Eigen::Quaternionf q; 30 | q = transform_matrix.block<3,3>(0,0); 31 | odometry_.pose.pose.orientation.x = q.x(); 32 | odometry_.pose.pose.orientation.y = q.y(); 33 | odometry_.pose.pose.orientation.z = q.z(); 34 | odometry_.pose.pose.orientation.w = q.w(); 35 | 36 | publisher_.publish(odometry_); 37 | } 38 | } -------------------------------------------------------------------------------- /src/lidar_localization/src/models/cloud_filter/voxel_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: voxel filter 模块实现 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-09 19:53:20 5 | */ 6 | #include "lidar_localization/models/cloud_filter/voxel_filter.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | 12 | VoxelFilter::VoxelFilter(const YAML::Node& node) { 13 | float leaf_size_x = node["leaf_size"][0].as(); 14 | float leaf_size_y = node["leaf_size"][1].as(); 15 | float leaf_size_z = node["leaf_size"][2].as(); 16 | 17 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 18 | } 19 | 20 | VoxelFilter::VoxelFilter(float leaf_size_x, float leaf_size_y, float leaf_size_z) { 21 | SetFilterParam(leaf_size_x, leaf_size_y, leaf_size_z); 22 | } 23 | 24 | bool VoxelFilter::SetFilterParam(float leaf_size_x, float leaf_size_y, float leaf_size_z) { 25 | voxel_filter_.setLeafSize(leaf_size_x, leaf_size_y, leaf_size_z); 26 | 27 | LOG(INFO) << "Voxel Filter params:" << std::endl 28 | << leaf_size_x << ", " 29 | << leaf_size_y << ", " 30 | << leaf_size_z 31 | << std::endl << std::endl; 32 | 33 | return true; 34 | } 35 | 36 | bool VoxelFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) { 37 | voxel_filter_.setInputCloud(input_cloud_ptr); 38 | voxel_filter_.filter(*filtered_cloud_ptr); 39 | 40 | return true; 41 | } 42 | } -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/sensor_data/imu_data.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2019-07-17 18:27:40 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 7 | #define LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace lidar_localization { 14 | class IMUData { 15 | public: 16 | struct LinearAcceleration { 17 | double x = 0.0; 18 | double y = 0.0; 19 | double z = 0.0; 20 | }; 21 | 22 | struct AngularVelocity { 23 | double x = 0.0; 24 | double y = 0.0; 25 | double z = 0.0; 26 | }; 27 | 28 | class Orientation { 29 | public: 30 | double x = 0.0; 31 | double y = 0.0; 32 | double z = 0.0; 33 | double w = 0.0; 34 | 35 | public: 36 | void Normlize() { 37 | double norm = sqrt(pow(x, 2.0) + pow(y, 2.0) + pow(z, 2.0) + pow(w, 2.0)); 38 | x /= norm; 39 | y /= norm; 40 | z /= norm; 41 | w /= norm; 42 | } 43 | }; 44 | 45 | double time = 0.0; 46 | LinearAcceleration linear_acceleration; 47 | AngularVelocity angular_velocity; 48 | Orientation orientation; 49 | 50 | public: 51 | // 把四元数转换成旋转矩阵送出去 52 | Eigen::Matrix3f GetOrientationMatrix(); 53 | static bool SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time); 54 | }; 55 | } 56 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/src/subscriber/velocity_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/velocity_subscriber.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization{ 11 | VelocitySubscriber::VelocitySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 12 | :nh_(nh) { 13 | subscriber_ = nh_.subscribe(topic_name, buff_size, &VelocitySubscriber::msg_callback, this); 14 | } 15 | 16 | void VelocitySubscriber::msg_callback(const geometry_msgs::TwistStampedConstPtr& twist_msg_ptr) { 17 | VelocityData velocity_data; 18 | velocity_data.time = twist_msg_ptr->header.stamp.toSec(); 19 | 20 | velocity_data.linear_velocity.x = twist_msg_ptr->twist.linear.x; 21 | velocity_data.linear_velocity.y = twist_msg_ptr->twist.linear.y; 22 | velocity_data.linear_velocity.z = twist_msg_ptr->twist.linear.z; 23 | 24 | velocity_data.angular_velocity.x = twist_msg_ptr->twist.angular.x; 25 | velocity_data.angular_velocity.y = twist_msg_ptr->twist.angular.y; 26 | velocity_data.angular_velocity.z = twist_msg_ptr->twist.angular.z; 27 | 28 | new_velocity_data_.push_back(velocity_data); 29 | } 30 | 31 | void VelocitySubscriber::ParseData(std::deque& velocity_data_buff) { 32 | if (new_velocity_data_.size() > 0) { 33 | velocity_data_buff.insert(velocity_data_buff.end(), new_velocity_data_.begin(), new_velocity_data_.end()); 34 | new_velocity_data_.clear(); 35 | } 36 | } 37 | } -------------------------------------------------------------------------------- /src/lidar_localization/src/tf_listener/tf_lisener.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: tf监听模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 16:10:31 5 | */ 6 | #include "lidar_localization/tf_listener/tf_listener.hpp" 7 | 8 | #include 9 | 10 | namespace lidar_localization { 11 | TFListener::TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id) 12 | :nh_(nh), base_frame_id_(base_frame_id), child_frame_id_(child_frame_id) { 13 | } 14 | 15 | bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) { 16 | try { 17 | tf::StampedTransform transform; 18 | listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform); 19 | TransformToMatrix(transform, transform_matrix); 20 | return true; 21 | } catch (tf::TransformException &ex) { 22 | return false; 23 | } 24 | } 25 | 26 | bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) { 27 | Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ()); 28 | 29 | double roll, pitch, yaw; 30 | tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll); 31 | Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX()); 32 | Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY()); 33 | Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ()); 34 | 35 | // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵 36 | transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix(); 37 | 38 | return true; 39 | } 40 | } -------------------------------------------------------------------------------- /src/lidar_localization/src/subscriber/imu_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅imu数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization{ 11 | IMUSubscriber::IMUSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 12 | :nh_(nh) { 13 | subscriber_ = nh_.subscribe(topic_name, buff_size, &IMUSubscriber::msg_callback, this); 14 | } 15 | 16 | void IMUSubscriber::msg_callback(const sensor_msgs::ImuConstPtr& imu_msg_ptr) { 17 | IMUData imu_data; 18 | imu_data.time = imu_msg_ptr->header.stamp.toSec(); 19 | 20 | imu_data.linear_acceleration.x = imu_msg_ptr->linear_acceleration.x; 21 | imu_data.linear_acceleration.y = imu_msg_ptr->linear_acceleration.y; 22 | imu_data.linear_acceleration.z = imu_msg_ptr->linear_acceleration.z; 23 | 24 | imu_data.angular_velocity.x = imu_msg_ptr->angular_velocity.x; 25 | imu_data.angular_velocity.y = imu_msg_ptr->angular_velocity.y; 26 | imu_data.angular_velocity.z = imu_msg_ptr->angular_velocity.z; 27 | 28 | imu_data.orientation.x = imu_msg_ptr->orientation.x; 29 | imu_data.orientation.y = imu_msg_ptr->orientation.y; 30 | imu_data.orientation.z = imu_msg_ptr->orientation.z; 31 | imu_data.orientation.w = imu_msg_ptr->orientation.w; 32 | 33 | new_imu_data_.push_back(imu_data); 34 | } 35 | 36 | void IMUSubscriber::ParseData(std::deque& imu_data_buff) { 37 | if (new_imu_data_.size() > 0) { 38 | imu_data_buff.insert(imu_data_buff.end(), new_imu_data_.begin(), new_imu_data_.end()); 39 | new_imu_data_.clear(); 40 | } 41 | } 42 | } -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/evaluation/evaluation_flow.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: evaluation facade 3 | * @Author: Ge Yao 4 | * @Date: 2021-01-30 22:38:22 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_EVALUATION_EVALUATION_FLOW_HPP_ 7 | #define LIDAR_LOCALIZATION_EVALUATION_EVALUATION_FLOW_HPP_ 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 16 | 17 | #include "lidar_localization/tf_listener/tf_listener.hpp" 18 | #include "lidar_localization/subscriber/imu_subscriber.hpp" 19 | #include "lidar_localization/subscriber/gnss_subscriber.hpp" 20 | 21 | #include "lidar_localization/publisher/odometry_publisher.hpp" 22 | 23 | namespace lidar_localization { 24 | 25 | class EvaluationFlow { 26 | public: 27 | EvaluationFlow(ros::NodeHandle& nh); 28 | 29 | bool Run(); 30 | bool SaveMap(); 31 | bool PublishGlobalMap(); 32 | 33 | private: 34 | bool InitSubscribers(ros::NodeHandle& nh, const YAML::Node& config_node); 35 | 36 | bool ReadData(); 37 | bool InitCalibration(); 38 | bool InitGNSS(); 39 | bool HasData(); 40 | bool ValidData(); 41 | bool UpdateLaserOdometry(); 42 | bool UpdateGNSSOdometry(); 43 | bool SaveTrajectory(); 44 | 45 | private: 46 | std::shared_ptr laser_odom_sub_ptr_; 47 | std::deque laser_odom_data_buff_; 48 | PoseData current_laser_odom_data_; 49 | 50 | std::shared_ptr lidar_to_imu_ptr_; 51 | Eigen::Matrix4f lidar_to_imu_ = Eigen::Matrix4f::Identity(); 52 | 53 | std::shared_ptr imu_sub_ptr_; 54 | std::deque imu_data_buff_; 55 | IMUData current_imu_data_; 56 | 57 | std::shared_ptr gnss_sub_ptr_; 58 | std::deque gnss_data_buff_; 59 | GNSSData current_gnss_data_; 60 | 61 | std::shared_ptr gnss_pub_ptr_; 62 | 63 | Eigen::Matrix4f laser_odometry_ = Eigen::Matrix4f::Identity(); 64 | Eigen::Matrix4f gnss_odometry_ = Eigen::Matrix4f::Identity(); 65 | }; 66 | 67 | } // namespace lidar_localization 68 | 69 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/registration/icp_svd_registration.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: ICP SVD 匹配模块 3 | * @Author: Ge Yao 4 | * @Date: 2020-10-24 21:46:57 5 | */ 6 | #ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_SVD_REGISTRATION_HPP_ 7 | #define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_SVD_REGISTRATION_HPP_ 8 | 9 | #include "lidar_localization/models/registration/registration_interface.hpp" 10 | 11 | #include 12 | 13 | namespace lidar_localization { 14 | class ICPSVDRegistration: public RegistrationInterface { 15 | public: 16 | ICPSVDRegistration(const YAML::Node& node); 17 | ICPSVDRegistration( 18 | float max_corr_dist, 19 | float trans_eps, 20 | float euc_fitness_eps, 21 | int max_iter 22 | ); 23 | 24 | bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override; 25 | bool ScanMatch( 26 | const CloudData::CLOUD_PTR& input_source, 27 | const Eigen::Matrix4f& predict_pose, 28 | CloudData::CLOUD_PTR& result_cloud_ptr, 29 | Eigen::Matrix4f& result_pose 30 | ) override; 31 | 32 | private: 33 | bool SetRegistrationParam( 34 | float max_corr_dist, 35 | float trans_eps, 36 | float euc_fitness_eps, 37 | int max_iter 38 | ); 39 | 40 | private: 41 | size_t GetCorrespondence( 42 | const CloudData::CLOUD_PTR &input_source, 43 | std::vector &xs, 44 | std::vector &ys 45 | ); 46 | 47 | void GetTransform( 48 | const std::vector &xs, 49 | const std::vector &ys, 50 | Eigen::Matrix4f &transformation_ 51 | ); 52 | 53 | bool IsSignificant( 54 | const Eigen::Matrix4f &transformation, 55 | const float trans_eps 56 | ); 57 | 58 | float max_corr_dist_; 59 | float trans_eps_; 60 | float euc_fitness_eps_; 61 | int max_iter_; 62 | 63 | CloudData::CLOUD_PTR input_target_; 64 | pcl::KdTreeFLANN::Ptr input_target_kdtree_; 65 | CloudData::CLOUD_PTR input_source_; 66 | 67 | Eigen::Matrix4f transformation_; 68 | }; 69 | } 70 | 71 | #endif -------------------------------------------------------------------------------- /src/lidar_localization/src/subscriber/odometry_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 订阅odometry数据 3 | * @Author: Ren Qian 4 | * @Date: 2019-06-14 16:44:18 5 | */ 6 | #include "lidar_localization/subscriber/odometry_subscriber.hpp" 7 | #include "glog/logging.h" 8 | 9 | namespace lidar_localization{ 10 | 11 | OdometrySubscriber::OdometrySubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 12 | :nh_(nh) { 13 | subscriber_ = nh_.subscribe(topic_name, buff_size, &OdometrySubscriber::msg_callback, this); 14 | } 15 | 16 | void OdometrySubscriber::msg_callback(const nav_msgs::OdometryConstPtr& odom_msg_ptr) { 17 | buff_mutex_.lock(); 18 | PoseData pose_data; 19 | pose_data.time = odom_msg_ptr->header.stamp.toSec(); 20 | 21 | // set the position: 22 | pose_data.pose(0,3) = odom_msg_ptr->pose.pose.position.x; 23 | pose_data.pose(1,3) = odom_msg_ptr->pose.pose.position.y; 24 | pose_data.pose(2,3) = odom_msg_ptr->pose.pose.position.z; 25 | 26 | // set the orientation: 27 | Eigen::Quaternionf q; 28 | q.x() = odom_msg_ptr->pose.pose.orientation.x; 29 | q.y() = odom_msg_ptr->pose.pose.orientation.y; 30 | q.z() = odom_msg_ptr->pose.pose.orientation.z; 31 | q.w() = odom_msg_ptr->pose.pose.orientation.w; 32 | pose_data.pose.block<3,3>(0,0) = q.matrix(); 33 | 34 | // set the linear velocity: 35 | pose_data.vel.v.x() = odom_msg_ptr->twist.twist.linear.x; 36 | pose_data.vel.v.y() = odom_msg_ptr->twist.twist.linear.y; 37 | pose_data.vel.v.z() = odom_msg_ptr->twist.twist.linear.z; 38 | 39 | // set the angular velocity: 40 | pose_data.vel.w.x() = odom_msg_ptr->twist.twist.angular.x; 41 | pose_data.vel.w.y() = odom_msg_ptr->twist.twist.angular.y; 42 | pose_data.vel.w.z() = odom_msg_ptr->twist.twist.angular.z; 43 | 44 | new_pose_data_.push_back(pose_data); 45 | 46 | buff_mutex_.unlock(); 47 | } 48 | 49 | void OdometrySubscriber::ParseData(std::deque& pose_data_buff) { 50 | buff_mutex_.lock(); 51 | if (new_pose_data_.size() > 0) { 52 | pose_data_buff.insert(pose_data_buff.end(), new_pose_data_.begin(), new_pose_data_.end()); 53 | new_pose_data_.clear(); 54 | } 55 | buff_mutex_.unlock(); 56 | } 57 | 58 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/lidar_localization/src/models/registration/ndt_registration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: NDT 匹配模块 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-08 21:46:45 5 | */ 6 | #include "lidar_localization/models/registration/ndt_registration.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | 12 | NDTRegistration::NDTRegistration(const YAML::Node& node) 13 | :ndt_ptr_(new pcl::NormalDistributionsTransform()) { 14 | 15 | float res = node["res"].as(); 16 | float step_size = node["step_size"].as(); 17 | float trans_eps = node["trans_eps"].as(); 18 | int max_iter = node["max_iter"].as(); 19 | 20 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 21 | } 22 | 23 | NDTRegistration::NDTRegistration(float res, float step_size, float trans_eps, int max_iter) 24 | :ndt_ptr_(new pcl::NormalDistributionsTransform()) { 25 | 26 | SetRegistrationParam(res, step_size, trans_eps, max_iter); 27 | } 28 | 29 | bool NDTRegistration::SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter) { 30 | ndt_ptr_->setResolution(res); 31 | ndt_ptr_->setStepSize(step_size); 32 | ndt_ptr_->setTransformationEpsilon(trans_eps); 33 | ndt_ptr_->setMaximumIterations(max_iter); 34 | 35 | LOG(INFO) << "NDT params:" << std::endl 36 | << "res: " << res << ", " 37 | << "step_size: " << step_size << ", " 38 | << "trans_eps: " << trans_eps << ", " 39 | << "max_iter: " << max_iter 40 | << std::endl << std::endl; 41 | 42 | return true; 43 | } 44 | 45 | bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) { 46 | ndt_ptr_->setInputTarget(input_target); 47 | 48 | return true; 49 | } 50 | 51 | bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 52 | const Eigen::Matrix4f& predict_pose, 53 | CloudData::CLOUD_PTR& result_cloud_ptr, 54 | Eigen::Matrix4f& result_pose) { 55 | ndt_ptr_->setInputSource(input_source); 56 | ndt_ptr_->align(*result_cloud_ptr, predict_pose); 57 | result_pose = ndt_ptr_->getFinalTransformation(); 58 | 59 | return true; 60 | } 61 | } -------------------------------------------------------------------------------- /src/lidar_localization/src/sensor_data/velocity_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: velocity data 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-23 22:20:41 5 | */ 6 | #include "lidar_localization/sensor_data/velocity_data.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | bool VelocityData::SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time) { 12 | // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置 13 | // 即找到与同步时间相邻的左右两个数据 14 | // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值 15 | while (UnsyncedData.size() >= 2) { 16 | if (UnsyncedData.front().time > sync_time) 17 | return false; 18 | if (UnsyncedData.at(1).time < sync_time) { 19 | UnsyncedData.pop_front(); 20 | continue; 21 | } 22 | if (sync_time - UnsyncedData.front().time > 0.2) { 23 | UnsyncedData.pop_front(); 24 | return false; 25 | } 26 | if (UnsyncedData.at(1).time - sync_time > 0.2) { 27 | return false; 28 | } 29 | break; 30 | } 31 | if (UnsyncedData.size() < 2) 32 | return false; 33 | 34 | VelocityData front_data = UnsyncedData.at(0); 35 | VelocityData back_data = UnsyncedData.at(1); 36 | VelocityData synced_data; 37 | 38 | double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 39 | double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time); 40 | synced_data.time = sync_time; 41 | synced_data.linear_velocity.x = front_data.linear_velocity.x * front_scale + back_data.linear_velocity.x * back_scale; 42 | synced_data.linear_velocity.y = front_data.linear_velocity.y * front_scale + back_data.linear_velocity.y * back_scale; 43 | synced_data.linear_velocity.z = front_data.linear_velocity.z * front_scale + back_data.linear_velocity.z * back_scale; 44 | synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale; 45 | synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale; 46 | synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale; 47 | 48 | SyncedData.push_back(synced_data); 49 | 50 | return true; 51 | } 52 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # toplevel CMakeLists.txt for a catkin workspace 2 | # catkin/cmake/toplevel.cmake 3 | 4 | cmake_minimum_required(VERSION 3.0.2) 5 | 6 | project(Project) 7 | 8 | set(CATKIN_TOPLEVEL TRUE) 9 | 10 | # search for catkin within the workspace 11 | set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") 12 | execute_process(COMMAND ${_cmd} 13 | RESULT_VARIABLE _res 14 | OUTPUT_VARIABLE _out 15 | ERROR_VARIABLE _err 16 | OUTPUT_STRIP_TRAILING_WHITESPACE 17 | ERROR_STRIP_TRAILING_WHITESPACE 18 | ) 19 | if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) 20 | # searching fot catkin resulted in an error 21 | string(REPLACE ";" " " _cmd_str "${_cmd}") 22 | message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") 23 | endif() 24 | 25 | # include catkin from workspace or via find_package() 26 | if(_res EQUAL 0) 27 | set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") 28 | # include all.cmake without add_subdirectory to let it operate in same scope 29 | include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) 30 | add_subdirectory("${_out}") 31 | 32 | else() 33 | # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument 34 | # or CMAKE_PREFIX_PATH from the environment 35 | if(NOT DEFINED CMAKE_PREFIX_PATH) 36 | if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") 37 | if(NOT WIN32) 38 | string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 39 | else() 40 | set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) 41 | endif() 42 | endif() 43 | endif() 44 | 45 | # list of catkin workspaces 46 | set(catkin_search_path "") 47 | foreach(path ${CMAKE_PREFIX_PATH}) 48 | if(EXISTS "${path}/.catkin") 49 | list(FIND catkin_search_path ${path} _index) 50 | if(_index EQUAL -1) 51 | list(APPEND catkin_search_path ${path}) 52 | endif() 53 | endif() 54 | endforeach() 55 | 56 | # search for catkin in all workspaces 57 | set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) 58 | find_package(catkin QUIET 59 | NO_POLICY_SCOPE 60 | PATHS ${catkin_search_path} 61 | NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) 62 | unset(CATKIN_TOPLEVEL_FIND_PACKAGE) 63 | 64 | if(NOT catkin_FOUND) 65 | message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") 66 | endif() 67 | endif() 68 | 69 | catkin_workspace() 70 | -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/utils/common.h: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #pragma once 38 | 39 | #include 40 | 41 | #include 42 | 43 | typedef pcl::PointXYZI PointType; 44 | 45 | inline double rad2deg(double radians) 46 | { 47 | return radians * 180.0 / M_PI; 48 | } 49 | 50 | inline double deg2rad(double degrees) 51 | { 52 | return degrees * M_PI / 180.0; 53 | } 54 | -------------------------------------------------------------------------------- /src/lidar_localization/src/models/registration/icp_registration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: ICP 匹配模块 3 | * @Author: Ge Yao 4 | * @Date: 2020-10-24 21:46:45 5 | */ 6 | #include "lidar_localization/models/registration/icp_registration.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | namespace lidar_localization { 11 | 12 | ICPRegistration::ICPRegistration( 13 | const YAML::Node& node 14 | ) : icp_ptr_(new pcl::IterativeClosestPoint()) { 15 | 16 | float max_corr_dist = node["max_corr_dist"].as(); 17 | float trans_eps = node["trans_eps"].as(); 18 | float euc_fitness_eps = node["euc_fitness_eps"].as(); 19 | int max_iter = node["max_iter"].as(); 20 | 21 | SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter); 22 | } 23 | 24 | ICPRegistration::ICPRegistration( 25 | float max_corr_dist, 26 | float trans_eps, 27 | float euc_fitness_eps, 28 | int max_iter 29 | ) : icp_ptr_(new pcl::IterativeClosestPoint()) { 30 | 31 | SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter); 32 | } 33 | 34 | bool ICPRegistration::SetRegistrationParam( 35 | float max_corr_dist, 36 | float trans_eps, 37 | float euc_fitness_eps, 38 | int max_iter 39 | ) { 40 | icp_ptr_->setMaxCorrespondenceDistance(max_corr_dist); 41 | icp_ptr_->setTransformationEpsilon(trans_eps); 42 | icp_ptr_->setEuclideanFitnessEpsilon(euc_fitness_eps); 43 | icp_ptr_->setMaximumIterations(max_iter); 44 | 45 | LOG(INFO) << "ICP params:" << std::endl 46 | << "max_corr_dist: " << max_corr_dist << ", " 47 | << "trans_eps: " << trans_eps << ", " 48 | << "euc_fitness_eps: " << euc_fitness_eps << ", " 49 | << "max_iter: " << max_iter 50 | << std::endl << std::endl; 51 | 52 | return true; 53 | } 54 | 55 | bool ICPRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) { 56 | icp_ptr_->setInputTarget(input_target); 57 | 58 | return true; 59 | } 60 | 61 | bool ICPRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 62 | const Eigen::Matrix4f& predict_pose, 63 | CloudData::CLOUD_PTR& result_cloud_ptr, 64 | Eigen::Matrix4f& result_pose) { 65 | icp_ptr_->setInputSource(input_source); 66 | icp_ptr_->align(*result_cloud_ptr, predict_pose); 67 | result_pose = icp_ptr_->getFinalTransformation(); 68 | 69 | return true; 70 | } 71 | 72 | } -------------------------------------------------------------------------------- /src/lidar_localization/src/sensor_data/gnss_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-06 20:42:23 5 | */ 6 | #include "lidar_localization/sensor_data/gnss_data.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | //静态成员变量必须在类外初始化 11 | bool lidar_localization::GNSSData::origin_position_inited = false; 12 | GeographicLib::LocalCartesian lidar_localization::GNSSData::geo_converter; 13 | 14 | namespace lidar_localization { 15 | void GNSSData::InitOriginPosition() { 16 | geo_converter.Reset(latitude, longitude, altitude); 17 | origin_position_inited = true; 18 | } 19 | 20 | void GNSSData::UpdateXYZ() { 21 | if (!origin_position_inited) { 22 | LOG(WARNING) << "GeoConverter has not set origin position"; 23 | } 24 | geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U); 25 | } 26 | 27 | bool GNSSData::SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time) { 28 | // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置 29 | // 即找到与同步时间相邻的左右两个数据 30 | // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值 31 | while (UnsyncedData.size() >= 2) { 32 | if (UnsyncedData.front().time > sync_time) 33 | return false; 34 | if (UnsyncedData.at(1).time < sync_time) { 35 | UnsyncedData.pop_front(); 36 | continue; 37 | } 38 | if (sync_time - UnsyncedData.front().time > 0.2) { 39 | UnsyncedData.pop_front(); 40 | return false; 41 | } 42 | if (UnsyncedData.at(1).time - sync_time > 0.2) { 43 | return false; 44 | } 45 | break; 46 | } 47 | if (UnsyncedData.size() < 2) 48 | return false; 49 | 50 | GNSSData front_data = UnsyncedData.at(0); 51 | GNSSData back_data = UnsyncedData.at(1); 52 | GNSSData synced_data; 53 | 54 | double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 55 | double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time); 56 | synced_data.time = sync_time; 57 | synced_data.status = back_data.status; 58 | synced_data.longitude = front_data.longitude * front_scale + back_data.longitude * back_scale; 59 | synced_data.latitude = front_data.latitude * front_scale + back_data.latitude * back_scale; 60 | synced_data.altitude = front_data.altitude * front_scale + back_data.altitude * back_scale; 61 | synced_data.local_E = front_data.local_E * front_scale + back_data.local_E * back_scale; 62 | synced_data.local_N = front_data.local_N * front_scale + back_data.local_N * back_scale; 63 | synced_data.local_U = front_data.local_U * front_scale + back_data.local_U * back_scale; 64 | 65 | SyncedData.push_back(synced_data); 66 | 67 | return true; 68 | } 69 | } -------------------------------------------------------------------------------- /src/lidar_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_localization 4 | 0.0.0 5 | The lidar_localization package 6 | 7 | 8 | 9 | 10 | rq 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | roscpp 55 | rospy 56 | roscpp 57 | rospy 58 | 59 | message_generation 60 | message_runtime 61 | 62 | rosbag 63 | std_msgs 64 | sensor_msgs 65 | geometry_msgs 66 | nav_msgs 67 | tf 68 | pcl_ros 69 | eigen_conversions 70 | std_srvs 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /src/lidar_localization/src/sensor_data/imu_data.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: imu data 3 | * @Author: Ren Qian 4 | * @Date: 2020-02-23 22:20:41 5 | */ 6 | #include "lidar_localization/sensor_data/imu_data.hpp" 7 | 8 | #include 9 | #include "glog/logging.h" 10 | 11 | namespace lidar_localization { 12 | Eigen::Matrix3f IMUData::GetOrientationMatrix() { 13 | Eigen::Quaterniond q(orientation.w, orientation.x, orientation.y, orientation.z); 14 | Eigen::Matrix3f matrix = q.matrix().cast(); 15 | 16 | return matrix; 17 | } 18 | 19 | bool IMUData::SyncData(std::deque& UnsyncedData, std::deque& SyncedData, double sync_time) { 20 | // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置 21 | // 即找到与同步时间相邻的左右两个数据 22 | // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值 23 | while (UnsyncedData.size() >= 2) { 24 | // UnsyncedData.front().time should be <= sync_time: 25 | if (UnsyncedData.front().time > sync_time) 26 | return false; 27 | // sync_time should be <= UnsyncedData.at(1).time: 28 | if (UnsyncedData.at(1).time < sync_time) { 29 | UnsyncedData.pop_front(); 30 | continue; 31 | } 32 | 33 | // sync_time - UnsyncedData.front().time should be <= 0.2: 34 | if (sync_time - UnsyncedData.front().time > 0.2) { 35 | UnsyncedData.pop_front(); 36 | return false; 37 | } 38 | // UnsyncedData.at(1).time - sync_time should be <= 0.2 39 | if (UnsyncedData.at(1).time - sync_time > 0.2) { 40 | return false; 41 | } 42 | break; 43 | } 44 | if (UnsyncedData.size() < 2) 45 | return false; 46 | 47 | IMUData front_data = UnsyncedData.at(0); 48 | IMUData back_data = UnsyncedData.at(1); 49 | IMUData synced_data; 50 | 51 | double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time); 52 | double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time); 53 | synced_data.time = sync_time; 54 | synced_data.linear_acceleration.x = front_data.linear_acceleration.x * front_scale + back_data.linear_acceleration.x * back_scale; 55 | synced_data.linear_acceleration.y = front_data.linear_acceleration.y * front_scale + back_data.linear_acceleration.y * back_scale; 56 | synced_data.linear_acceleration.z = front_data.linear_acceleration.z * front_scale + back_data.linear_acceleration.z * back_scale; 57 | synced_data.angular_velocity.x = front_data.angular_velocity.x * front_scale + back_data.angular_velocity.x * back_scale; 58 | synced_data.angular_velocity.y = front_data.angular_velocity.y * front_scale + back_data.angular_velocity.y * back_scale; 59 | synced_data.angular_velocity.z = front_data.angular_velocity.z * front_scale + back_data.angular_velocity.z * back_scale; 60 | // 四元数插值有线性插值和球面插值，球面插值更准确，但是两个四元数差别不大是，二者精度相当 61 | // 由于是对相邻两时刻姿态插值，姿态差比较小，所以可以用线性插值 62 | synced_data.orientation.x = front_data.orientation.x * front_scale + back_data.orientation.x * back_scale; 63 | synced_data.orientation.y = front_data.orientation.y * front_scale + back_data.orientation.y * back_scale; 64 | synced_data.orientation.z = front_data.orientation.z * front_scale + back_data.orientation.z * back_scale; 65 | synced_data.orientation.w = front_data.orientation.w * front_scale + back_data.orientation.w * back_scale; 66 | // 线性插值之后要归一化 67 | synced_data.orientation.Normlize(); 68 | 69 | SyncedData.push_back(synced_data); 70 | 71 | return true; 72 | } 73 | } -------------------------------------------------------------------------------- /src/lidar_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.2) 2 | project(lidar_localization) 3 | 4 | SET(CMAKE_BUILD_TYPE "Release") 5 | SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") 6 | add_compile_options(-std=c++11) 7 | add_definitions(-std=c++11) 8 | 9 | find_package( 10 | catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | rosbag 14 | std_msgs 15 | sensor_msgs 16 | geometry_msgs 17 | nav_msgs 18 | tf 19 | pcl_ros 20 | eigen_conversions 21 | message_generation 22 | std_srvs 23 | ) 24 | 25 | add_service_files( 26 | FILES 27 | saveMap.srv 28 | ) 29 | 30 | generate_messages( 31 | DEPENDENCIES 32 | std_msgs 33 | ) 34 | 35 | set(ALL_TARGET_LIBRARIES "") 36 | 37 | include(cmake/glog.cmake) 38 | include(cmake/YAML.cmake) 39 | include(cmake/geographic.cmake) 40 | include(cmake/PCL.cmake) 41 | include(cmake/eigen3.cmake) 42 | include(cmake/sophus.cmake) 43 | include(cmake/ceres.cmake) 44 | 45 | include_directories( 46 | include ${catkin_INCLUDE_DIRS} 47 | ) 48 | 49 | include(cmake/global_defination.cmake) 50 | catkin_package() 51 | 52 | file(GLOB_RECURSE ALL_SRCS "*.cpp") 53 | file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp") 54 | 55 | list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS}) 56 | 57 | add_executable(aloam_scan_registration_node src/aloam_scan_registration_node.cpp ${ALL_SRCS}) 58 | add_dependencies(aloam_scan_registration_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 59 | target_link_libraries(aloam_scan_registration_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES}) 60 | 61 | add_executable(aloam_laser_odometry_node src/aloam_laser_odometry_node.cpp ${ALL_SRCS}) 62 | add_dependencies(aloam_laser_odometry_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 63 | target_link_libraries(aloam_laser_odometry_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES}) 64 | 65 | add_executable(aloam_mapping_node src/aloam_mapping_node.cpp ${ALL_SRCS}) 66 | add_dependencies(aloam_mapping_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 67 | target_link_libraries(aloam_mapping_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES}) 68 | 69 | add_executable(evaluation_node src/evaluation_node.cpp ${ALL_SRCS}) 70 | add_dependencies(evaluation_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) 71 | target_link_libraries(evaluation_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES}) 72 | 73 | ############# 74 | ## Install ## 75 | ############# 76 | 77 | # all install targets should use catkin DESTINATION variables 78 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 79 | 80 | ## Mark executable scripts (Python etc.) for installation 81 | ## in contrast to setup.py, you can choose the destination 82 | # catkin_install_python(PROGRAMS 83 | # scripts/my_python_script 84 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 85 | # ) 86 | 87 | ## Mark executables for installation 88 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 89 | install(TARGETS 90 | aloam_scan_registration_node 91 | aloam_laser_odometry_node 92 | aloam_mapping_node 93 | evaluation_node 94 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 95 | ) 96 | 97 | ## Mark cpp header files for installation 98 | install(DIRECTORY 99 | include/ 100 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 101 | FILES_MATCHING PATTERN "*.h" 102 | PATTERN ".svn" EXCLUDE 103 | ) 104 | 105 | ## Mark other directories for installation: 106 | install(DIRECTORY 107 | launch/ 108 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 109 | FILES_MATCHING PATTERN "*.launch" 110 | ) 111 | install(DIRECTORY 112 | config/ 113 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config 114 | FILES_MATCHING PATTERN "*.yaml" 115 | ) 116 | install(DIRECTORY 117 | rviz/ 118 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz 119 | FILES_MATCHING PATTERN "*.rviz" 120 | ) -------------------------------------------------------------------------------- /src/lidar_localization/src/models/registration/icp_svd_registration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: ICP 匹配模块 3 | * @Author: Ge Yao 4 | * @Date: 2020-10-24 21:46:45 5 | */ 6 | #include "lidar_localization/models/registration/icp_svd_registration.hpp" 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "glog/logging.h" 17 | 18 | namespace lidar_localization { 19 | 20 | ICPSVDRegistration::ICPSVDRegistration( 21 | const YAML::Node& node 22 | ) : input_target_kdtree_(new pcl::KdTreeFLANN()) { 23 | // parse params: 24 | float max_corr_dist = node["max_corr_dist"].as(); 25 | float trans_eps = node["trans_eps"].as(); 26 | float euc_fitness_eps = node["euc_fitness_eps"].as(); 27 | int max_iter = node["max_iter"].as(); 28 | 29 | SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter); 30 | } 31 | 32 | ICPSVDRegistration::ICPSVDRegistration( 33 | float max_corr_dist, 34 | float trans_eps, 35 | float euc_fitness_eps, 36 | int max_iter 37 | ) : input_target_kdtree_(new pcl::KdTreeFLANN()) { 38 | SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter); 39 | } 40 | 41 | bool ICPSVDRegistration::SetRegistrationParam( 42 | float max_corr_dist, 43 | float trans_eps, 44 | float euc_fitness_eps, 45 | int max_iter 46 | ) { 47 | // set params: 48 | max_corr_dist_ = max_corr_dist; 49 | trans_eps_ = trans_eps; 50 | euc_fitness_eps_ = euc_fitness_eps; 51 | max_iter_ = max_iter; 52 | 53 | LOG(INFO) << "ICP SVD params:" << std::endl 54 | << "max_corr_dist: " << max_corr_dist_ << ", " 55 | << "trans_eps: " << trans_eps_ << ", " 56 | << "euc_fitness_eps: " << euc_fitness_eps_ << ", " 57 | << "max_iter: " << max_iter_ 58 | << std::endl << std::endl; 59 | 60 | return true; 61 | } 62 | 63 | bool ICPSVDRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) { 64 | input_target_ = input_target; 65 | input_target_kdtree_->setInputCloud(input_target_); 66 | 67 | return true; 68 | } 69 | 70 | bool ICPSVDRegistration::ScanMatch( 71 | const CloudData::CLOUD_PTR& input_source, 72 | const Eigen::Matrix4f& predict_pose, 73 | CloudData::CLOUD_PTR& result_cloud_ptr, 74 | Eigen::Matrix4f& result_pose 75 | ) { 76 | input_source_ = input_source; 77 | 78 | // pre-process input source: 79 | CloudData::CLOUD_PTR transformed_input_source(new CloudData::CLOUD()); 80 | pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose); 81 | 82 | // init estimation: 83 | transformation_.setIdentity(); 84 | 85 | // do estimation: 86 | int curr_iter = 0; 87 | while (curr_iter < max_iter_) { 88 | // apply current estimation: 89 | CloudData::CLOUD_PTR curr_input_source(new CloudData::CLOUD()); 90 | pcl::transformPointCloud(*transformed_input_source, *curr_input_source, transformation_); 91 | 92 | // get correspondence: 93 | std::vector xs; 94 | std::vector ys; 95 | 96 | // do not have enough correspondence -- break: 97 | if (GetCorrespondence(curr_input_source, xs, ys) < 3) 98 | break; 99 | 100 | // update current transform: 101 | Eigen::Matrix4f delta_transformation; 102 | GetTransform(xs, ys, delta_transformation); 103 | 104 | // whether the transformation update is significant: 105 | if (!IsSignificant(delta_transformation, trans_eps_)) 106 | break; 107 | 108 | transformation_ = delta_transformation * transformation_; 109 | 110 | ++curr_iter; 111 | } 112 | 113 | // set output: 114 | result_pose = transformation_ * predict_pose; 115 | pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose); 116 | 117 | return true; 118 | } 119 | 120 | size_t ICPSVDRegistration::GetCorrespondence( 121 | const CloudData::CLOUD_PTR &input_source, 122 | std::vector &xs, 123 | std::vector &ys 124 | ) { 125 | const float MAX_CORR_DIST_SQR = max_corr_dist_ * max_corr_dist_; 126 | 127 | size_t num_corr = 0; 128 | 129 | for (size_t i = 0; i < input_source->points.size(); ++i) { 130 | std::vector corr_ind; 131 | std::vector corr_sq_dis; 132 | input_target_kdtree_->nearestKSearch( 133 | input_source->at(i), 134 | 1, 135 | corr_ind, corr_sq_dis 136 | ); 137 | 138 | if (corr_sq_dis.at(0) > MAX_CORR_DIST_SQR) 139 | continue; 140 | 141 | // add correspondence: 142 | Eigen::Vector3f x( 143 | input_target_->at(corr_ind.at(0)).x, 144 | input_target_->at(corr_ind.at(0)).y, 145 | input_target_->at(corr_ind.at(0)).z 146 | ); 147 | Eigen::Vector3f y( 148 | input_source->at(i).x, 149 | input_source->at(i).y, 150 | input_source->at(i).z 151 | ); 152 | 153 | xs.push_back(x); 154 | ys.push_back(y); 155 | 156 | ++num_corr; 157 | } 158 | 159 | return num_corr; 160 | } 161 | 162 | void ICPSVDRegistration::GetTransform( 163 | const std::vector &xs, 164 | const std::vector &ys, 165 | Eigen::Matrix4f &transformation_ 166 | ) { 167 | const size_t N = xs.size(); 168 | 169 | // TODO -- find centroids of mu_x and mu_y: 170 | 171 | // TODO -- build H: 172 | 173 | // TODO -- solve R: 174 | 175 | // TODO -- solve t: 176 | 177 | // set output: 178 | transformation_.setIdentity(); 179 | } 180 | 181 | bool ICPSVDRegistration::IsSignificant( 182 | const Eigen::Matrix4f &transformation, 183 | const float trans_eps 184 | ) { 185 | // a. translation magnitude -- norm: 186 | float translation_magnitude = transformation.block<3, 1>(0, 3).norm(); 187 | // b. rotation magnitude -- angle: 188 | float rotation_magnitude = fabs( 189 | acos( 190 | (transformation.block<3, 3>(0, 0).trace() - 1.0f) / 2.0f 191 | ) 192 | ); 193 | 194 | return ( 195 | (translation_magnitude > trans_eps) || 196 | (rotation_magnitude > trans_eps) 197 | ); 198 | } 199 | 200 | } -------------------------------------------------------------------------------- /src/lidar_localization/src/evaluation/evaluation_flow.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @Description: evaluation facade 3 | * @Author: Ge Yao 4 | * @Date: 2021-01-30 22:38:22 5 | */ 6 | #include "lidar_localization/evaluation/evaluation_flow.hpp" 7 | 8 | #include "glog/logging.h" 9 | 10 | #include "lidar_localization/tools/file_manager.hpp" 11 | #include "lidar_localization/global_defination/global_defination.h" 12 | 13 | namespace lidar_localization { 14 | 15 | EvaluationFlow::EvaluationFlow(ros::NodeHandle& nh) { 16 | std::string config_file_path = WORK_SPACE_PATH + "/config/dataset/config.yaml"; 17 | YAML::Node config_node = YAML::LoadFile(config_file_path); 18 | 19 | InitSubscribers(nh, config_node["measurements"]); 20 | 21 | gnss_pub_ptr_ = std::make_shared(nh, "/gnss_odom", "/map", "/velo_link", 100); 22 | } 23 | 24 | bool EvaluationFlow::Run() { 25 | if (!ReadData()) { 26 | return false; 27 | } 28 | 29 | if (!InitCalibration()) { 30 | return false; 31 | } 32 | 33 | 34 | if (!InitGNSS()) { 35 | return false; 36 | } 37 | 38 | while( HasData() ) { 39 | if (!ValidData()) { 40 | continue; 41 | } 42 | 43 | UpdateLaserOdometry(); 44 | UpdateGNSSOdometry(); 45 | 46 | SaveTrajectory(); 47 | } 48 | 49 | return true; 50 | } 51 | 52 | bool EvaluationFlow::InitSubscribers(ros::NodeHandle& nh, const YAML::Node& config_node) { 53 | laser_odom_sub_ptr_ = std::make_shared( 54 | nh, 55 | "/laser_odom_scan_to_map", 1000 56 | ); 57 | 58 | lidar_to_imu_ptr_ = std::make_shared( 59 | nh, 60 | config_node["imu"]["frame_id"].as(), config_node["lidar"]["frame_id"].as() 61 | ); 62 | imu_sub_ptr_ = std::make_shared( 63 | nh, 64 | config_node["imu"]["topic_name"].as(), config_node["imu"]["queue_size"].as() 65 | ); 66 | gnss_sub_ptr_ = std::make_shared( 67 | nh, 68 | config_node["gnss"]["topic_name"].as(), config_node["gnss"]["queue_size"].as() 69 | ); 70 | 71 | return true; 72 | } 73 | 74 | bool EvaluationFlow::ReadData() { 75 | static bool evaluator_inited = false; 76 | 77 | laser_odom_sub_ptr_->ParseData(laser_odom_data_buff_); 78 | 79 | static std::deque unsynced_imu_; 80 | static std::deque unsynced_gnss_; 81 | 82 | imu_sub_ptr_->ParseData(unsynced_imu_); 83 | gnss_sub_ptr_->ParseData(unsynced_gnss_); 84 | 85 | if ( laser_odom_data_buff_.empty() ) { 86 | return false; 87 | } 88 | 89 | double laser_odom_time = laser_odom_data_buff_.front().time; 90 | 91 | bool valid_imu = IMUData::SyncData(unsynced_imu_, imu_data_buff_, laser_odom_time); 92 | bool valid_gnss = GNSSData::SyncData(unsynced_gnss_, gnss_data_buff_, laser_odom_time); 93 | 94 | if ( !evaluator_inited ) { 95 | if (!valid_imu || !valid_gnss) { 96 | laser_odom_data_buff_.pop_front(); 97 | return false; 98 | } 99 | evaluator_inited = true; 100 | } 101 | 102 | return true; 103 | } 104 | 105 | bool EvaluationFlow::InitCalibration() { 106 | static bool calibration_received = false; 107 | if (!calibration_received) { 108 | if (lidar_to_imu_ptr_->LookupData(lidar_to_imu_)) { 109 | calibration_received = true; 110 | } 111 | } 112 | 113 | return calibration_received; 114 | } 115 | 116 | bool EvaluationFlow::InitGNSS() { 117 | static bool gnss_inited = false; 118 | 119 | if (!gnss_inited) { 120 | GNSSData gnss_data = gnss_data_buff_.front(); 121 | gnss_data.InitOriginPosition(); 122 | gnss_inited = true; 123 | } 124 | 125 | return gnss_inited; 126 | } 127 | 128 | bool EvaluationFlow::HasData() { 129 | if ( laser_odom_data_buff_.empty() ) 130 | return false; 131 | if ( imu_data_buff_.empty() ) 132 | return false; 133 | if ( gnss_data_buff_.empty() ) 134 | return false; 135 | 136 | return true; 137 | } 138 | 139 | bool EvaluationFlow::ValidData() { 140 | current_laser_odom_data_ = laser_odom_data_buff_.front(); 141 | current_imu_data_ = imu_data_buff_.front(); 142 | current_gnss_data_ = gnss_data_buff_.front(); 143 | 144 | double d_time = current_laser_odom_data_.time - current_imu_data_.time; 145 | if (d_time < -0.08) { 146 | laser_odom_data_buff_.pop_front(); 147 | return false; 148 | } 149 | 150 | if (d_time > 0.08) { 151 | imu_data_buff_.pop_front(); 152 | gnss_data_buff_.pop_front(); 153 | return false; 154 | } 155 | 156 | laser_odom_data_buff_.pop_front(); 157 | imu_data_buff_.pop_front(); 158 | gnss_data_buff_.pop_front(); 159 | 160 | return true; 161 | } 162 | 163 | bool EvaluationFlow::UpdateLaserOdometry() { 164 | laser_odometry_ = current_laser_odom_data_.pose; 165 | 166 | return true; 167 | } 168 | 169 | bool EvaluationFlow::UpdateGNSSOdometry() { 170 | static bool is_synced = false; 171 | static Eigen::Matrix4f gnss_to_odom = Eigen::Matrix4f::Identity(); 172 | 173 | gnss_odometry_ = Eigen::Matrix4f::Identity(); 174 | 175 | current_gnss_data_.UpdateXYZ(); 176 | gnss_odometry_(0,3) = current_gnss_data_.local_E; 177 | gnss_odometry_(1,3) = current_gnss_data_.local_N; 178 | gnss_odometry_(2,3) = current_gnss_data_.local_U; 179 | gnss_odometry_.block<3,3>(0,0) = current_imu_data_.GetOrientationMatrix(); 180 | gnss_odometry_ *= lidar_to_imu_; 181 | 182 | // init transform from GNSS frame to laser odometry frame: 183 | if (!is_synced) { 184 | gnss_to_odom = laser_odometry_ * gnss_odometry_.inverse(); 185 | is_synced = true; 186 | } 187 | 188 | gnss_odometry_ = gnss_to_odom * gnss_odometry_; 189 | 190 | gnss_pub_ptr_->Publish(gnss_odometry_); 191 | 192 | return true; 193 | } 194 | 195 | bool EvaluationFlow::SaveTrajectory() { 196 | static std::ofstream ground_truth, laser_odom; 197 | static bool is_file_created = false; 198 | 199 | if (!is_file_created) { 200 | if (!FileManager::CreateDirectory(WORK_SPACE_PATH + "/slam_data/trajectory")) 201 | return false; 202 | if (!FileManager::CreateFile(ground_truth, WORK_SPACE_PATH + "/slam_data/trajectory/ground_truth.txt")) 203 | return false; 204 | if (!FileManager::CreateFile(laser_odom, WORK_SPACE_PATH + "/slam_data/trajectory/laser_odom.txt")) 205 | return false; 206 | is_file_created = true; 207 | } 208 | 209 | for (int i = 0; i < 3; ++i) { 210 | for (int j = 0; j < 4; ++j) { 211 | ground_truth << gnss_odometry_(i, j); 212 | laser_odom << laser_odometry_(i, j); 213 | if (i == 2 && j == 3) { 214 | ground_truth << std::endl; 215 | laser_odom << std::endl; 216 | } else { 217 | ground_truth << " "; 218 | laser_odom << " "; 219 | } 220 | } 221 | } 222 | 223 | return true; 224 | } 225 | 226 | } // namespace lidar_localization -------------------------------------------------------------------------------- /src/lidar_localization/include/lidar_localization/models/loam/aloam_factor.hpp: -------------------------------------------------------------------------------- 1 | // Author: Tong Qin qintonguav@gmail.com 2 | // Shaozu Cao saozu.cao@connect.ust.hk 3 | 4 | // 5 | // TODO: implement analytic Jacobians for LOAM residuals in this file 6 | // 7 | 8 | #include 9 | #include 10 | 11 | // 12 | // TODO: Sophus is ready to use if you have a good undestanding of Lie algebra. 13 | // 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | Eigen::Matrix skew(Eigen::Matrix& mat_in){ 26 | Eigen::Matrix skew_mat; 27 | skew_mat.setZero(); 28 | skew_mat(0,1) = -mat_in(2); 29 | skew_mat(0,2) = mat_in(1); 30 | skew_mat(1,2) = -mat_in(0); 31 | skew_mat(1,0) = mat_in(2); 32 | skew_mat(2,0) = -mat_in(1); 33 | skew_mat(2,1) = mat_in(0); 34 | return skew_mat; 35 | } 36 | 37 | 38 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){ 39 | Eigen::Vector3d omega(se3.data()); 40 | Eigen::Vector3d upsilon(se3.data()+3); 41 | Eigen::Matrix3d Omega = skew(omega); 42 | 43 | double theta = omega.norm(); 44 | double half_theta = 0.5*theta; 45 | 46 | double imag_factor; 47 | double real_factor = cos(half_theta); 48 | if(theta<1e-10) 49 | { 50 | double theta_sq = theta*theta; 51 | double theta_po4 = theta_sq*theta_sq; 52 | imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4; 53 | } 54 | else 55 | { 56 | double sin_half_theta = sin(half_theta); 57 | imag_factor = sin_half_theta/theta; 58 | } 59 | 60 | q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z()); 61 | 62 | 63 | Eigen::Matrix3d J; 64 | if (theta<1e-10) 65 | { 66 | J = q.matrix(); 67 | } 68 | else 69 | { 70 | Eigen::Matrix3d Omega2 = Omega*Omega; 71 | J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2); 72 | } 73 | 74 | t = J*upsilon; 75 | } 76 | 77 | 78 | 79 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 80 | public: 81 | 82 | EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_) 83 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_) {} 84 | virtual ~EdgeAnalyticCostFunction() {} 85 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 86 | { 87 | Eigen::Map q_last_curr(parameters[0]); 88 | Eigen::Map t_last_curr(parameters[0] + 4); 89 | Eigen::Vector3d lp; 90 | lp = q_last_curr * curr_point + t_last_curr; //new point 91 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b); 92 | Eigen::Vector3d de = last_point_a - last_point_b; 93 | 94 | Eigen::Vector3d residualsVector; 95 | residualsVector << nu.x() / de.norm(), nu.y() / de.norm(), nu.z() / de.norm(); 96 | residuals[0] = residualsVector.norm(); 97 | 98 | 99 | if(jacobians != NULL) 100 | { 101 | if(jacobians[0] != NULL) 102 | { 103 | Eigen::Matrix3d skew_lp = skew(lp); 104 | Eigen::Matrix dp_by_so3; 105 | dp_by_so3.block<3,3>(0,0) = -skew_lp; 106 | (dp_by_so3.block<3,3>(0, 3)).setIdentity(); 107 | Eigen::Map > J_se3(jacobians[0]); 108 | J_se3.setZero(); 109 | Eigen::Vector3d re = last_point_b - last_point_a; 110 | Eigen::Matrix3d skew_re = skew(re); 111 | 112 | J_se3.block<1,6>(0,0) = (residualsVector/residuals[0]).transpose()*skew_re * dp_by_so3/de.norm(); 113 | 114 | } 115 | } 116 | 117 | return true; 118 | } 119 | 120 | 121 | Eigen::Vector3d curr_point; 122 | Eigen::Vector3d last_point_a; 123 | Eigen::Vector3d last_point_b; 124 | }; 125 | 126 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 127 | public: 128 | SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) 129 | : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_) {} 130 | virtual ~SurfNormAnalyticCostFunction() {} 131 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 132 | { 133 | Eigen::Map q_w_curr(parameters[0]); 134 | Eigen::Map t_w_curr(parameters[0] + 4); 135 | Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr; 136 | 137 | residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm; 138 | 139 | if(jacobians != NULL) 140 | { 141 | if(jacobians[0] != NULL) 142 | { 143 | Eigen::Matrix3d skew_point_w = skew(point_w); 144 | 145 | Eigen::Matrix dp_by_so3; 146 | dp_by_so3.block<3,3>(0,0) = -skew_point_w; 147 | (dp_by_so3.block<3,3>(0, 3)).setIdentity(); 148 | Eigen::Map > J_se3(jacobians[0]); 149 | J_se3.setZero(); 150 | J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_so3; 151 | 152 | } 153 | } 154 | return true; 155 | } 156 | 157 | Eigen::Vector3d curr_point; 158 | Eigen::Vector3d plane_unit_norm; 159 | double negative_OA_dot_norm; 160 | }; 161 | 162 | class PoseSE3Parameterization : public ceres::LocalParameterization { 163 | public: 164 | 165 | PoseSE3Parameterization() {} 166 | virtual ~PoseSE3Parameterization() {} 167 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const 168 | { 169 | Eigen::Map trans(x + 4); 170 | 171 | Eigen::Quaterniond delta_q; 172 | Eigen::Vector3d delta_t; 173 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 174 | Eigen::Map quater(x); 175 | Eigen::Map quater_plus(x_plus_delta); 176 | Eigen::Map trans_plus(x_plus_delta + 4); 177 | 178 | quater_plus = delta_q * quater; 179 | trans_plus = delta_q * trans + delta_t; 180 | 181 | return true; 182 | } 183 | virtual bool ComputeJacobian(const double* x, double* jacobian) const 184 | { 185 | Eigen::Map> j(jacobian); 186 | (j.topRows(6)).setIdentity(); 187 | (j.bottomRows(1)).setZero(); 188 | } 189 | virtual int GlobalSize() const { return 7; } 190 | virtual int LocalSize() const { return 6; } 191 | }; 192 | 193 | struct LidarEdgeFactor 194 | { 195 | LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, 196 | Eigen::Vector3d last_point_b_, double s_) 197 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {} 198 | 199 | template 200 | bool operator()(const T *q, const T *t, T *residual) const 201 | { 202 | 203 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 204 | Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())}; 205 | Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())}; 206 | 207 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 208 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 209 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 210 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 211 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 212 | 213 | Eigen::Matrix lp; 214 | lp = q_last_curr * cp + t_last_curr; 215 | 216 | Eigen::Matrix nu = (lp - lpa).cross(lp - lpb); 217 | Eigen::Matrix de = lpa - lpb; 218 | 219 | residual[0] = nu.x() / de.norm(); 220 | residual[1] = nu.y() / de.norm(); 221 | residual[2] = nu.z() / de.norm(); 222 | 223 | return true; 224 | } 225 | 226 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_, 227 | const Eigen::Vector3d last_point_b_, const double s_) 228 | { 229 | return (new ceres::AutoDiffCostFunction< 230 | LidarEdgeFactor, 3, 4, 3>( 231 | new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); 232 | } 233 | 234 | Eigen::Vector3d curr_point, last_point_a, last_point_b; 235 | double s; 236 | }; 237 | 238 | struct LidarPlaneFactor 239 | { 240 | LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, 241 | Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_) 242 | : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_), 243 | last_point_m(last_point_m_), s(s_) 244 | { 245 | ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m); 246 | ljm_norm.normalize(); 247 | } 248 | 249 | template 250 | bool operator()(const T *q, const T *t, T *residual) const 251 | { 252 | 253 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 254 | Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; 255 | //Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())}; 256 | //Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())}; 257 | Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())}; 258 | 259 | //Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]}; 260 | Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]}; 261 | Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)}; 262 | q_last_curr = q_identity.slerp(T(s), q_last_curr); 263 | Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]}; 264 | 265 | Eigen::Matrix lp; 266 | lp = q_last_curr * cp + t_last_curr; 267 | 268 | residual[0] = (lp - lpj).dot(ljm); 269 | 270 | return true; 271 | } 272 | 273 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_, 274 | const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_, 275 | const double s_) 276 | { 277 | return (new ceres::AutoDiffCostFunction< 278 | LidarPlaneFactor, 1, 4, 3>( 279 | new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_))); 280 | } 281 | 282 | Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m; 283 | Eigen::Vector3d ljm_norm; 284 | double s; 285 | }; 286 | 287 | struct LidarPlaneNormFactor 288 | { 289 | 290 | LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, 291 | double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), 292 | negative_OA_dot_norm(negative_OA_dot_norm_) {} 293 | 294 | template 295 | bool operator()(const T *q, const T *t, T *residual) const 296 | { 297 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 298 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 299 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 300 | Eigen::Matrix point_w; 301 | point_w = q_w_curr * cp + t_w_curr; 302 | 303 | Eigen::Matrix norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z())); 304 | residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); 305 | return true; 306 | } 307 | 308 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_, 309 | const double negative_OA_dot_norm_) 310 | { 311 | return (new ceres::AutoDiffCostFunction< 312 | LidarPlaneNormFactor, 1, 4, 3>( 313 | new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_))); 314 | } 315 | 316 | Eigen::Vector3d curr_point; 317 | Eigen::Vector3d plane_unit_norm; 318 | double negative_OA_dot_norm; 319 | }; 320 | 321 | 322 | struct LidarDistanceFactor 323 | { 324 | 325 | LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) 326 | : curr_point(curr_point_), closed_point(closed_point_){} 327 | 328 | template 329 | bool operator()(const T *q, const T *t, T *residual) const 330 | { 331 | Eigen::Quaternion q_w_curr{q[3], q[0], q[1], q[2]}; 332 | Eigen::Matrix t_w_curr{t[0], t[1], t[2]}; 333 | Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())}; 334 | Eigen::Matrix point_w; 335 | point_w = q_w_curr * cp + t_w_curr; 336 | 337 | 338 | residual[0] = point_w.x() - T(closed_point.x()); 339 | residual[1] = point_w.y() - T(closed_point.y()); 340 | residual[2] = point_w.z() - T(closed_point.z()); 341 | return true; 342 | } 343 | 344 | static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_) 345 | { 346 | return (new ceres::AutoDiffCostFunction< 347 | LidarDistanceFactor, 3, 4, 3>( 348 | new LidarDistanceFactor(curr_point_, closed_point_))); 349 | } 350 | 351 | Eigen::Vector3d curr_point; 352 | Eigen::Vector3d closed_point; 353 | }; 354 | -------------------------------------------------------------------------------- /src/lidar_localization/src/aloam_scan_registration_node.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include "lidar_localization/utils/common.h" 56 | #include "lidar_localization/utils/tic_toc.h" 57 | 58 | using std::atan2; 59 | using std::cos; 60 | using std::sin; 61 | 62 | const double scanPeriod = 0.1; 63 | 64 | const int systemDelay = 0; 65 | int systemInitCount = 0; 66 | bool systemInited = false; 67 | int N_SCANS = 0; 68 | float cloudCurvature[400000]; 69 | int cloudSortInd[400000]; 70 | int cloudNeighborPicked[400000]; 71 | int cloudLabel[400000]; 72 | 73 | bool comp (int i,int j) { return (cloudCurvature[i] pubEachScan; 82 | 83 | bool PUB_EACH_LINE = false; 84 | 85 | double MINIMUM_RANGE = 0.1; 86 | 87 | template 88 | void removeClosedPointCloud( 89 | const pcl::PointCloud &cloud_in, 90 | pcl::PointCloud &cloud_out, float thres 91 | ) { 92 | if (&cloud_in != &cloud_out) 93 | { 94 | cloud_out.header = cloud_in.header; 95 | cloud_out.points.resize(cloud_in.points.size()); 96 | } 97 | 98 | size_t j = 0; 99 | 100 | for (size_t i = 0; i < cloud_in.points.size(); ++i) 101 | { 102 | if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres) 103 | continue; 104 | cloud_out.points[j] = cloud_in.points[i]; 105 | j++; 106 | } 107 | if (j != cloud_in.points.size()) 108 | { 109 | cloud_out.points.resize(j); 110 | } 111 | 112 | cloud_out.height = 1; 113 | cloud_out.width = static_cast(j); 114 | cloud_out.is_dense = true; 115 | } 116 | 117 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 118 | { 119 | if (!systemInited) 120 | { 121 | systemInitCount++; 122 | if (systemInitCount >= systemDelay) 123 | { 124 | systemInited = true; 125 | std::cout << "Scan Registration has been Inited." << std::endl; 126 | } 127 | else 128 | return; 129 | } 130 | 131 | TicToc t_whole; 132 | TicToc t_prepare; 133 | std::vector scanStartInd(N_SCANS, 0); 134 | std::vector scanEndInd(N_SCANS, 0); 135 | 136 | pcl::PointCloud laserCloudIn; 137 | pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 138 | std::vector indices; 139 | 140 | pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); 141 | removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); 142 | 143 | int cloudSize = laserCloudIn.points.size(); 144 | float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); 145 | float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, 146 | laserCloudIn.points[cloudSize - 1].x) + 147 | 2 * M_PI; 148 | 149 | if (endOri - startOri > 3 * M_PI) 150 | { 151 | endOri -= 2 * M_PI; 152 | } 153 | else if (endOri - startOri < M_PI) 154 | { 155 | endOri += 2 * M_PI; 156 | } 157 | //printf("end Ori %f\n", endOri); 158 | 159 | bool halfPassed = false; 160 | int count = cloudSize; 161 | PointType point; 162 | std::vector> laserCloudScans(N_SCANS); 163 | for (int i = 0; i < cloudSize; i++) 164 | { 165 | point.x = laserCloudIn.points[i].x; 166 | point.y = laserCloudIn.points[i].y; 167 | point.z = laserCloudIn.points[i].z; 168 | 169 | float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; 170 | int scanID = 0; 171 | 172 | if (N_SCANS == 16) 173 | { 174 | scanID = int((angle + 15) / 2 + 0.5); 175 | if (scanID > (N_SCANS - 1) || scanID < 0) 176 | { 177 | count--; 178 | continue; 179 | } 180 | } 181 | else if (N_SCANS == 32) 182 | { 183 | scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); 184 | if (scanID > (N_SCANS - 1) || scanID < 0) 185 | { 186 | count--; 187 | continue; 188 | } 189 | } 190 | else if (N_SCANS == 64) 191 | { 192 | if (angle >= -8.83) 193 | scanID = int((2 - angle) * 3.0 + 0.5); 194 | else 195 | scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); 196 | 197 | // use [0 50] > 50 remove outlies 198 | if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0) 199 | { 200 | count--; 201 | continue; 202 | } 203 | } 204 | else 205 | { 206 | printf("wrong scan number\n"); 207 | ROS_BREAK(); 208 | } 209 | //printf("angle %f scanID %d \n", angle, scanID); 210 | 211 | float ori = -atan2(point.y, point.x); 212 | if (!halfPassed) 213 | { 214 | if (ori < startOri - M_PI / 2) 215 | { 216 | ori += 2 * M_PI; 217 | } 218 | else if (ori > startOri + M_PI * 3 / 2) 219 | { 220 | ori -= 2 * M_PI; 221 | } 222 | 223 | if (ori - startOri > M_PI) 224 | { 225 | halfPassed = true; 226 | } 227 | } 228 | else 229 | { 230 | ori += 2 * M_PI; 231 | if (ori < endOri - M_PI * 3 / 2) 232 | { 233 | ori += 2 * M_PI; 234 | } 235 | else if (ori > endOri + M_PI / 2) 236 | { 237 | ori -= 2 * M_PI; 238 | } 239 | } 240 | 241 | float relTime = (ori - startOri) / (endOri - startOri); 242 | point.intensity = scanID + scanPeriod * relTime; 243 | laserCloudScans[scanID].push_back(point); 244 | } 245 | 246 | cloudSize = count; 247 | printf("points size %d \n", cloudSize); 248 | 249 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 250 | for (int i = 0; i < N_SCANS; i++) 251 | { 252 | scanStartInd[i] = laserCloud->size() + 5; 253 | *laserCloud += laserCloudScans[i]; 254 | scanEndInd[i] = laserCloud->size() - 6; 255 | } 256 | 257 | printf("prepare time %f \n", t_prepare.toc()); 258 | 259 | for (int i = 5; i < cloudSize - 5; i++) 260 | { 261 | float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x; 262 | float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y; 263 | float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z; 264 | 265 | cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; 266 | cloudSortInd[i] = i; 267 | cloudNeighborPicked[i] = 0; 268 | cloudLabel[i] = 0; 269 | } 270 | 271 | 272 | TicToc t_pts; 273 | 274 | pcl::PointCloud cornerPointsSharp; 275 | pcl::PointCloud cornerPointsLessSharp; 276 | pcl::PointCloud surfPointsFlat; 277 | pcl::PointCloud surfPointsLessFlat; 278 | 279 | float t_q_sort = 0; 280 | for (int i = 0; i < N_SCANS; i++) 281 | { 282 | if( scanEndInd[i] - scanStartInd[i] < 6) 283 | continue; 284 | pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud); 285 | for (int j = 0; j < 6; j++) 286 | { 287 | int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 288 | int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; 289 | 290 | TicToc t_tmp; 291 | std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); 292 | t_q_sort += t_tmp.toc(); 293 | 294 | int largestPickedNum = 0; 295 | for (int k = ep; k >= sp; k--) 296 | { 297 | int ind = cloudSortInd[k]; 298 | 299 | if (cloudNeighborPicked[ind] == 0 && 300 | cloudCurvature[ind] > 0.1) 301 | { 302 | 303 | largestPickedNum++; 304 | if (largestPickedNum <= 2) 305 | { 306 | cloudLabel[ind] = 2; 307 | cornerPointsSharp.push_back(laserCloud->points[ind]); 308 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 309 | } 310 | else if (largestPickedNum <= 20) 311 | { 312 | cloudLabel[ind] = 1; 313 | cornerPointsLessSharp.push_back(laserCloud->points[ind]); 314 | } 315 | else 316 | { 317 | break; 318 | } 319 | 320 | cloudNeighborPicked[ind] = 1; 321 | 322 | for (int l = 1; l <= 5; l++) 323 | { 324 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; 325 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; 326 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; 327 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 328 | { 329 | break; 330 | } 331 | 332 | cloudNeighborPicked[ind + l] = 1; 333 | } 334 | for (int l = -1; l >= -5; l--) 335 | { 336 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; 337 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; 338 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; 339 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 340 | { 341 | break; 342 | } 343 | 344 | cloudNeighborPicked[ind + l] = 1; 345 | } 346 | } 347 | } 348 | 349 | int smallestPickedNum = 0; 350 | for (int k = sp; k <= ep; k++) 351 | { 352 | int ind = cloudSortInd[k]; 353 | 354 | if (cloudNeighborPicked[ind] == 0 && 355 | cloudCurvature[ind] < 0.1) 356 | { 357 | 358 | cloudLabel[ind] = -1; 359 | surfPointsFlat.push_back(laserCloud->points[ind]); 360 | 361 | smallestPickedNum++; 362 | if (smallestPickedNum >= 4) 363 | { 364 | break; 365 | } 366 | 367 | cloudNeighborPicked[ind] = 1; 368 | for (int l = 1; l <= 5; l++) 369 | { 370 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x; 371 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y; 372 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z; 373 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 374 | { 375 | break; 376 | } 377 | 378 | cloudNeighborPicked[ind + l] = 1; 379 | } 380 | for (int l = -1; l >= -5; l--) 381 | { 382 | float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x; 383 | float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y; 384 | float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z; 385 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 386 | { 387 | break; 388 | } 389 | 390 | cloudNeighborPicked[ind + l] = 1; 391 | } 392 | } 393 | } 394 | 395 | for (int k = sp; k <= ep; k++) 396 | { 397 | if (cloudLabel[k] <= 0) 398 | { 399 | surfPointsLessFlatScan->push_back(laserCloud->points[k]); 400 | } 401 | } 402 | } 403 | 404 | pcl::PointCloud surfPointsLessFlatScanDS; 405 | pcl::VoxelGrid downSizeFilter; 406 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 407 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 408 | downSizeFilter.filter(surfPointsLessFlatScanDS); 409 | 410 | surfPointsLessFlat += surfPointsLessFlatScanDS; 411 | } 412 | printf("sort q time %f \n", t_q_sort); 413 | printf("seperate points time %f \n", t_pts.toc()); 414 | 415 | 416 | sensor_msgs::PointCloud2 laserCloudOutMsg; 417 | pcl::toROSMsg(*laserCloud, laserCloudOutMsg); 418 | laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; 419 | laserCloudOutMsg.header.frame_id = "/velo_link"; 420 | pubLaserCloud.publish(laserCloudOutMsg); 421 | 422 | sensor_msgs::PointCloud2 cornerPointsSharpMsg; 423 | pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg); 424 | cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp; 425 | cornerPointsSharpMsg.header.frame_id = "/velo_link"; 426 | pubCornerPointsSharp.publish(cornerPointsSharpMsg); 427 | 428 | sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; 429 | pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg); 430 | cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp; 431 | cornerPointsLessSharpMsg.header.frame_id = "/velo_link"; 432 | pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); 433 | 434 | sensor_msgs::PointCloud2 surfPointsFlat2; 435 | pcl::toROSMsg(surfPointsFlat, surfPointsFlat2); 436 | surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp; 437 | surfPointsFlat2.header.frame_id = "/velo_link"; 438 | pubSurfPointsFlat.publish(surfPointsFlat2); 439 | 440 | sensor_msgs::PointCloud2 surfPointsLessFlat2; 441 | pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2); 442 | surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp; 443 | surfPointsLessFlat2.header.frame_id = "/velo_link"; 444 | pubSurfPointsLessFlat.publish(surfPointsLessFlat2); 445 | 446 | // pub each scam 447 | if(PUB_EACH_LINE) 448 | { 449 | for(int i = 0; i< N_SCANS; i++) 450 | { 451 | sensor_msgs::PointCloud2 scanMsg; 452 | pcl::toROSMsg(laserCloudScans[i], scanMsg); 453 | scanMsg.header.stamp = laserCloudMsg->header.stamp; 454 | scanMsg.header.frame_id = "/velo_link"; 455 | pubEachScan[i].publish(scanMsg); 456 | } 457 | } 458 | 459 | printf("scan registration time %f ms *************\n", t_whole.toc()); 460 | if(t_whole.toc() > 100) 461 | ROS_WARN("scan registration process over 100ms"); 462 | } 463 | 464 | int main(int argc, char **argv) 465 | { 466 | ros::init(argc, argv, "aloam_scan_registration_node"); 467 | ros::NodeHandle nh; 468 | 469 | nh.param("scan_line", N_SCANS, 16); 470 | 471 | nh.param("minimum_range", MINIMUM_RANGE, 0.1); 472 | 473 | printf("scan line number %d \n", N_SCANS); 474 | 475 | if( N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64 ) 476 | { 477 | printf("only support velodyne with 16, 32 or 64 scan line!"); 478 | return EXIT_SUCCESS; 479 | } 480 | 481 | ros::Subscriber subLaserCloud = nh.subscribe("/kitti/velo/pointcloud", 100, laserCloudHandler); 482 | 483 | pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100); 484 | 485 | pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100); 486 | 487 | pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100); 488 | 489 | pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100); 490 | 491 | pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100); 492 | 493 | pubRemovePoints = nh.advertise("/laser_remove_points", 100); 494 | 495 | if(PUB_EACH_LINE) 496 | { 497 | for(int i = 0; i < N_SCANS; i++) 498 | { 499 | ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100); 500 | pubEachScan.push_back(tmp); 501 | } 502 | } 503 | 504 | ros::spin(); 505 | 506 | return EXIT_SUCCESS; 507 | } 508 | -------------------------------------------------------------------------------- /src/lidar_localization/rviz/front_end.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /GT1 10 | - /GT1/gtOdometry1 11 | - /GT1/gtOdometry1/Shape1 12 | - /odometry1 13 | - /odometry1/odomPath1 14 | - /odometry1/PointCloud21 15 | - /scan1/raw_data1 16 | - /scan1/raw_data1/Status1 17 | - /Image1 18 | Splitter Ratio: 0.5 19 | Tree Height: 517 20 | - Class: rviz/Selection 21 | Name: Selection 22 | - Class: rviz/Tool Properties 23 | Expanded: 24 | - /2D Pose Estimate1 25 | - /2D Nav Goal1 26 | - /Publish Point1 27 | Name: Tool Properties 28 | Splitter Ratio: 0.5886790156364441 29 | - Class: rviz/Views 30 | Expanded: 31 | - /Current View1 32 | Name: Views 33 | Splitter Ratio: 0.5 34 | - Class: rviz/Time 35 | Experimental: false 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: Image 39 | Preferences: 40 | PromptSaveOnExit: true 41 | Toolbars: 42 | toolButtonStyle: 2 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 0.5 47 | Cell Size: 1 48 | Class: rviz/Grid 49 | Color: 160; 160; 164 50 | Enabled: false 51 | Line Style: 52 | Line Width: 0.029999999329447746 53 | Value: Lines 54 | Name: Grid 55 | Normal Cell Count: 0 56 | Offset: 57 | X: 0 58 | Y: 0 59 | Z: 0 60 | Plane: XY 61 | Plane Cell Count: 160 62 | Reference Frame: 63 | Value: false 64 | - Class: rviz/Axes 65 | Enabled: true 66 | Length: 1 67 | Name: Axes 68 | Radius: 0.10000000149011612 69 | Reference Frame: 70 | Value: true 71 | - Class: rviz/Group 72 | Displays: 73 | - Angle Tolerance: 0.10000000149011612 74 | Class: rviz/Odometry 75 | Covariance: 76 | Orientation: 77 | Alpha: 0.5 78 | Color: 255; 255; 127 79 | Color Style: Unique 80 | Frame: Local 81 | Offset: 1 82 | Scale: 1 83 | Value: true 84 | Position: 85 | Alpha: 0.30000001192092896 86 | Color: 204; 51; 204 87 | Scale: 1 88 | Value: true 89 | Value: true 90 | Enabled: true 91 | Keep: 0 92 | Name: gtOdometry 93 | Position Tolerance: 0.10000000149011612 94 | Shape: 95 | Alpha: 1 96 | Axes Length: 1 97 | Axes Radius: 0.10000000149011612 98 | Color: 25; 255; 0 99 | Head Length: 0.30000001192092896 100 | Head Radius: 0.10000000149011612 101 | Shaft Length: 1 102 | Shaft Radius: 0.05000000074505806 103 | Value: Arrow 104 | Topic: /gnss_odom 105 | Unreliable: false 106 | Value: true 107 | Enabled: true 108 | Name: GT 109 | - Class: rviz/Group 110 | Displays: 111 | - Alpha: 1 112 | Buffer Length: 1 113 | Class: rviz/Path 114 | Color: 255; 85; 0 115 | Enabled: true 116 | Head Diameter: 0.30000001192092896 117 | Head Length: 0.20000000298023224 118 | Length: 0.30000001192092896 119 | Line Style: Lines 120 | Line Width: 0.10000000149011612 121 | Name: odomPath 122 | Offset: 123 | X: 0 124 | Y: 0 125 | Z: 0 126 | Pose Color: 255; 85; 255 127 | Pose Style: None 128 | Radius: 0.029999999329447746 129 | Shaft Diameter: 0.10000000149011612 130 | Shaft Length: 0.10000000149011612 131 | Topic: /laser_odom_scan_to_map_path 132 | Unreliable: false 133 | Value: true 134 | - Alpha: 1 135 | Autocompute Intensity Bounds: true 136 | Autocompute Value Bounds: 137 | Max Value: 10 138 | Min Value: -10 139 | Value: true 140 | Axis: Z 141 | Channel Name: intensity 142 | Class: rviz/PointCloud2 143 | Color: 255; 255; 255 144 | Color Transformer: FlatColor 145 | Decay Time: 0 146 | Enabled: false 147 | Invert Rainbow: false 148 | Max Color: 255; 255; 255 149 | Max Intensity: 63.10047912597656 150 | Min Color: 0; 0; 0 151 | Min Intensity: -0.0067862942814826965 152 | Name: PointCloud2 153 | Position Transformer: XYZ 154 | Queue Size: 10 155 | Selectable: true 156 | Size (Pixels): 3 157 | Size (m): 0.05000000074505806 158 | Style: Flat Squares 159 | Topic: /velodyne_cloud_2 160 | Unreliable: false 161 | Use Fixed Frame: true 162 | Use rainbow: true 163 | Value: false 164 | Enabled: true 165 | Name: odometry 166 | - Class: rviz/Group 167 | Displays: 168 | - Alpha: 1 169 | Autocompute Intensity Bounds: true 170 | Autocompute Value Bounds: 171 | Max Value: 10 172 | Min Value: -10 173 | Value: true 174 | Axis: Z 175 | Channel Name: intensity 176 | Class: rviz/PointCloud2 177 | Color: 255; 255; 255 178 | Color Transformer: FlatColor 179 | Decay Time: 0 180 | Enabled: true 181 | Invert Rainbow: false 182 | Max Color: 255; 255; 255 183 | Max Intensity: -999999 184 | Min Color: 0; 0; 0 185 | Min Intensity: 999999 186 | Name: allMapCloud 187 | Position Transformer: XYZ 188 | Queue Size: 10 189 | Selectable: true 190 | Size (Pixels): 3 191 | Size (m): 0.05000000074505806 192 | Style: Flat Squares 193 | Topic: /laser_cloud_map 194 | Unreliable: false 195 | Use Fixed Frame: true 196 | Use rainbow: true 197 | Value: true 198 | - Alpha: 1 199 | Autocompute Intensity Bounds: true 200 | Autocompute Value Bounds: 201 | Max Value: 10 202 | Min Value: -10 203 | Value: true 204 | Axis: Z 205 | Channel Name: intensity 206 | Class: rviz/PointCloud2 207 | Color: 255; 255; 255 208 | Color Transformer: FlatColor 209 | Decay Time: 0 210 | Enabled: true 211 | Invert Rainbow: false 212 | Max Color: 255; 255; 255 213 | Max Intensity: 15 214 | Min Color: 0; 0; 0 215 | Min Intensity: 0 216 | Name: surround 217 | Position Transformer: XYZ 218 | Queue Size: 1 219 | Selectable: true 220 | Size (Pixels): 1 221 | Size (m): 0.05000000074505806 222 | Style: Squares 223 | Topic: /laser_cloud_surround 224 | Unreliable: false 225 | Use Fixed Frame: true 226 | Use rainbow: true 227 | Value: true 228 | - Alpha: 1 229 | Autocompute Intensity Bounds: true 230 | Autocompute Value Bounds: 231 | Max Value: 10 232 | Min Value: -10 233 | Value: true 234 | Axis: Z 235 | Channel Name: intensity 236 | Class: rviz/PointCloud2 237 | Color: 255; 255; 255 238 | Color Transformer: Intensity 239 | Decay Time: 0 240 | Enabled: true 241 | Invert Rainbow: false 242 | Max Color: 255; 255; 255 243 | Max Intensity: 50.117347717285156 244 | Min Color: 0; 0; 0 245 | Min Intensity: -0.01879953220486641 246 | Name: currPoints 247 | Position Transformer: XYZ 248 | Queue Size: 10 249 | Selectable: true 250 | Size (Pixels): 2 251 | Size (m): 0.009999999776482582 252 | Style: Points 253 | Topic: /velodyne_cloud_registered 254 | Unreliable: false 255 | Use Fixed Frame: true 256 | Use rainbow: true 257 | Value: true 258 | - Alpha: 1 259 | Autocompute Intensity Bounds: true 260 | Autocompute Value Bounds: 261 | Max Value: 10 262 | Min Value: -10 263 | Value: true 264 | Axis: Z 265 | Channel Name: intensity 266 | Class: rviz/PointCloud2 267 | Color: 255; 0; 0 268 | Color Transformer: FlatColor 269 | Decay Time: 0 270 | Enabled: false 271 | Invert Rainbow: false 272 | Max Color: 255; 255; 255 273 | Max Intensity: 63.03843307495117 274 | Min Color: 255; 0; 0 275 | Min Intensity: -0.006431261543184519 276 | Name: corner 277 | Position Transformer: XYZ 278 | Queue Size: 10 279 | Selectable: true 280 | Size (Pixels): 3 281 | Size (m): 0.20000000298023224 282 | Style: Flat Squares 283 | Topic: /mapping_corner 284 | Unreliable: false 285 | Use Fixed Frame: true 286 | Use rainbow: true 287 | Value: false 288 | - Alpha: 1 289 | Autocompute Intensity Bounds: true 290 | Autocompute Value Bounds: 291 | Max Value: 10 292 | Min Value: -10 293 | Value: true 294 | Axis: Z 295 | Channel Name: intensity 296 | Class: rviz/PointCloud2 297 | Color: 255; 0; 0 298 | Color Transformer: FlatColor 299 | Decay Time: 0 300 | Enabled: false 301 | Invert Rainbow: false 302 | Max Color: 255; 255; 255 303 | Max Intensity: 63.0818977355957 304 | Min Color: 0; 0; 0 305 | Min Intensity: -0.005241604056209326 306 | Name: surf 307 | Position Transformer: XYZ 308 | Queue Size: 10 309 | Selectable: true 310 | Size (Pixels): 3 311 | Size (m): 0.20000000298023224 312 | Style: Flat Squares 313 | Topic: /mapping_surf 314 | Unreliable: false 315 | Use Fixed Frame: true 316 | Use rainbow: true 317 | Value: false 318 | - Alpha: 1 319 | Autocompute Intensity Bounds: true 320 | Autocompute Value Bounds: 321 | Max Value: 10 322 | Min Value: -10 323 | Value: true 324 | Axis: Z 325 | Channel Name: intensity 326 | Class: rviz/PointCloud2 327 | Color: 0; 255; 0 328 | Color Transformer: FlatColor 329 | Decay Time: 0 330 | Enabled: false 331 | Invert Rainbow: false 332 | Max Color: 255; 255; 255 333 | Max Intensity: 17.08800506591797 334 | Min Color: 0; 0; 0 335 | Min Intensity: -0.006790489889681339 336 | Name: used_corner 337 | Position Transformer: XYZ 338 | Queue Size: 10 339 | Selectable: true 340 | Size (Pixels): 3 341 | Size (m): 0.5 342 | Style: Flat Squares 343 | Topic: /mapping_used_corner 344 | Unreliable: false 345 | Use Fixed Frame: true 346 | Use rainbow: true 347 | Value: false 348 | - Alpha: 1 349 | Autocompute Intensity Bounds: true 350 | Autocompute Value Bounds: 351 | Max Value: 10 352 | Min Value: -10 353 | Value: true 354 | Axis: Z 355 | Channel Name: intensity 356 | Class: rviz/PointCloud2 357 | Color: 0; 255; 0 358 | Color Transformer: FlatColor 359 | Decay Time: 0 360 | Enabled: false 361 | Invert Rainbow: false 362 | Max Color: 0; 255; 0 363 | Max Intensity: 63.0818977355957 364 | Min Color: 0; 0; 0 365 | Min Intensity: -0.005241604056209326 366 | Name: used_surf 367 | Position Transformer: XYZ 368 | Queue Size: 10 369 | Selectable: true 370 | Size (Pixels): 3 371 | Size (m): 0.5 372 | Style: Flat Squares 373 | Topic: /mapping_used_surf 374 | Unreliable: false 375 | Use Fixed Frame: true 376 | Use rainbow: true 377 | Value: false 378 | - Alpha: 1 379 | Autocompute Intensity Bounds: true 380 | Autocompute Value Bounds: 381 | Max Value: 10 382 | Min Value: -10 383 | Value: true 384 | Axis: Z 385 | Channel Name: intensity 386 | Class: rviz/PointCloud2 387 | Color: 255; 255; 255 388 | Color Transformer: FlatColor 389 | Decay Time: 0 390 | Enabled: false 391 | Invert Rainbow: false 392 | Max Color: 255; 255; 255 393 | Max Intensity: 21.062847137451172 394 | Min Color: 0; 0; 0 395 | Min Intensity: -0.006625724025070667 396 | Name: map_corner 397 | Position Transformer: XYZ 398 | Queue Size: 10 399 | Selectable: true 400 | Size (Pixels): 3 401 | Size (m): 0.10000000149011612 402 | Style: Flat Squares 403 | Topic: /mapping_map_corner 404 | Unreliable: false 405 | Use Fixed Frame: true 406 | Use rainbow: true 407 | Value: false 408 | - Alpha: 1 409 | Autocompute Intensity Bounds: true 410 | Autocompute Value Bounds: 411 | Max Value: 10 412 | Min Value: -10 413 | Value: true 414 | Axis: Z 415 | Channel Name: intensity 416 | Class: rviz/PointCloud2 417 | Color: 255; 255; 255 418 | Color Transformer: FlatColor 419 | Decay Time: 0 420 | Enabled: false 421 | Invert Rainbow: false 422 | Max Color: 255; 255; 255 423 | Max Intensity: 63.08392333984375 424 | Min Color: 0; 0; 0 425 | Min Intensity: -0.00876065157353878 426 | Name: map_surf 427 | Position Transformer: XYZ 428 | Queue Size: 10 429 | Selectable: true 430 | Size (Pixels): 3 431 | Size (m): 0.10000000149011612 432 | Style: Flat Squares 433 | Topic: /mapping_map_surf 434 | Unreliable: false 435 | Use Fixed Frame: true 436 | Use rainbow: true 437 | Value: false 438 | - Alpha: 1 439 | Buffer Length: 1 440 | Class: rviz/Path 441 | Color: 25; 255; 0 442 | Enabled: false 443 | Head Diameter: 0.30000001192092896 444 | Head Length: 0.20000000298023224 445 | Length: 0.30000001192092896 446 | Line Style: Lines 447 | Line Width: 0.029999999329447746 448 | Name: leftcameraPath 449 | Offset: 450 | X: 0 451 | Y: 0 452 | Z: 0 453 | Pose Color: 255; 85; 255 454 | Pose Style: None 455 | Radius: 0.029999999329447746 456 | Shaft Diameter: 0.10000000149011612 457 | Shaft Length: 0.10000000149011612 458 | Topic: /lc_path 459 | Unreliable: false 460 | Value: false 461 | Enabled: true 462 | Name: mapping 463 | - Class: rviz/Group 464 | Displays: 465 | - Alpha: 1 466 | Autocompute Intensity Bounds: true 467 | Autocompute Value Bounds: 468 | Max Value: 10 469 | Min Value: -10 470 | Value: true 471 | Axis: Z 472 | Channel Name: intensity 473 | Class: rviz/PointCloud2 474 | Color: 255; 255; 255 475 | Color Transformer: FlatColor 476 | Decay Time: 0 477 | Enabled: false 478 | Invert Rainbow: false 479 | Max Color: 255; 255; 255 480 | Max Intensity: 15 481 | Min Color: 0; 0; 0 482 | Min Intensity: 0 483 | Name: sharp 484 | Position Transformer: XYZ 485 | Queue Size: 10 486 | Selectable: true 487 | Size (Pixels): 3 488 | Size (m): 0.009999999776482582 489 | Style: Points 490 | Topic: /laser_cloud_sharp 491 | Unreliable: false 492 | Use Fixed Frame: true 493 | Use rainbow: true 494 | Value: false 495 | - Alpha: 1 496 | Autocompute Intensity Bounds: true 497 | Autocompute Value Bounds: 498 | Max Value: 10 499 | Min Value: -10 500 | Value: true 501 | Axis: Z 502 | Channel Name: intensity 503 | Class: rviz/PointCloud2 504 | Color: 255; 255; 255 505 | Color Transformer: Intensity 506 | Decay Time: 0 507 | Enabled: false 508 | Invert Rainbow: false 509 | Max Color: 255; 255; 255 510 | Max Intensity: 63.03987121582031 511 | Min Color: 0; 0; 0 512 | Min Intensity: 0.0263746976852417 513 | Name: flat 514 | Position Transformer: XYZ 515 | Queue Size: 10 516 | Selectable: true 517 | Size (Pixels): 3 518 | Size (m): 0.05000000074505806 519 | Style: Flat Squares 520 | Topic: /laser_cloud_flat 521 | Unreliable: false 522 | Use Fixed Frame: true 523 | Use rainbow: true 524 | Value: false 525 | - Alpha: 1 526 | Autocompute Intensity Bounds: true 527 | Autocompute Value Bounds: 528 | Max Value: 10 529 | Min Value: -10 530 | Value: true 531 | Axis: Z 532 | Channel Name: intensity 533 | Class: rviz/PointCloud2 534 | Color: 255; 255; 255 535 | Color Transformer: FlatColor 536 | Decay Time: 0 537 | Enabled: true 538 | Invert Rainbow: false 539 | Max Color: 255; 255; 255 540 | Max Intensity: 0.9900000095367432 541 | Min Color: 0; 0; 0 542 | Min Intensity: 0 543 | Name: raw_data 544 | Position Transformer: XYZ 545 | Queue Size: 10 546 | Selectable: true 547 | Size (Pixels): 3 548 | Size (m): 0.05000000074505806 549 | Style: Flat Squares 550 | Topic: /kitti/velo/pointcloud 551 | Unreliable: false 552 | Use Fixed Frame: true 553 | Use rainbow: true 554 | Value: true 555 | - Alpha: 1 556 | Autocompute Intensity Bounds: true 557 | Autocompute Value Bounds: 558 | Max Value: 10 559 | Min Value: -10 560 | Value: true 561 | Axis: Z 562 | Channel Name: intensity 563 | Class: rviz/PointCloud2 564 | Color: 255; 0; 0 565 | Color Transformer: FlatColor 566 | Decay Time: 0 567 | Enabled: false 568 | Invert Rainbow: false 569 | Max Color: 255; 255; 255 570 | Max Intensity: 0.12361729145050049 571 | Min Color: 0; 0; 0 572 | Min Intensity: -0.00711920578032732 573 | Name: scan 574 | Position Transformer: XYZ 575 | Queue Size: 10 576 | Selectable: true 577 | Size (Pixels): 3 578 | Size (m): 0.20000000298023224 579 | Style: Flat Squares 580 | Topic: /laser_scanid_63 581 | Unreliable: false 582 | Use Fixed Frame: true 583 | Use rainbow: true 584 | Value: false 585 | - Alpha: 1 586 | Autocompute Intensity Bounds: true 587 | Autocompute Value Bounds: 588 | Max Value: 10 589 | Min Value: -10 590 | Value: true 591 | Axis: Z 592 | Channel Name: intensity 593 | Class: rviz/PointCloud2 594 | Color: 255; 85; 0 595 | Color Transformer: FlatColor 596 | Decay Time: 0 597 | Enabled: false 598 | Invert Rainbow: false 599 | Max Color: 255; 255; 255 600 | Max Intensity: 62.07567596435547 601 | Min Color: 0; 0; 0 602 | Min Intensity: -0.007054195739328861 603 | Name: removPoints 604 | Position Transformer: XYZ 605 | Queue Size: 10 606 | Selectable: true 607 | Size (Pixels): 3 608 | Size (m): 0.20000000298023224 609 | Style: Flat Squares 610 | Topic: /laser_remove_points 611 | Unreliable: false 612 | Use Fixed Frame: true 613 | Use rainbow: true 614 | Value: false 615 | Enabled: true 616 | Name: scan 617 | - Class: rviz/Image 618 | Enabled: true 619 | Image Topic: /kitti/camera_color_left/image_raw 620 | Max Value: 1 621 | Median window: 5 622 | Min Value: 0 623 | Name: Image 624 | Normalize Range: true 625 | Queue Size: 2 626 | Transport Hint: raw 627 | Unreliable: false 628 | Value: true 629 | - Class: rviz/TF 630 | Enabled: true 631 | Frame Timeout: 15 632 | Frames: 633 | All Enabled: true 634 | base_link: 635 | Value: true 636 | camera_color_left: 637 | Value: true 638 | camera_color_right: 639 | Value: true 640 | camera_gray_left: 641 | Value: true 642 | camera_gray_right: 643 | Value: true 644 | imu_link: 645 | Value: true 646 | map: 647 | Value: true 648 | velo_link: 649 | Value: true 650 | world: 651 | Value: true 652 | Marker Scale: 1 653 | Name: TF 654 | Show Arrows: true 655 | Show Axes: true 656 | Show Names: true 657 | Tree: 658 | map: 659 | velo_link: 660 | {} 661 | Update Interval: 0 662 | Value: true 663 | Enabled: true 664 | Global Options: 665 | Background Color: 0; 0; 0 666 | Default Light: true 667 | Fixed Frame: map 668 | Frame Rate: 30 669 | Name: root 670 | Tools: 671 | - Class: rviz/Interact 672 | Hide Inactive Objects: true 673 | - Class: rviz/MoveCamera 674 | - Class: rviz/Select 675 | - Class: rviz/FocusCamera 676 | - Class: rviz/Measure 677 | - Class: rviz/SetInitialPose 678 | Theta std deviation: 0.2617993950843811 679 | Topic: /initialpose 680 | X std deviation: 0.5 681 | Y std deviation: 0.5 682 | - Class: rviz/SetGoal 683 | Topic: /move_base_simple/goal 684 | - Class: rviz/PublishPoint 685 | Single click: true 686 | Topic: /clicked_point 687 | Value: true 688 | Views: 689 | Current: 690 | Angle: 0 691 | Class: rviz/TopDownOrtho 692 | Enable Stereo Rendering: 693 | Stereo Eye Separation: 0.05999999865889549 694 | Stereo Focal Distance: 1 695 | Swap Stereo Eyes: false 696 | Value: false 697 | Invert Z Axis: false 698 | Name: Current View 699 | Near Clip Distance: 0.009999999776482582 700 | Scale: 2.7374796867370605 701 | Target Frame: velo_link 702 | Value: TopDownOrtho (rviz) 703 | X: -20.822071075439453 704 | Y: -18.995574951171875 705 | Saved: ~ 706 | Window Geometry: 707 | Displays: 708 | collapsed: false 709 | Height: 1007 710 | Hide Left Dock: false 711 | Hide Right Dock: false 712 | Image: 713 | collapsed: false 714 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000350fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000290000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002d4000000ba0000001600ffffff000000010000010f00000350fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e00000350000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000080a0000003efc0100000002fb0000000800540069006d006501000000000000080a0000026c00fffffffb0000000800540069006d00650100000000000004500000000000000000000005850000035000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 715 | Selection: 716 | collapsed: false 717 | Time: 718 | collapsed: false 719 | Tool Properties: 720 | collapsed: false 721 | Views: 722 | collapsed: false 723 | Width: 2058 724 | X: 0 725 | Y: 0 726 | -------------------------------------------------------------------------------- /src/lidar_localization/src/aloam_laser_odometry_node.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include "lidar_localization/utils/common.h" 56 | #include "lidar_localization/utils/tic_toc.h" 57 | 58 | #include "lidar_localization/models/loam/aloam_factor.hpp" 59 | 60 | #define DISTORTION 0 61 | 62 | 63 | int corner_correspondence = 0, plane_correspondence = 0; 64 | 65 | constexpr double SCAN_PERIOD = 0.1; 66 | constexpr double DISTANCE_SQ_THRESHOLD = 25; 67 | constexpr double NEARBY_SCAN = 2.5; 68 | 69 | int skipFrameNum = 5; 70 | bool systemInited = false; 71 | 72 | double timeCornerPointsSharp = 0; 73 | double timeCornerPointsLessSharp = 0; 74 | double timeSurfPointsFlat = 0; 75 | double timeSurfPointsLessFlat = 0; 76 | double timeLaserCloudFullRes = 0; 77 | 78 | pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN()); 79 | pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN()); 80 | 81 | pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); 82 | pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); 83 | pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); 84 | pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); 85 | 86 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); 87 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); 88 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); 89 | 90 | int laserCloudCornerLastNum = 0; 91 | int laserCloudSurfLastNum = 0; 92 | 93 | // Transformation from current frame to world frame 94 | Eigen::Quaterniond q_w_curr(1, 0, 0, 0); 95 | Eigen::Vector3d t_w_curr(0, 0, 0); 96 | 97 | // q_curr_last(x, y, z, w), t_curr_last 98 | // double para_q[4] = {0, 0, 0, 1}; 99 | // double para_t[3] = {0, 0, 0}; 100 | // Eigen::Map q_last_curr(para_q); 101 | // Eigen::Map t_last_curr(para_t); 102 | 103 | 104 | 105 | // q_curr_last(x, y, z, w), t_curr_last 106 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; 107 | Eigen::Map q_last_curr(parameters); 108 | Eigen::Map t_last_curr(parameters+4); 109 | 110 | 111 | 112 | std::queue cornerSharpBuf; 113 | std::queue cornerLessSharpBuf; 114 | std::queue surfFlatBuf; 115 | std::queue surfLessFlatBuf; 116 | std::queue fullPointsBuf; 117 | std::mutex mBuf; 118 | 119 | // undistort lidar point 120 | void TransformToStart(PointType const *const pi, PointType *const po) 121 | { 122 | //interpolation ratio 123 | double s; 124 | if (DISTORTION) 125 | s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; 126 | else 127 | s = 1.0; 128 | //s = 1; 129 | Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr); 130 | Eigen::Vector3d t_point_last = s * t_last_curr; 131 | Eigen::Vector3d point(pi->x, pi->y, pi->z); 132 | Eigen::Vector3d un_point = q_point_last * point + t_point_last; 133 | 134 | po->x = un_point.x(); 135 | po->y = un_point.y(); 136 | po->z = un_point.z(); 137 | po->intensity = pi->intensity; 138 | } 139 | 140 | // transform all lidar points to the start of the next frame 141 | 142 | void TransformToEnd(PointType const *const pi, PointType *const po) 143 | { 144 | // undistort point first 145 | pcl::PointXYZI un_point_tmp; 146 | TransformToStart(pi, &un_point_tmp); 147 | 148 | Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z); 149 | Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr); 150 | 151 | po->x = point_end.x(); 152 | po->y = point_end.y(); 153 | po->z = point_end.z(); 154 | 155 | //Remove distortion time info 156 | po->intensity = int(pi->intensity); 157 | } 158 | 159 | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2) 160 | { 161 | mBuf.lock(); 162 | cornerSharpBuf.push(cornerPointsSharp2); 163 | mBuf.unlock(); 164 | } 165 | 166 | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2) 167 | { 168 | mBuf.lock(); 169 | cornerLessSharpBuf.push(cornerPointsLessSharp2); 170 | mBuf.unlock(); 171 | } 172 | 173 | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2) 174 | { 175 | mBuf.lock(); 176 | surfFlatBuf.push(surfPointsFlat2); 177 | mBuf.unlock(); 178 | } 179 | 180 | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2) 181 | { 182 | mBuf.lock(); 183 | surfLessFlatBuf.push(surfPointsLessFlat2); 184 | mBuf.unlock(); 185 | } 186 | 187 | //receive all point cloud 188 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) 189 | { 190 | mBuf.lock(); 191 | fullPointsBuf.push(laserCloudFullRes2); 192 | mBuf.unlock(); 193 | } 194 | 195 | int main(int argc, char **argv) 196 | { 197 | ros::init(argc, argv, "aloam_laser_odometry_node"); 198 | ros::NodeHandle nh; 199 | 200 | nh.param("mapping_skip_frame", skipFrameNum, 2); 201 | 202 | printf("Mapping %d Hz \n", 10 / skipFrameNum); 203 | 204 | ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler); 205 | 206 | ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler); 207 | 208 | ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler); 209 | 210 | ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler); 211 | 212 | ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler); 213 | 214 | ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100); 215 | 216 | ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100); 217 | 218 | ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100); 219 | 220 | ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_scan_to_scan", 100); 221 | 222 | ros::Publisher pubLaserPath = nh.advertise("/laser_odom_scan_to_scan_path", 100); 223 | 224 | nav_msgs::Path laserPath; 225 | 226 | int frameCount = 0; 227 | ros::Rate rate(100); 228 | 229 | while (ros::ok()) 230 | { 231 | ros::spinOnce(); 232 | 233 | if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() && 234 | !surfFlatBuf.empty() && !surfLessFlatBuf.empty() && 235 | !fullPointsBuf.empty()) 236 | { 237 | timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec(); 238 | timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec(); 239 | timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec(); 240 | timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec(); 241 | timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec(); 242 | 243 | if (timeCornerPointsSharp != timeLaserCloudFullRes || 244 | timeCornerPointsLessSharp != timeLaserCloudFullRes || 245 | timeSurfPointsFlat != timeLaserCloudFullRes || 246 | timeSurfPointsLessFlat != timeLaserCloudFullRes) 247 | { 248 | printf("unsync messeage!"); 249 | ROS_BREAK(); 250 | } 251 | 252 | mBuf.lock(); 253 | cornerPointsSharp->clear(); 254 | pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp); 255 | cornerSharpBuf.pop(); 256 | 257 | cornerPointsLessSharp->clear(); 258 | pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp); 259 | cornerLessSharpBuf.pop(); 260 | 261 | surfPointsFlat->clear(); 262 | pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat); 263 | surfFlatBuf.pop(); 264 | 265 | surfPointsLessFlat->clear(); 266 | pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat); 267 | surfLessFlatBuf.pop(); 268 | 269 | laserCloudFullRes->clear(); 270 | pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes); 271 | fullPointsBuf.pop(); 272 | mBuf.unlock(); 273 | 274 | TicToc t_whole; 275 | // initializing 276 | if (!systemInited) 277 | { 278 | systemInited = true; 279 | std::cout << "Initialization finished \n"; 280 | } 281 | else 282 | { 283 | int cornerPointsSharpNum = cornerPointsSharp->points.size(); 284 | int surfPointsFlatNum = surfPointsFlat->points.size(); 285 | 286 | TicToc t_opt; 287 | for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter) 288 | { 289 | corner_correspondence = 0; 290 | plane_correspondence = 0; 291 | 292 | //ceres::LossFunction *loss_function = NULL; 293 | // ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 294 | // ceres::LocalParameterization *q_parameterization = 295 | // new ceres::EigenQuaternionParameterization(); 296 | // ceres::Problem::Options problem_options; 297 | // 298 | // ceres::Problem problem(problem_options); 299 | // problem.AddParameterBlock(para_q, 4, q_parameterization); 300 | // problem.AddParameterBlock(para_t, 3); 301 | 302 | 303 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 304 | ceres::Problem::Options problem_options; 305 | ceres::Problem problem(problem_options); 306 | problem.AddParameterBlock(parameters,7,new PoseSE3Parameterization()); 307 | 308 | pcl::PointXYZI pointSel; 309 | std::vector pointSearchInd; 310 | std::vector pointSearchSqDis; 311 | 312 | TicToc t_data; 313 | // find correspondence for corner features 314 | for (int i = 0; i < cornerPointsSharpNum; ++i) 315 | { 316 | TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); 317 | kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 318 | 319 | int closestPointInd = -1, minPointInd2 = -1; 320 | if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) 321 | { 322 | closestPointInd = pointSearchInd[0]; 323 | int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); 324 | 325 | double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; 326 | // search in the direction of increasing scan line 327 | for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j) 328 | { 329 | // if in the same scan line, continue 330 | if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) 331 | continue; 332 | 333 | // if not in nearby scans, end the loop 334 | if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) 335 | break; 336 | 337 | double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 338 | (laserCloudCornerLast->points[j].x - pointSel.x) + 339 | (laserCloudCornerLast->points[j].y - pointSel.y) * 340 | (laserCloudCornerLast->points[j].y - pointSel.y) + 341 | (laserCloudCornerLast->points[j].z - pointSel.z) * 342 | (laserCloudCornerLast->points[j].z - pointSel.z); 343 | 344 | if (pointSqDis < minPointSqDis2) 345 | { 346 | // find nearer point 347 | minPointSqDis2 = pointSqDis; 348 | minPointInd2 = j; 349 | } 350 | } 351 | 352 | // search in the direction of decreasing scan line 353 | for (int j = closestPointInd - 1; j >= 0; --j) 354 | { 355 | // if in the same scan line, continue 356 | if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID) 357 | continue; 358 | 359 | // if not in nearby scans, end the loop 360 | if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) 361 | break; 362 | 363 | double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 364 | (laserCloudCornerLast->points[j].x - pointSel.x) + 365 | (laserCloudCornerLast->points[j].y - pointSel.y) * 366 | (laserCloudCornerLast->points[j].y - pointSel.y) + 367 | (laserCloudCornerLast->points[j].z - pointSel.z) * 368 | (laserCloudCornerLast->points[j].z - pointSel.z); 369 | 370 | if (pointSqDis < minPointSqDis2) 371 | { 372 | // find nearer point 373 | minPointSqDis2 = pointSqDis; 374 | minPointInd2 = j; 375 | } 376 | } 377 | } 378 | if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid 379 | { 380 | Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, 381 | cornerPointsSharp->points[i].y, 382 | cornerPointsSharp->points[i].z); 383 | Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, 384 | laserCloudCornerLast->points[closestPointInd].y, 385 | laserCloudCornerLast->points[closestPointInd].z); 386 | Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, 387 | laserCloudCornerLast->points[minPointInd2].y, 388 | laserCloudCornerLast->points[minPointInd2].z); 389 | 390 | // double s; 391 | // if (DISTORTION) 392 | // s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD; 393 | // else 394 | // s = 1.0; 395 | // ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s); 396 | // problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 397 | // corner_correspondence++; 398 | 399 | ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, last_point_a, last_point_b); 400 | problem.AddResidualBlock(cost_function, loss_function, parameters); 401 | corner_correspondence++; 402 | 403 | } 404 | } 405 | 406 | // find correspondence for plane features 407 | for (int i = 0; i < surfPointsFlatNum; ++i) 408 | { 409 | TransformToStart(&(surfPointsFlat->points[i]), &pointSel); 410 | kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 411 | 412 | int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; 413 | if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) 414 | { 415 | closestPointInd = pointSearchInd[0]; 416 | 417 | // get closest point's scan ID 418 | int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity); 419 | double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD; 420 | 421 | // search in the direction of increasing scan line 422 | for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j) 423 | { 424 | // if not in nearby scans, end the loop 425 | if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) 426 | break; 427 | 428 | double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 429 | (laserCloudSurfLast->points[j].x - pointSel.x) + 430 | (laserCloudSurfLast->points[j].y - pointSel.y) * 431 | (laserCloudSurfLast->points[j].y - pointSel.y) + 432 | (laserCloudSurfLast->points[j].z - pointSel.z) * 433 | (laserCloudSurfLast->points[j].z - pointSel.z); 434 | 435 | // if in the same or lower scan line 436 | if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2) 437 | { 438 | minPointSqDis2 = pointSqDis; 439 | minPointInd2 = j; 440 | } 441 | // if in the higher scan line 442 | else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3) 443 | { 444 | minPointSqDis3 = pointSqDis; 445 | minPointInd3 = j; 446 | } 447 | } 448 | 449 | // search in the direction of decreasing scan line 450 | for (int j = closestPointInd - 1; j >= 0; --j) 451 | { 452 | // if not in nearby scans, end the loop 453 | if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) 454 | break; 455 | 456 | double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 457 | (laserCloudSurfLast->points[j].x - pointSel.x) + 458 | (laserCloudSurfLast->points[j].y - pointSel.y) * 459 | (laserCloudSurfLast->points[j].y - pointSel.y) + 460 | (laserCloudSurfLast->points[j].z - pointSel.z) * 461 | (laserCloudSurfLast->points[j].z - pointSel.z); 462 | 463 | // if in the same or higher scan line 464 | if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) 465 | { 466 | minPointSqDis2 = pointSqDis; 467 | minPointInd2 = j; 468 | } 469 | else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) 470 | { 471 | // find nearer point 472 | minPointSqDis3 = pointSqDis; 473 | minPointInd3 = j; 474 | } 475 | } 476 | 477 | if (minPointInd2 >= 0 && minPointInd3 >= 0) 478 | { 479 | 480 | Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, 481 | surfPointsFlat->points[i].y, 482 | surfPointsFlat->points[i].z); 483 | Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, 484 | laserCloudSurfLast->points[closestPointInd].y, 485 | laserCloudSurfLast->points[closestPointInd].z); 486 | Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, 487 | laserCloudSurfLast->points[minPointInd2].y, 488 | laserCloudSurfLast->points[minPointInd2].z); 489 | Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, 490 | laserCloudSurfLast->points[minPointInd3].y, 491 | laserCloudSurfLast->points[minPointInd3].z); 492 | 493 | // double s; 494 | // if (DISTORTION) 495 | // s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD; 496 | // else 497 | // s = 1.0; 498 | // ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s); 499 | // problem.AddResidualBlock(cost_function, loss_function, para_q, para_t); 500 | // plane_correspondence++; 501 | Eigen::Vector3d pj=last_point_a; 502 | Eigen::Vector3d pl=last_point_b; 503 | Eigen::Vector3d pm=last_point_c; 504 | 505 | Eigen::Vector3d norm = (pl-pj).cross(pm-pj)/( (pl-pj).cross(pm-pj) ).norm(); 506 | double negative_OA_dot_norm = -pj.dot(norm); 507 | 508 | ceres::CostFunction *cost_function = new SurfNormAnalyticCostFunction(curr_point, norm, negative_OA_dot_norm); 509 | problem.AddResidualBlock(cost_function, loss_function, parameters); 510 | plane_correspondence++; 511 | 512 | } 513 | } 514 | } 515 | 516 | //printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence); 517 | printf("data association time %f ms \n", t_data.toc()); 518 | 519 | if ((corner_correspondence + plane_correspondence) < 10) 520 | { 521 | printf("less correspondence! *************************************************\n"); 522 | } 523 | 524 | TicToc t_solver; 525 | ceres::Solver::Options options; 526 | options.linear_solver_type = ceres::DENSE_QR; 527 | options.max_num_iterations = 4; 528 | options.minimizer_progress_to_stdout = false; 529 | ceres::Solver::Summary summary; 530 | ceres::Solve(options, &problem, &summary); 531 | printf("solver time %f ms \n", t_solver.toc()); 532 | } 533 | printf("optimization twice time %f \n", t_opt.toc()); 534 | 535 | t_w_curr = t_w_curr + q_w_curr * t_last_curr; 536 | q_w_curr = q_w_curr * q_last_curr; 537 | } 538 | 539 | TicToc t_pub; 540 | 541 | // publish odometry, scan-to-scan matching: 542 | nav_msgs::Odometry laserOdometry; 543 | laserOdometry.header.frame_id = "/map"; 544 | laserOdometry.child_frame_id = "/velo_link"; 545 | laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 546 | laserOdometry.pose.pose.orientation.x = q_w_curr.x(); 547 | laserOdometry.pose.pose.orientation.y = q_w_curr.y(); 548 | laserOdometry.pose.pose.orientation.z = q_w_curr.z(); 549 | laserOdometry.pose.pose.orientation.w = q_w_curr.w(); 550 | laserOdometry.pose.pose.position.x = t_w_curr.x(); 551 | laserOdometry.pose.pose.position.y = t_w_curr.y(); 552 | laserOdometry.pose.pose.position.z = t_w_curr.z(); 553 | pubLaserOdometry.publish(laserOdometry); 554 | 555 | geometry_msgs::PoseStamped laserPose; 556 | laserPose.header = laserOdometry.header; 557 | laserPose.pose = laserOdometry.pose.pose; 558 | laserPath.header.stamp = laserOdometry.header.stamp; 559 | laserPath.poses.push_back(laserPose); 560 | laserPath.header.frame_id = "/map"; 561 | pubLaserPath.publish(laserPath); 562 | 563 | // transform corner features and plane features to the scan end point 564 | if (0) 565 | { 566 | int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); 567 | for (int i = 0; i < cornerPointsLessSharpNum; i++) 568 | { 569 | TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); 570 | } 571 | 572 | int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); 573 | for (int i = 0; i < surfPointsLessFlatNum; i++) 574 | { 575 | TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); 576 | } 577 | 578 | int laserCloudFullResNum = laserCloudFullRes->points.size(); 579 | for (int i = 0; i < laserCloudFullResNum; i++) 580 | { 581 | TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); 582 | } 583 | } 584 | 585 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 586 | cornerPointsLessSharp = laserCloudCornerLast; 587 | laserCloudCornerLast = laserCloudTemp; 588 | 589 | laserCloudTemp = surfPointsLessFlat; 590 | surfPointsLessFlat = laserCloudSurfLast; 591 | laserCloudSurfLast = laserCloudTemp; 592 | 593 | laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 594 | laserCloudSurfLastNum = laserCloudSurfLast->points.size(); 595 | 596 | // std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n'; 597 | 598 | kdtreeCornerLast->setInputCloud(laserCloudCornerLast); 599 | kdtreeSurfLast->setInputCloud(laserCloudSurfLast); 600 | 601 | if (frameCount % skipFrameNum == 0) 602 | { 603 | frameCount = 0; 604 | 605 | sensor_msgs::PointCloud2 laserCloudCornerLast2; 606 | pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); 607 | laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 608 | laserCloudCornerLast2.header.frame_id = "/camera"; 609 | pubLaserCloudCornerLast.publish(laserCloudCornerLast2); 610 | 611 | sensor_msgs::PointCloud2 laserCloudSurfLast2; 612 | pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); 613 | laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 614 | laserCloudSurfLast2.header.frame_id = "/camera"; 615 | pubLaserCloudSurfLast.publish(laserCloudSurfLast2); 616 | 617 | sensor_msgs::PointCloud2 laserCloudFullRes3; 618 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); 619 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 620 | laserCloudFullRes3.header.frame_id = "/camera"; 621 | pubLaserCloudFullRes.publish(laserCloudFullRes3); 622 | } 623 | printf("publication time %f ms \n", t_pub.toc()); 624 | printf("whole laserOdometry time %f ms \n \n", t_whole.toc()); 625 | if(t_whole.toc() > 100) 626 | ROS_WARN("odometry process over 100ms"); 627 | 628 | frameCount++; 629 | } 630 | rate.sleep(); 631 | } 632 | 633 | return EXIT_SUCCESS; 634 | } 635 | -------------------------------------------------------------------------------- /src/lidar_localization/src/aloam_mapping_node.cpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the following paper: 2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 4 | 5 | // Modifier: Tong Qin qintonguav@gmail.com 6 | // Shaozu Cao saozu.cao@connect.ust.hk 7 | 8 | 9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University 10 | // Further contributions copyright (c) 2016, Southwest Research Institute 11 | // All rights reserved. 12 | // 13 | // Redistribution and use in source and binary forms, with or without 14 | // modification, are permitted provided that the following conditions are met: 15 | // 16 | // 1. Redistributions of source code must retain the above copyright notice, 17 | // this list of conditions and the following disclaimer. 18 | // 2. Redistributions in binary form must reproduce the above copyright notice, 19 | // this list of conditions and the following disclaimer in the documentation 20 | // and/or other materials provided with the distribution. 21 | // 3. Neither the name of the copyright holder nor the names of its 22 | // contributors may be used to endorse or promote products derived from this 23 | // software without specific prior written permission. 24 | // 25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | // POSSIBILITY OF SUCH DAMAGE. 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | #include "lidar_localization/utils/common.h" 62 | #include "lidar_localization/utils/tic_toc.h" 63 | 64 | #include "lidar_localization/models/loam/aloam_factor.hpp" 65 | 66 | 67 | int frameCount = 0; 68 | 69 | double timeLaserCloudCornerLast = 0; 70 | double timeLaserCloudSurfLast = 0; 71 | double timeLaserCloudFullRes = 0; 72 | double timeLaserOdometry = 0; 73 | 74 | 75 | int laserCloudCenWidth = 10; 76 | int laserCloudCenHeight = 10; 77 | int laserCloudCenDepth = 5; 78 | const int laserCloudWidth = 21; 79 | const int laserCloudHeight = 21; 80 | const int laserCloudDepth = 11; 81 | 82 | 83 | const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851 84 | 85 | 86 | int laserCloudValidInd[125]; 87 | int laserCloudSurroundInd[125]; 88 | 89 | // input: from odom 90 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); 91 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); 92 | 93 | // ouput: all visualble cube points 94 | pcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud()); 95 | 96 | // surround points in map to build tree 97 | pcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud()); 98 | pcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud()); 99 | 100 | //input & output: points in one frame. local --> global 101 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); 102 | 103 | // points in every cube 104 | pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum]; 105 | pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum]; 106 | 107 | //kd-tree 108 | pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN()); 109 | pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN()); 110 | 111 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; 112 | Eigen::Map q_w_curr(parameters); 113 | Eigen::Map t_w_curr(parameters + 4); 114 | 115 | // wmap_T_odom * odom_T_curr = wmap_T_curr; 116 | // transformation between odom's world and map's world frame 117 | Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0); 118 | Eigen::Vector3d t_wmap_wodom(0, 0, 0); 119 | 120 | Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0); 121 | Eigen::Vector3d t_wodom_curr(0, 0, 0); 122 | 123 | 124 | std::queue cornerLastBuf; 125 | std::queue surfLastBuf; 126 | std::queue fullResBuf; 127 | std::queue odometryBuf; 128 | std::mutex mBuf; 129 | 130 | pcl::VoxelGrid downSizeFilterCorner; 131 | pcl::VoxelGrid downSizeFilterSurf; 132 | 133 | std::vector pointSearchInd; 134 | std::vector pointSearchSqDis; 135 | 136 | PointType pointOri, pointSel; 137 | 138 | ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath; 139 | 140 | nav_msgs::Path laserAfterMappedPath; 141 | 142 | // set initial guess 143 | void transformAssociateToMap() 144 | { 145 | q_w_curr = q_wmap_wodom * q_wodom_curr; 146 | t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 147 | } 148 | 149 | void transformUpdate() 150 | { 151 | q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); 152 | t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; 153 | } 154 | 155 | void pointAssociateToMap(PointType const *const pi, PointType *const po) 156 | { 157 | Eigen::Vector3d point_curr(pi->x, pi->y, pi->z); 158 | Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr; 159 | po->x = point_w.x(); 160 | po->y = point_w.y(); 161 | po->z = point_w.z(); 162 | po->intensity = pi->intensity; 163 | //po->intensity = 1.0; 164 | } 165 | 166 | void pointAssociateTobeMapped(PointType const *const pi, PointType *const po) 167 | { 168 | Eigen::Vector3d point_w(pi->x, pi->y, pi->z); 169 | Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr); 170 | po->x = point_curr.x(); 171 | po->y = point_curr.y(); 172 | po->z = point_curr.z(); 173 | po->intensity = pi->intensity; 174 | } 175 | 176 | void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2) 177 | { 178 | mBuf.lock(); 179 | cornerLastBuf.push(laserCloudCornerLast2); 180 | mBuf.unlock(); 181 | } 182 | 183 | void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2) 184 | { 185 | mBuf.lock(); 186 | surfLastBuf.push(laserCloudSurfLast2); 187 | mBuf.unlock(); 188 | } 189 | 190 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2) 191 | { 192 | mBuf.lock(); 193 | fullResBuf.push(laserCloudFullRes2); 194 | mBuf.unlock(); 195 | } 196 | 197 | //receive odomtry 198 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry) 199 | { 200 | mBuf.lock(); 201 | odometryBuf.push(laserOdometry); 202 | mBuf.unlock(); 203 | 204 | // high frequence publish 205 | Eigen::Quaterniond q_wodom_curr; 206 | Eigen::Vector3d t_wodom_curr; 207 | q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x; 208 | q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y; 209 | q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z; 210 | q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w; 211 | t_wodom_curr.x() = laserOdometry->pose.pose.position.x; 212 | t_wodom_curr.y() = laserOdometry->pose.pose.position.y; 213 | t_wodom_curr.z() = laserOdometry->pose.pose.position.z; 214 | 215 | Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr; 216 | Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 217 | 218 | nav_msgs::Odometry odomAftMapped; 219 | odomAftMapped.header.frame_id = "/map"; 220 | odomAftMapped.child_frame_id = "/velo_link"; 221 | odomAftMapped.header.stamp = laserOdometry->header.stamp; 222 | odomAftMapped.pose.pose.orientation.x = q_w_curr.x(); 223 | odomAftMapped.pose.pose.orientation.y = q_w_curr.y(); 224 | odomAftMapped.pose.pose.orientation.z = q_w_curr.z(); 225 | odomAftMapped.pose.pose.orientation.w = q_w_curr.w(); 226 | odomAftMapped.pose.pose.position.x = t_w_curr.x(); 227 | odomAftMapped.pose.pose.position.y = t_w_curr.y(); 228 | odomAftMapped.pose.pose.position.z = t_w_curr.z(); 229 | pubOdomAftMappedHighFrec.publish(odomAftMapped); 230 | } 231 | 232 | void process() 233 | { 234 | while(1) 235 | { 236 | while (!cornerLastBuf.empty() && !surfLastBuf.empty() && 237 | !fullResBuf.empty() && !odometryBuf.empty()) 238 | { 239 | mBuf.lock(); 240 | while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) 241 | odometryBuf.pop(); 242 | if (odometryBuf.empty()) 243 | { 244 | mBuf.unlock(); 245 | break; 246 | } 247 | 248 | while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) 249 | surfLastBuf.pop(); 250 | if (surfLastBuf.empty()) 251 | { 252 | mBuf.unlock(); 253 | break; 254 | } 255 | 256 | while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec()) 257 | fullResBuf.pop(); 258 | if (fullResBuf.empty()) 259 | { 260 | mBuf.unlock(); 261 | break; 262 | } 263 | 264 | timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec(); 265 | timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec(); 266 | timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec(); 267 | timeLaserOdometry = odometryBuf.front()->header.stamp.toSec(); 268 | 269 | if (timeLaserCloudCornerLast != timeLaserOdometry || 270 | timeLaserCloudSurfLast != timeLaserOdometry || 271 | timeLaserCloudFullRes != timeLaserOdometry) 272 | { 273 | printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry); 274 | printf("unsync messeage!"); 275 | mBuf.unlock(); 276 | break; 277 | } 278 | 279 | laserCloudCornerLast->clear(); 280 | pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast); 281 | cornerLastBuf.pop(); 282 | 283 | laserCloudSurfLast->clear(); 284 | pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast); 285 | surfLastBuf.pop(); 286 | 287 | laserCloudFullRes->clear(); 288 | pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes); 289 | fullResBuf.pop(); 290 | 291 | q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x; 292 | q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y; 293 | q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z; 294 | q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w; 295 | t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x; 296 | t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y; 297 | t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z; 298 | odometryBuf.pop(); 299 | 300 | while(!cornerLastBuf.empty()) 301 | { 302 | cornerLastBuf.pop(); 303 | printf("drop lidar frame in mapping for real time performance \n"); 304 | } 305 | 306 | mBuf.unlock(); 307 | 308 | TicToc t_whole; 309 | 310 | transformAssociateToMap(); 311 | 312 | TicToc t_shift; 313 | int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth; 314 | int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight; 315 | int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth; 316 | 317 | if (t_w_curr.x() + 25.0 < 0) 318 | centerCubeI--; 319 | if (t_w_curr.y() + 25.0 < 0) 320 | centerCubeJ--; 321 | if (t_w_curr.z() + 25.0 < 0) 322 | centerCubeK--; 323 | 324 | while (centerCubeI < 3) 325 | { 326 | for (int j = 0; j < laserCloudHeight; j++) 327 | { 328 | for (int k = 0; k < laserCloudDepth; k++) 329 | { 330 | int i = laserCloudWidth - 1; 331 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 332 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 333 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 334 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 335 | for (; i >= 1; i--) 336 | { 337 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 338 | laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 339 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 340 | laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 341 | } 342 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 343 | laserCloudCubeCornerPointer; 344 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 345 | laserCloudCubeSurfPointer; 346 | laserCloudCubeCornerPointer->clear(); 347 | laserCloudCubeSurfPointer->clear(); 348 | } 349 | } 350 | 351 | centerCubeI++; 352 | laserCloudCenWidth++; 353 | } 354 | 355 | while (centerCubeI >= laserCloudWidth - 3) 356 | { 357 | for (int j = 0; j < laserCloudHeight; j++) 358 | { 359 | for (int k = 0; k < laserCloudDepth; k++) 360 | { 361 | int i = 0; 362 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 363 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 364 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 365 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 366 | for (; i < laserCloudWidth - 1; i++) 367 | { 368 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 369 | laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 370 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 371 | laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 372 | } 373 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 374 | laserCloudCubeCornerPointer; 375 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 376 | laserCloudCubeSurfPointer; 377 | laserCloudCubeCornerPointer->clear(); 378 | laserCloudCubeSurfPointer->clear(); 379 | } 380 | } 381 | 382 | centerCubeI--; 383 | laserCloudCenWidth--; 384 | } 385 | 386 | while (centerCubeJ < 3) 387 | { 388 | for (int i = 0; i < laserCloudWidth; i++) 389 | { 390 | for (int k = 0; k < laserCloudDepth; k++) 391 | { 392 | int j = laserCloudHeight - 1; 393 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 394 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 395 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 396 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 397 | for (; j >= 1; j--) 398 | { 399 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 400 | laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k]; 401 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 402 | laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k]; 403 | } 404 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 405 | laserCloudCubeCornerPointer; 406 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 407 | laserCloudCubeSurfPointer; 408 | laserCloudCubeCornerPointer->clear(); 409 | laserCloudCubeSurfPointer->clear(); 410 | } 411 | } 412 | 413 | centerCubeJ++; 414 | laserCloudCenHeight++; 415 | } 416 | 417 | while (centerCubeJ >= laserCloudHeight - 3) 418 | { 419 | for (int i = 0; i < laserCloudWidth; i++) 420 | { 421 | for (int k = 0; k < laserCloudDepth; k++) 422 | { 423 | int j = 0; 424 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 425 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 426 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 427 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 428 | for (; j < laserCloudHeight - 1; j++) 429 | { 430 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 431 | laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k]; 432 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 433 | laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k]; 434 | } 435 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 436 | laserCloudCubeCornerPointer; 437 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 438 | laserCloudCubeSurfPointer; 439 | laserCloudCubeCornerPointer->clear(); 440 | laserCloudCubeSurfPointer->clear(); 441 | } 442 | } 443 | 444 | centerCubeJ--; 445 | laserCloudCenHeight--; 446 | } 447 | 448 | while (centerCubeK < 3) 449 | { 450 | for (int i = 0; i < laserCloudWidth; i++) 451 | { 452 | for (int j = 0; j < laserCloudHeight; j++) 453 | { 454 | int k = laserCloudDepth - 1; 455 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 456 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 457 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 458 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 459 | for (; k >= 1; k--) 460 | { 461 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 462 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)]; 463 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 464 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)]; 465 | } 466 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 467 | laserCloudCubeCornerPointer; 468 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 469 | laserCloudCubeSurfPointer; 470 | laserCloudCubeCornerPointer->clear(); 471 | laserCloudCubeSurfPointer->clear(); 472 | } 473 | } 474 | 475 | centerCubeK++; 476 | laserCloudCenDepth++; 477 | } 478 | 479 | while (centerCubeK >= laserCloudDepth - 3) 480 | { 481 | for (int i = 0; i < laserCloudWidth; i++) 482 | { 483 | for (int j = 0; j < laserCloudHeight; j++) 484 | { 485 | int k = 0; 486 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer = 487 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 488 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer = 489 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 490 | for (; k < laserCloudDepth - 1; k++) 491 | { 492 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 493 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)]; 494 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 495 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)]; 496 | } 497 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 498 | laserCloudCubeCornerPointer; 499 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 500 | laserCloudCubeSurfPointer; 501 | laserCloudCubeCornerPointer->clear(); 502 | laserCloudCubeSurfPointer->clear(); 503 | } 504 | } 505 | 506 | centerCubeK--; 507 | laserCloudCenDepth--; 508 | } 509 | 510 | int laserCloudValidNum = 0; 511 | int laserCloudSurroundNum = 0; 512 | 513 | for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) 514 | { 515 | for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) 516 | { 517 | for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) 518 | { 519 | if (i >= 0 && i < laserCloudWidth && 520 | j >= 0 && j < laserCloudHeight && 521 | k >= 0 && k < laserCloudDepth) 522 | { 523 | laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; 524 | laserCloudValidNum++; 525 | laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; 526 | laserCloudSurroundNum++; 527 | } 528 | } 529 | } 530 | } 531 | 532 | laserCloudCornerFromMap->clear(); 533 | laserCloudSurfFromMap->clear(); 534 | for (int i = 0; i < laserCloudValidNum; i++) 535 | { 536 | *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]]; 537 | *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]]; 538 | } 539 | int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size(); 540 | int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size(); 541 | 542 | 543 | pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud()); 544 | downSizeFilterCorner.setInputCloud(laserCloudCornerLast); 545 | downSizeFilterCorner.filter(*laserCloudCornerStack); 546 | int laserCloudCornerStackNum = laserCloudCornerStack->points.size(); 547 | 548 | pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud()); 549 | downSizeFilterSurf.setInputCloud(laserCloudSurfLast); 550 | downSizeFilterSurf.filter(*laserCloudSurfStack); 551 | int laserCloudSurfStackNum = laserCloudSurfStack->points.size(); 552 | 553 | printf("map prepare time %f ms\n", t_shift.toc()); 554 | printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum); 555 | if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50) 556 | { 557 | TicToc t_opt; 558 | TicToc t_tree; 559 | kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); 560 | kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap); 561 | printf("build tree time %f ms \n", t_tree.toc()); 562 | 563 | for (int iterCount = 0; iterCount < 2; iterCount++) 564 | { 565 | //ceres::LossFunction *loss_function = NULL; 566 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 567 | ceres::LocalParameterization *q_parameterization = 568 | new ceres::EigenQuaternionParameterization(); 569 | ceres::Problem::Options problem_options; 570 | 571 | ceres::Problem problem(problem_options); 572 | problem.AddParameterBlock(parameters, 4, q_parameterization); 573 | problem.AddParameterBlock(parameters + 4, 3); 574 | 575 | TicToc t_data; 576 | int corner_num = 0; 577 | 578 | for (int i = 0; i < laserCloudCornerStackNum; i++) 579 | { 580 | pointOri = laserCloudCornerStack->points[i]; 581 | //double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z; 582 | pointAssociateToMap(&pointOri, &pointSel); 583 | kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 584 | 585 | if (pointSearchSqDis[4] < 1.0) 586 | { 587 | std::vector nearCorners; 588 | Eigen::Vector3d center(0, 0, 0); 589 | for (int j = 0; j < 5; j++) 590 | { 591 | Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x, 592 | laserCloudCornerFromMap->points[pointSearchInd[j]].y, 593 | laserCloudCornerFromMap->points[pointSearchInd[j]].z); 594 | center = center + tmp; 595 | nearCorners.push_back(tmp); 596 | } 597 | center = center / 5.0; 598 | 599 | Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); 600 | for (int j = 0; j < 5; j++) 601 | { 602 | Eigen::Matrix tmpZeroMean = nearCorners[j] - center; 603 | covMat = covMat + tmpZeroMean * tmpZeroMean.transpose(); 604 | } 605 | 606 | Eigen::SelfAdjointEigenSolver saes(covMat); 607 | 608 | // if is indeed line feature 609 | // note Eigen library sort eigenvalues in increasing order 610 | Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); 611 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); 612 | if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) 613 | { 614 | Eigen::Vector3d point_on_line = center; 615 | Eigen::Vector3d point_a, point_b; 616 | point_a = 0.1 * unit_direction + point_on_line; 617 | point_b = -0.1 * unit_direction + point_on_line; 618 | 619 | ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0); 620 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); 621 | corner_num++; 622 | } 623 | } 624 | /* 625 | else if(pointSearchSqDis[4] < 0.01 * sqrtDis) 626 | { 627 | Eigen::Vector3d center(0, 0, 0); 628 | for (int j = 0; j < 5; j++) 629 | { 630 | Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x, 631 | laserCloudCornerFromMap->points[pointSearchInd[j]].y, 632 | laserCloudCornerFromMap->points[pointSearchInd[j]].z); 633 | center = center + tmp; 634 | } 635 | center = center / 5.0; 636 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); 637 | ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center); 638 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); 639 | } 640 | */ 641 | } 642 | 643 | int surf_num = 0; 644 | for (int i = 0; i < laserCloudSurfStackNum; i++) 645 | { 646 | pointOri = laserCloudSurfStack->points[i]; 647 | //double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z; 648 | pointAssociateToMap(&pointOri, &pointSel); 649 | kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 650 | 651 | Eigen::Matrix matA0; 652 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones(); 653 | if (pointSearchSqDis[4] < 1.0) 654 | { 655 | 656 | for (int j = 0; j < 5; j++) 657 | { 658 | matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x; 659 | matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y; 660 | matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z; 661 | //printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2)); 662 | } 663 | // find the norm of plane 664 | Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); 665 | double negative_OA_dot_norm = 1 / norm.norm(); 666 | norm.normalize(); 667 | 668 | // Here n(pa, pb, pc) is unit norm of plane 669 | bool planeValid = true; 670 | for (int j = 0; j < 5; j++) 671 | { 672 | // if OX * n > 0.2, then plane is not fit well 673 | if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x + 674 | norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y + 675 | norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2) 676 | { 677 | planeValid = false; 678 | break; 679 | } 680 | } 681 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); 682 | if (planeValid) 683 | { 684 | ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm); 685 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); 686 | surf_num++; 687 | } 688 | } 689 | /* 690 | else if(pointSearchSqDis[4] < 0.01 * sqrtDis) 691 | { 692 | Eigen::Vector3d center(0, 0, 0); 693 | for (int j = 0; j < 5; j++) 694 | { 695 | Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x, 696 | laserCloudSurfFromMap->points[pointSearchInd[j]].y, 697 | laserCloudSurfFromMap->points[pointSearchInd[j]].z); 698 | center = center + tmp; 699 | } 700 | center = center / 5.0; 701 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z); 702 | ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center); 703 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4); 704 | } 705 | */ 706 | } 707 | 708 | //printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num); 709 | //printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num); 710 | 711 | printf("mapping data assosiation time %f ms \n", t_data.toc()); 712 | 713 | TicToc t_solver; 714 | ceres::Solver::Options options; 715 | options.linear_solver_type = ceres::DENSE_QR; 716 | options.max_num_iterations = 4; 717 | options.minimizer_progress_to_stdout = false; 718 | options.check_gradients = false; 719 | options.gradient_check_relative_precision = 1e-4; 720 | ceres::Solver::Summary summary; 721 | ceres::Solve(options, &problem, &summary); 722 | printf("mapping solver time %f ms \n", t_solver.toc()); 723 | 724 | //printf("time %f \n", timeLaserOdometry); 725 | //printf("corner factor num %d surf factor num %d\n", corner_num, surf_num); 726 | //printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2], 727 | // parameters[4], parameters[5], parameters[6]); 728 | } 729 | printf("mapping optimization time %f \n", t_opt.toc()); 730 | } 731 | else 732 | { 733 | ROS_WARN("time Map corner and surf num are not enough"); 734 | } 735 | transformUpdate(); 736 | 737 | TicToc t_add; 738 | for (int i = 0; i < laserCloudCornerStackNum; i++) 739 | { 740 | pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel); 741 | 742 | int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; 743 | int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; 744 | int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; 745 | 746 | if (pointSel.x + 25.0 < 0) 747 | cubeI--; 748 | if (pointSel.y + 25.0 < 0) 749 | cubeJ--; 750 | if (pointSel.z + 25.0 < 0) 751 | cubeK--; 752 | 753 | if (cubeI >= 0 && cubeI < laserCloudWidth && 754 | cubeJ >= 0 && cubeJ < laserCloudHeight && 755 | cubeK >= 0 && cubeK < laserCloudDepth) 756 | { 757 | int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; 758 | laserCloudCornerArray[cubeInd]->push_back(pointSel); 759 | } 760 | } 761 | 762 | for (int i = 0; i < laserCloudSurfStackNum; i++) 763 | { 764 | pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel); 765 | 766 | int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth; 767 | int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight; 768 | int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth; 769 | 770 | if (pointSel.x + 25.0 < 0) 771 | cubeI--; 772 | if (pointSel.y + 25.0 < 0) 773 | cubeJ--; 774 | if (pointSel.z + 25.0 < 0) 775 | cubeK--; 776 | 777 | if (cubeI >= 0 && cubeI < laserCloudWidth && 778 | cubeJ >= 0 && cubeJ < laserCloudHeight && 779 | cubeK >= 0 && cubeK < laserCloudDepth) 780 | { 781 | int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; 782 | laserCloudSurfArray[cubeInd]->push_back(pointSel); 783 | } 784 | } 785 | printf("add points time %f ms\n", t_add.toc()); 786 | 787 | 788 | TicToc t_filter; 789 | for (int i = 0; i < laserCloudValidNum; i++) 790 | { 791 | int ind = laserCloudValidInd[i]; 792 | 793 | pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud()); 794 | downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]); 795 | downSizeFilterCorner.filter(*tmpCorner); 796 | laserCloudCornerArray[ind] = tmpCorner; 797 | 798 | pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud()); 799 | downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]); 800 | downSizeFilterSurf.filter(*tmpSurf); 801 | laserCloudSurfArray[ind] = tmpSurf; 802 | } 803 | printf("filter time %f ms \n", t_filter.toc()); 804 | 805 | TicToc t_pub; 806 | //publish surround map for every 5 frame 807 | if (frameCount % 5 == 0) 808 | { 809 | laserCloudSurround->clear(); 810 | for (int i = 0; i < laserCloudSurroundNum; i++) 811 | { 812 | int ind = laserCloudSurroundInd[i]; 813 | *laserCloudSurround += *laserCloudCornerArray[ind]; 814 | *laserCloudSurround += *laserCloudSurfArray[ind]; 815 | } 816 | 817 | sensor_msgs::PointCloud2 laserCloudSurround3; 818 | pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3); 819 | laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry); 820 | laserCloudSurround3.header.frame_id = "/map"; 821 | pubLaserCloudSurround.publish(laserCloudSurround3); 822 | } 823 | 824 | if (frameCount % 20 == 0) 825 | { 826 | pcl::PointCloud laserCloudMap; 827 | for (int i = 0; i < 4851; i++) 828 | { 829 | laserCloudMap += *laserCloudCornerArray[i]; 830 | laserCloudMap += *laserCloudSurfArray[i]; 831 | } 832 | sensor_msgs::PointCloud2 laserCloudMsg; 833 | pcl::toROSMsg(laserCloudMap, laserCloudMsg); 834 | laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry); 835 | laserCloudMsg.header.frame_id = "/map"; 836 | pubLaserCloudMap.publish(laserCloudMsg); 837 | } 838 | 839 | int laserCloudFullResNum = laserCloudFullRes->points.size(); 840 | for (int i = 0; i < laserCloudFullResNum; i++) 841 | { 842 | pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); 843 | } 844 | 845 | sensor_msgs::PointCloud2 laserCloudFullRes3; 846 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); 847 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry); 848 | laserCloudFullRes3.header.frame_id = "/map"; 849 | pubLaserCloudFullRes.publish(laserCloudFullRes3); 850 | 851 | printf("mapping pub time %f ms \n", t_pub.toc()); 852 | 853 | printf("whole mapping time %f ms +++++\n", t_whole.toc()); 854 | 855 | nav_msgs::Odometry odomAftMapped; 856 | odomAftMapped.header.frame_id = "/map"; 857 | odomAftMapped.child_frame_id = "/velo_link"; 858 | odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry); 859 | odomAftMapped.pose.pose.orientation.x = q_w_curr.x(); 860 | odomAftMapped.pose.pose.orientation.y = q_w_curr.y(); 861 | odomAftMapped.pose.pose.orientation.z = q_w_curr.z(); 862 | odomAftMapped.pose.pose.orientation.w = q_w_curr.w(); 863 | odomAftMapped.pose.pose.position.x = t_w_curr.x(); 864 | odomAftMapped.pose.pose.position.y = t_w_curr.y(); 865 | odomAftMapped.pose.pose.position.z = t_w_curr.z(); 866 | pubOdomAftMapped.publish(odomAftMapped); 867 | 868 | geometry_msgs::PoseStamped laserAfterMappedPose; 869 | laserAfterMappedPose.header = odomAftMapped.header; 870 | laserAfterMappedPose.pose = odomAftMapped.pose.pose; 871 | laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp; 872 | laserAfterMappedPath.header.frame_id = "/map"; 873 | laserAfterMappedPath.poses.push_back(laserAfterMappedPose); 874 | pubLaserAfterMappedPath.publish(laserAfterMappedPath); 875 | 876 | static tf::TransformBroadcaster br; 877 | tf::Transform transform; 878 | tf::Quaternion q; 879 | transform.setOrigin( 880 | tf::Vector3( 881 | t_w_curr(0), 882 | t_w_curr(1), 883 | t_w_curr(2) 884 | ) 885 | ); 886 | q.setW(q_w_curr.w()); 887 | q.setX(q_w_curr.x()); 888 | q.setY(q_w_curr.y()); 889 | q.setZ(q_w_curr.z()); 890 | transform.setRotation(q); 891 | br.sendTransform( 892 | tf::StampedTransform( 893 | transform, 894 | odomAftMapped.header.stamp, 895 | "/map", "/velo_link" 896 | ) 897 | ); 898 | 899 | frameCount++; 900 | } 901 | std::chrono::milliseconds dura(2); 902 | std::this_thread::sleep_for(dura); 903 | } 904 | } 905 | 906 | int main(int argc, char **argv) 907 | { 908 | ros::init(argc, argv, "aloam_mapping_node"); 909 | ros::NodeHandle nh; 910 | 911 | float lineRes = 0; 912 | float planeRes = 0; 913 | nh.param("mapping_line_resolution", lineRes, 0.4); 914 | nh.param("mapping_plane_resolution", planeRes, 0.8); 915 | printf("line resolution %f plane resolution %f \n", lineRes, planeRes); 916 | downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes); 917 | downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes); 918 | 919 | ros::Subscriber subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler); 920 | 921 | ros::Subscriber subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler); 922 | 923 | ros::Subscriber subLaserOdometry = nh.subscribe("/laser_odom_scan_to_scan", 100, laserOdometryHandler); 924 | 925 | ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_3", 100, laserCloudFullResHandler); 926 | 927 | pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 100); 928 | 929 | pubLaserCloudMap = nh.advertise("/laser_cloud_map", 100); 930 | 931 | pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_registered", 100); 932 | 933 | pubOdomAftMapped = nh.advertise("/laser_odom_scan_to_map", 100); 934 | 935 | pubOdomAftMappedHighFrec = nh.advertise("/laser_odom_scan_to_map_high_freq", 100); 936 | 937 | pubLaserAfterMappedPath = nh.advertise("/laser_odom_scan_to_map_path", 100); 938 | 939 | for (int i = 0; i < laserCloudNum; i++) 940 | { 941 | laserCloudCornerArray[i].reset(new pcl::PointCloud()); 942 | laserCloudSurfArray[i].reset(new pcl::PointCloud()); 943 | } 944 | 945 | std::thread mapping_process{process}; 946 | 947 | ros::spin(); 948 | 949 | return 0; 950 | } --------------------------------------------------------------------------------