├── CMakeLists.txt ├── README.md ├── config ├── exp_config │ └── exp_port.yaml └── rviz_config │ └── lins_rviz_config.rviz ├── include ├── Estimator.h ├── KalmanFilter.hpp ├── LidarIris.h ├── MapRingBuffer.h ├── StateEstimator.hpp ├── fftm │ └── fftm.hpp ├── gps_common │ └── conversions.h ├── integrationBase.h ├── math_utils.h ├── parameters.h ├── sc_lego_loam │ ├── KDTreeVectorOfVectorsAdaptor.h │ ├── Scancontext.h │ ├── nanoflann.hpp │ └── tictoc.h ├── sensor_utils.hpp ├── tic_toc.h ├── utility.h └── xmldom │ └── XmlDomDocument.h ├── launch ├── ImuOdometry.rviz ├── exp_launch │ ├── GDUTdemo.gps │ ├── KITTIdemo.gps │ ├── KITTIdemo2.gps │ ├── demo.rviz │ ├── run_port_exp.launch │ └── satellite.rviz ├── imuOdometry.launch ├── localization.launch ├── localization_demo.rviz └── staticTFpub.launch ├── lins.kdev4 ├── log.md ├── package.xml ├── pic ├── GPSdetector1.png ├── GPSdetector2.png ├── IMU_GPS_LIDAR_Traj.png ├── global_graph.png ├── kitti_mapping_evo1.png ├── kitti_mapping_evo2.png ├── kitti_mapping_with_gps.png ├── kitti_mapping_without_gps.png ├── loop compared.png ├── vehicle and sensor mounting.png └── 融合建图效果2.png ├── src ├── ImuFactorDemo.cpp ├── LidarIris.cpp ├── fftm.cpp ├── image_projection_node.cpp ├── lib │ ├── Estimator.cpp │ ├── Scancontext.cpp │ ├── XmlDomDocument.cpp │ └── parameters.cpp ├── lidar_mapping_node.cpp ├── lins_fusion_node.cpp ├── mapLocalization.cpp ├── staticTFpub_node.cpp └── transform_fusion_node.cpp └── urdf └── smartcar.urdf /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.0) 2 | project(lins) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3") 5 | set(CMAKE_CXX_STANDARD 14) 6 | SET(CMAKE_BUILD_TYPE Release) 7 | 8 | # set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} "/usr/lib/x86_64-linux-gnu/cmake") 9 | # set(OpenCV_DIR "/usr/local/share/OpenCV") 10 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | cloud_msgs 14 | cv_bridge 15 | geometry_msgs 16 | image_transport 17 | nav_msgs 18 | pcl_conversions 19 | pcl_ros 20 | roscpp 21 | rospy 22 | sensor_msgs 23 | std_msgs 24 | tf 25 | sleipnir_msgs 26 | # livox_ros_driver 27 | ) 28 | 29 | find_package(GTSAM REQUIRED QUIET) 30 | find_package(PCL REQUIRED QUIET) 31 | find_package(OpenCV 3 REQUIRED QUIET) 32 | find_package(Eigen3 3.3 REQUIRED) 33 | find_package (GeographicLib REQUIRED) 34 | 35 | # message(WARNING " OpenCV library status:") 36 | # message(WARNING " config: ${OpenCV_DIR}") 37 | # message(WARNING " version: ${OpenCV_VERSION}") 38 | # message(WARNING " libraries: ${OpenCV_LIBS}") 39 | # message(WARNING " include path: ${OpenCV_INCLUDE_DIRS}") 40 | 41 | catkin_package( 42 | INCLUDE_DIRS include 43 | CATKIN_DEPENDS cloud_msgs 44 | DEPENDS PCL 45 | ) 46 | 47 | 48 | include_directories( 49 | include 50 | ${catkin_INCLUDE_DIRS} 51 | ${PCL_INCLUDE_DIRS} 52 | ${OpenCV_INCLUDE_DIRS} 53 | ${GTSAM_INCLUDE_DIR} 54 | ${EIGEN3_INCLUDE_DIR} 55 | ) 56 | 57 | link_directories( 58 | include 59 | ${OpenCV_LIBRARY_DIRS} 60 | ${PCL_LIBRARY_DIRS} 61 | ${GTSAM_LIBRARY_DIRS} 62 | ) 63 | 64 | 65 | list(APPEND SOURCE_FILES 66 | ${PROJECT_SOURCE_DIR}/src/lib/parameters.cpp 67 | ) 68 | 69 | list(APPEND LINS_FILES 70 | ${PROJECT_SOURCE_DIR}/src/lins_fusion_node.cpp 71 | ${PROJECT_SOURCE_DIR}/src/lib/Estimator.cpp 72 | ) 73 | 74 | list(APPEND LINK_LIBS 75 | ${OpenCV_LIBS} 76 | ${catkin_LIBRARIES} 77 | ${PCL_LIBRARIES} 78 | ${OpenCV_LIBRARY_DIRS} 79 | ${GeographicLib_LIBRARIES} 80 | ) 81 | 82 | add_executable(lins_fusion_node ${LINS_FILES} ${SOURCE_FILES}) 83 | target_link_libraries(lins_fusion_node ${LINK_LIBS}) 84 | 85 | add_executable(image_projection_node src/image_projection_node.cpp ${SOURCE_FILES}) 86 | # add_dependencies(image_projection_node ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp) 87 | target_link_libraries(image_projection_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 88 | 89 | add_executable(lidar_mapping_node src/lidar_mapping_node.cpp ${SOURCE_FILES} src/lib/Scancontext.cpp src/LidarIris.cpp src/fftm.cpp) 90 | target_link_libraries(lidar_mapping_node ${LINK_LIBS} gtsam Eigen3::Eigen) 91 | 92 | add_executable(transform_fusion_node src/transform_fusion_node.cpp ${SOURCE_FILES}) 93 | target_link_libraries(transform_fusion_node ${LINK_LIBS}) 94 | 95 | add_executable(mapLocalization src/mapLocalization.cpp ${SOURCE_FILES}) 96 | target_link_libraries(mapLocalization ${LINK_LIBS} gtsam Eigen3::Eigen) 97 | 98 | add_executable(ImuFactorDemo 99 | src/ImuFactorDemo.cpp 100 | ) 101 | target_link_libraries(ImuFactorDemo 102 | ${catkin_LIBRARIES} 103 | ${PCL_LIBRARIES} 104 | ${OpenCV_LIBRARIES} 105 | gtsam 106 | Eigen3::Eigen 107 | ) 108 | 109 | add_executable(staticTFpub_node src/staticTFpub_node.cpp src/lib/XmlDomDocument.cpp ${SOURCE_FILES}) 110 | target_link_libraries(staticTFpub_node ${catkin_LIBRARIES} -lxerces-c) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # lins-gps-iris 2 | 3 | We propose a system that can fuse IMU, LiDAR and intermittent GPS measurements to achieve high precision localization and mapping in large-scale environment. The Iterated Error-State Kalman Filter (IESKF) is used to fuse IMU and LiDAR measurements to estimate relative motion information quickly. Based on the factor graph, LiDAR, GPS measurements and loop-closure are transformed into constraints for joint-optimization to construct global map. A real-time GPS outlier detection method based on IMU pre-integration theory and residual chi-square test is designed. In addition, the use of robust kernel is supported to implicitly reduce the impact of undetected GPS outliers on the system. A LiDAR-based loop-closure detector is designed, which can search for a pair of point clouds with similar geometric information. 4 | 5 | ![](./pic/融合建图效果2.png) 6 | 7 | ## System architecture 8 | 9 | ![](./pic/global_graph.png) 10 | 11 | 12 | 13 | The main contributions can be summarized as follows: 14 | 15 | 1) An IESKF is applied to tightly couple IMU and LiDAR to implement LIO algorithm. The motion information can be estimated by LIO quickly and accurately. 16 | 17 | 2) A robust and sensitive GPS outlier detection method is proposed. It can detect GPS outliers and reduce the impact of outliers on the system. 18 | 19 | 3) A lightweight and precise loop-closure detection method is proposed. It can evaluate the similarity between point clouds, and correct the error in pose graph. 20 | 21 | ## Dependency 22 | 23 | * [ROS](http://wiki.ros.org/ROS/Installation) (tested with Melodic) 24 | * [gtsam](https://github.com/borglab/gtsam/releases) (Georgia Tech Smoothing and Mapping library) 25 | * [cloud_msgs](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/tree/master/cloud_msgs) 26 | * [xw_gps_driver](https://github.com/kuzen/xw_gps_driver) 27 | 28 | ## Install 29 | 30 | ``` 31 | cd ~/catkin_ws/stc 32 | git clone https://github.com/GDUT-Kyle/lins-gps-iris.git 33 | cd .. 34 | catkin build lins 35 | ``` 36 | 37 | ## Notes 38 | 39 | 1. The system can set the dynamic parameter "useGPSfactor" in the launch file to determine whether it needs to integrate GPS data. 40 | 2. If the GPS data is integrated, the user needs to calibrate the GPS module and the external parameters of the LiDAR in advance. 41 | 3. After setting "SaveMap" to true, the mapping module of this system can save the feature point map for relocation. 42 | 4. In this system, GPS data is our custom ROS message type, users can refer to [xw_gps_driver](https://github.com/kuzen/xw_gps_driver). 43 | 44 | ## Sample datasets 45 | 46 | **Our own collected datasets contain high-precision mapping information. Since we do not have surveying and mapping qualifications, we do not share the dataset due to legal issues.** 47 | 48 | ## Run 49 | 50 | ### A. Mapping 51 | 52 | ``` 53 | roslaunch lins imuOdometry.launch 54 | roslaunch lins run_port_exp.launch 55 | ``` 56 | 57 | ### B. Localization 58 | 59 | ``` 60 | roslaunch lins localization.launch 61 | ``` 62 | 63 | ## Experimental results 64 | 65 | ### A. GPS-Challenge environment test (KITTI) 66 | 67 | 68 | 69 |
(a) Raw GPS measurements
70 | 71 | 72 | 73 |
(b) Proposed method (with GPS)
74 | 75 | ![](./pic/kitti_mapping_without_gps.png) 76 | 77 |
(c) Mapping (without GPS)
78 | 79 | ![](./pic/kitti_mapping_with_gps.png) 80 | 81 |
(d) Mapping (with GPS)
82 | 83 | ![](./pic/kitti_mapping_evo1.png) 84 | 85 | ![kitti_mapping_evo2](./pic/kitti_mapping_evo2.png) 86 | 87 | ### B. Loop-Closure test 88 | 89 | 90 | 91 | ## References 92 | 93 | [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM) 94 | 95 | [LINS---LiDAR-inertial-SLAM](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) 96 | 97 | [SC-LeGO-LOAM](https://github.com/irapkaist/SC-LeGO-LOAM) 98 | 99 | [LiDAR-Iris](https://github.com/JoestarK/LiDAR-Iris) 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /config/exp_config/exp_port.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | # settings 4 | calibrate_imu: 0 # 0: no imu calibration and use default values. 1: calibrate imu 5 | show_configuration: 0 6 | average_nums: 100 7 | imu_lidar_extrinsic_angle: 0.0 8 | imu_misalign_angle: 0.0 9 | line_num: 16 10 | scan_num: 1800 11 | scan_period: 0.1 12 | edge_threshold: 1.0 13 | surf_threshold: 0.1 14 | nearest_feature_search_sq_dist: 25 15 | verbose: 0 16 | icp_freq: 1 17 | max_lidar_nums: 200000 18 | num_iter: 30 19 | lidar_scale: 1 20 | lidar_std: 0.01 21 | 22 | # topic names 23 | imu_topic: "/imu/data" 24 | lidar_topic: "/sensing/lidar/rslidar_points" 25 | lidar_odometry_topic: "/laser_odom_to_init" 26 | lidar_mapping_topic: "/integrated_to_init" 27 | 28 | # kyle parameters 29 | # OriLon: 113.387985229 30 | # OriLat: 23.040807724 31 | # OriAlt: 2.96000003815 32 | # OriYaw: 76.1139984131 33 | # OriPitch: -1.33500003815 34 | # OriRoll: 1.82000005245 35 | # compensate_init_yaw: 0.03 36 | # compensate_init_pitch: 0.0 37 | # compensate_init_roll: 0.0 38 | 39 | # noice parameters 40 | acc_n: 70000 41 | gyr_n: 0.1 42 | acc_w: 500 43 | gyr_w: 0.05 44 | 45 | init_pos_std: !!opencv-matrix 46 | rows: 3 47 | cols: 1 48 | dt: d 49 | data: [0.0, 0.0, 0.0] 50 | 51 | init_vel_std: !!opencv-matrix 52 | rows: 3 53 | cols: 1 54 | dt: d 55 | data: [0.0, 0.0, 0.0] 56 | 57 | init_att_std: !!opencv-matrix 58 | rows: 3 59 | cols: 1 60 | dt: d 61 | data: [0.0, 0.0, 0.0] 62 | 63 | init_acc_std: !!opencv-matrix 64 | rows: 3 65 | cols: 1 66 | dt: d 67 | data: [0.01, 0.01, 0.02] 68 | 69 | init_gyr_std: !!opencv-matrix 70 | rows: 3 71 | cols: 1 72 | dt: d 73 | data: [0.002, 0.002, 0.002] 74 | 75 | # initial IMU biases 76 | init_ba: !!opencv-matrix 77 | rows: 3 78 | cols: 1 79 | dt: d 80 | data: [-0.015774,0.143237,-0.0263845] 81 | 82 | init_bw: !!opencv-matrix 83 | rows: 3 84 | cols: 1 85 | dt: d 86 | data: [-0.00275058,-0.000165954,0.00262913] 87 | 88 | # extrinsic parameters 89 | init_tbl: !!opencv-matrix 90 | rows: 3 91 | cols: 1 92 | dt: d 93 | data: [0.0,0.0,0.0] 94 | 95 | init_rbl: !!opencv-matrix 96 | rows: 3 97 | cols: 3 98 | dt: d 99 | data: [1, 0, 0, 100 | 0, 1, 0, 101 | 0, 0, 1] 102 | 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /include/Estimator.h: -------------------------------------------------------------------------------- 1 | // This file is part of LINS. 2 | // 3 | // Copyright (C) 2020 Chao Qin , 4 | // Robotics and Multiperception Lab (RAM-LAB ), 5 | // The Hong Kong University of Science and Technology 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | 18 | #ifndef INCLUDE_ESTIMATOR_H_ 19 | #define INCLUDE_ESTIMATOR_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include "cloud_msgs/cloud_info.h" 49 | 50 | using namespace std; 51 | using namespace Eigen; 52 | using namespace math_utils; 53 | using namespace sensor_utils; 54 | 55 | namespace fusion { 56 | 57 | class LinsFusion { 58 | public: 59 | LinsFusion(ros::NodeHandle& nh, ros::NodeHandle& pnh); 60 | // 析构函数，删除该对象 61 | ~LinsFusion(); 62 | 63 | // 主功能函数 64 | void run(); 65 | // 初始化函数，设置topic的定义和发布，开辟一些必要的空间，然后就一直等待回调函数 66 | void initialization(); 67 | void publishTopics(); 68 | // 发布lidar odometry的里程计信息和tf 69 | void publishOdometryYZX(double timeStamp); 70 | inline void publishCloudMsg(ros::Publisher& publisher, 71 | pcl::PointCloud::Ptr cloud, 72 | const ros::Time& stamp, 73 | const std::string& frameID) { 74 | sensor_msgs::PointCloud2 msg; 75 | pcl::toROSMsg(*cloud, msg); 76 | msg.header.stamp = stamp; 77 | msg.header.frame_id = frameID; 78 | publisher.publish(msg); 79 | } 80 | 81 | void imuCallback(const sensor_msgs::Imu::ConstPtr& imuIn); 82 | void laserCloudCallback( 83 | const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); 84 | void laserCloudInfoCallback(const cloud_msgs::cloud_infoConstPtr& msgIn); 85 | void outlierCloudCallback( 86 | const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg); 87 | // Map-refined odom. feedback (1 Hz) 88 | void mapOdometryCallback(const nav_msgs::Odometry::ConstPtr& odometryMsg); 89 | 90 | // 执行IKF进行状态估计 91 | void performStateEstimation(); 92 | void processFirstPointCloud(); 93 | bool processPointClouds(); 94 | void performImuBiasEstimation(); 95 | void alignIMUtoVehicle(const V3D& rpy, const V3D& acc_in, const V3D& gyr_in, 96 | V3D& acc_out, V3D& gyr_out); 97 | 98 | private: 99 | ros::NodeHandle nh_; 100 | ros::NodeHandle pnh_; 101 | 102 | // !@StateEstimator 103 | StateEstimator* estimator; 104 | 105 | // !@Subscriber 106 | ros::Subscriber subLaserCloud; 107 | ros::Subscriber subLaserCloudInfo; 108 | ros::Subscriber subOutlierCloud; 109 | ros::Subscriber subImu; 110 | ros::Subscriber subGPS_; 111 | ros::Subscriber subMapOdom_; 112 | 113 | // !@Publishers 114 | ros::Publisher pubUndistortedPointCloud; 115 | 116 | ros::Publisher pubCornerPointsSharp; 117 | ros::Publisher pubCornerPointsLessSharp; 118 | ros::Publisher pubSurfPointsFlat; 119 | ros::Publisher pubSurfPointsLessFlat; 120 | 121 | ros::Publisher pubLaserCloudCornerLast; 122 | ros::Publisher pubLaserCloudSurfLast; 123 | ros::Publisher pubOutlierCloudLast; 124 | 125 | ros::Publisher pubLaserOdometry; 126 | ros::Publisher pubIMUOdometry; 127 | ros::Publisher pubGpsOdometry; 128 | 129 | ros::Publisher pubLaserOdom; 130 | 131 | // !@PointCloudPtrs 132 | pcl::PointCloud::Ptr distortedPointCloud; // 畸变点云？ 133 | pcl::PointCloud::Ptr outlierPointCloud; // 局外点云 134 | 135 | // !@Messages 136 | nav_msgs::Odometry laserOdom; 137 | nav_msgs::Odometry laserOdometry; 138 | nav_msgs::Odometry imuOdometry; 139 | nav_msgs::Odometry gpsOdometry; 140 | 141 | // !@Buffers 142 | MapRingBuffer imuBuf_; 143 | MapRingBuffer pclBuf_; 144 | MapRingBuffer outlierBuf_; 145 | MapRingBuffer cloudInfoBuf_; 146 | MapRingBuffer gpsBuf_; 147 | 148 | // !@Time 149 | int scan_counter_; 150 | double duration_; 151 | 152 | // !@Measurements 153 | V3D acc_raw_; 154 | V3D gyr_raw_; 155 | V3D acc_aligned_; 156 | V3D gyr_aligned_; 157 | V3D misalign_euler_angles_; 158 | V3D ba_tmp_; 159 | V3D bw_tmp_; 160 | V3D ba_init_; 161 | V3D bw_init_; 162 | double scan_time_; 163 | double last_imu_time_; 164 | double last_scan_time_; 165 | int sample_counter_; 166 | bool isInititialized; 167 | bool isImuCalibrated; 168 | }; 169 | } // namespace fusion 170 | 171 | #endif // INCLUDE_ESTIMATOR_H_ 172 | -------------------------------------------------------------------------------- /include/KalmanFilter.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of LINS. 2 | // 3 | // Copyright (C) 2020 Chao Qin , 4 | // Robotics and Multiperception Lab (RAM-LAB ), 5 | // The Hong Kong University of Science and Technology 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | 18 | #ifndef INCLUDE_KALMANFILTER_HPP_ 19 | #define INCLUDE_KALMANFILTER_HPP_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | using namespace math_utils; 29 | using namespace parameter; 30 | 31 | namespace filter { 32 | 33 | // GlobalState Class contains state variables including position, velocity, 34 | // attitude, acceleration bias, gyroscope bias, and gravity 35 | // 该类包含位置，速度，姿态，加速度计误差，陀螺仪误差和重力，论文公式(2) 36 | class GlobalState { 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | // 状态向量维度 40 | static constexpr unsigned int DIM_OF_STATE_ = 18; 41 | // 噪声向量维度 42 | static constexpr unsigned int DIM_OF_NOISE_ = 12; 43 | // 各变量的首元素索引值 44 | static constexpr unsigned int pos_ = 0; 45 | static constexpr unsigned int vel_ = 3; 46 | static constexpr unsigned int att_ = 6; 47 | static constexpr unsigned int acc_ = 9; 48 | static constexpr unsigned int gyr_ = 12; 49 | static constexpr unsigned int gra_ = 15; 50 | 51 | GlobalState() { setIdentity(); } 52 | 53 | // 设置状态量 54 | GlobalState(const V3D& rn, const V3D& vn, const Q4D& qbn, const V3D& ba, 55 | const V3D& bw) { 56 | setIdentity(); 57 | rn_ = rn; // position 58 | vn_ = vn; // velocity 59 | qbn_ = qbn; // attitude 60 | ba_ = ba; // acceleration bias 61 | bw_ = bw; // gyroscope bias 62 | } 63 | 64 | // 析构，删除当前对象 65 | ~GlobalState() {} 66 | 67 | // 初始化时使用 68 | void setIdentity() { 69 | rn_.setZero(); 70 | vn_.setZero(); 71 | qbn_.setIdentity(); 72 | ba_.setZero(); 73 | bw_.setZero(); 74 | gn_ << 0.0, 0.0, -G0; // gravity, 9.81 75 | } 76 | 77 | // boxPlus operator 78 | // 定义boxplus operator, 论文公式(19) 79 | void boxPlus(const Eigen::Matrix& xk, 80 | GlobalState& stateOut) { 81 | stateOut.rn_ = rn_ + xk.template segment<3>(pos_); 82 | stateOut.vn_ = vn_ + xk.template segment<3>(vel_); 83 | stateOut.ba_ = ba_ + xk.template segment<3>(acc_); 84 | stateOut.bw_ = bw_ + xk.template segment<3>(gyr_); 85 | // 欧拉角转四元数 86 | Q4D dq = axis2Quat(xk.template segment<3>(att_)); 87 | stateOut.qbn_ = (qbn_ * dq).normalized(); 88 | 89 | stateOut.gn_ = gn_ + xk.template segment<3>(gra_); 90 | } 91 | 92 | // boxMinus operator 93 | // 定义boxMinus operator, 类似论文公式(19) 94 | void boxMinus(const GlobalState& stateIn, 95 | Eigen::Matrix& xk) { 96 | xk.template segment<3>(pos_) = rn_ - stateIn.rn_; 97 | xk.template segment<3>(vel_) = vn_ - stateIn.vn_; 98 | xk.template segment<3>(acc_) = ba_ - stateIn.ba_; 99 | xk.template segment<3>(gyr_) = bw_ - stateIn.bw_; 100 | V3D da = Quat2axis(stateIn.qbn_.inverse() * qbn_); 101 | xk.template segment<3>(att_) = da; 102 | 103 | xk.template segment<3>(gra_) = gn_ - stateIn.gn_; 104 | } 105 | 106 | // 重载"=" 107 | GlobalState& operator=(const GlobalState& other) { 108 | if (this == &other) return *this; 109 | 110 | this->rn_ = other.rn_; 111 | this->vn_ = other.vn_; 112 | this->qbn_ = other.qbn_; 113 | this->ba_ = other.ba_; 114 | this->bw_ = other.bw_; 115 | this->gn_ = other.gn_; 116 | 117 | return *this; 118 | } 119 | 120 | // !@State 121 | // 各状态变量，共18维的向量 122 | V3D rn_; // position in n-frame 123 | V3D vn_; // velocity in n-frame 124 | Q4D qbn_; // rotation from b-frame to n-frame 125 | V3D ba_; // acceleartion bias 126 | V3D bw_; // gyroscope bias 127 | V3D gn_; // gravity 128 | }; 129 | 130 | // 状态预测器 131 | class StatePredictor { 132 | public: 133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 134 | StatePredictor() { reset(); } 135 | 136 | ~StatePredictor() {} 137 | 138 | bool predict(double dt, const V3D& acc, const V3D& gyr, 139 | bool update_jacobian_ = true) { 140 | // 系统未初始化 141 | if (!isInitialized()) return false; 142 | 143 | // 未初始化Imu 144 | if (!flag_init_imu_) { 145 | flag_init_imu_ = true; 146 | acc_last = acc; 147 | gyr_last = gyr; 148 | } 149 | 150 | // Average acceleration and angular rate 滤波作用？ 151 | // 这里的状态不是误差状态,是正儿八经的状态[pos,vel,att,acc,gyr,gra] 152 | GlobalState state_tmp = state_; 153 | // 去除上一次加速度中的重力成分,乘以姿态即转到世界坐标系下 154 | V3D un_acc_0 = state_tmp.qbn_ * (acc_last - state_tmp.ba_) + state_tmp.gn_; 155 | // 与上一次角速度做加权平均 156 | V3D un_gyr = 0.5 * (gyr_last + gyr) - state_tmp.bw_; 157 | // 求角度变化量，再转化成四元数形式 158 | Q4D dq = axis2Quat(un_gyr * dt); 159 | // 求当前临时状态量中的姿态 160 | state_tmp.qbn_ = (state_tmp.qbn_ * dq).normalized(); 161 | // 去除本次加速度中的重力成分 162 | V3D un_acc_1 = state_tmp.qbn_ * (acc - state_tmp.ba_) + state_tmp.gn_; 163 | // 两次加速度加权平均 164 | V3D un_acc = 0.5 * (un_acc_0 + un_acc_1); 165 | 166 | // State integral 167 | // 通过IMU测量进行积分，得到新的position和velocity 168 | // x = x + v*t + a*t^2 169 | // // v = v + a*t 170 | state_tmp.rn_ = state_tmp.rn_ + dt * state_tmp.vn_ + 0.5 * dt * dt * un_acc; 171 | state_tmp.vn_ = state_tmp.vn_ + dt * un_acc; 172 | 173 | // 更新jacobian矩阵 174 | if (update_jacobian_) { 175 | // 论文公式(6),注意这里的状态会比论文中多了重力加速度的3d向量 176 | MXD Ft = 177 | MXD::Zero(GlobalState::DIM_OF_STATE_, GlobalState::DIM_OF_STATE_); // 18*18 178 | Ft.block<3, 3>(GlobalState::pos_, GlobalState::vel_) = M3D::Identity(); 179 | 180 | Ft.block<3, 3>(GlobalState::vel_, GlobalState::att_) = 181 | -state_tmp.qbn_.toRotationMatrix() * skew(acc - state_tmp.ba_); 182 | Ft.block<3, 3>(GlobalState::vel_, GlobalState::acc_) = 183 | -state_tmp.qbn_.toRotationMatrix(); 184 | Ft.block<3, 3>(GlobalState::vel_, GlobalState::gra_) = M3D::Identity(); 185 | 186 | Ft.block<3, 3>(GlobalState::att_, GlobalState::att_) = 187 | skew(gyr - state_tmp.bw_); 188 | Ft.block<3, 3>(GlobalState::att_, GlobalState::gyr_) = -M3D::Identity(); 189 | 190 | // 论文公式(7),注意这里的状态会比论文中多了重力加速度的3d向量 191 | MXD Gt = 192 | MXD::Zero(GlobalState::DIM_OF_STATE_, GlobalState::DIM_OF_NOISE_); // 18*12 193 | Gt.block<3, 3>(GlobalState::vel_, 0) = -state_tmp.qbn_.toRotationMatrix(); 194 | Gt.block<3, 3>(GlobalState::att_, 3) = -M3D::Identity(); 195 | Gt.block<3, 3>(GlobalState::acc_, 6) = M3D::Identity(); 196 | Gt.block<3, 3>(GlobalState::gyr_, 9) = M3D::Identity(); 197 | Gt = Gt * dt; 198 | 199 | // 这里应该对应论文公式(9)更新预测协方差，但是公式好像不太对应 200 | // 解答:论文中的公式是通过泰勒展开后去一阶项,因此有F_ = I + Ft * dt,这里为了提高 201 | // 求解精度,不妨取到二阶项,因为Ft比较稀疏,计算耗时不高 202 | // 具体参考《Estimation techniques for low-cost inertial navigation》 203 | const MXD I = 204 | MXD::Identity(GlobalState::DIM_OF_STATE_, GlobalState::DIM_OF_STATE_); 205 | F_ = I + Ft * dt + 0.5 * Ft * Ft * dt * dt; 206 | 207 | // jacobian_ = F * jacobian_; 208 | covariance_ = 209 | F_ * covariance_ * F_.transpose() + Gt * noise_ * Gt.transpose(); 210 | // 这样可近似原始协方差矩阵,但是它是半正定的,会更鲁棒 211 | covariance_ = 0.5 * (covariance_ + covariance_.transpose()).eval(); 212 | } 213 | 214 | // 更新滤波器当前状态 215 | state_ = state_tmp; 216 | // 更新滤波器当前时间戳 217 | time_ += dt; 218 | // 记录本次IMU测量，下次会使用 219 | acc_last = acc; 220 | gyr_last = gyr; 221 | return true; 222 | } 223 | 224 | // 从imu测量计算当前的roll和pitch 225 | static void calculateRPfromIMU(const V3D& acc, double& roll, double& pitch) { 226 | pitch = -sign(acc.z()) * asin(acc.x() / G0); 227 | roll = sign(acc.z()) * asin(acc.y() / G0); 228 | } 229 | 230 | // 设置滤波器状态 231 | void set(const GlobalState& state) { state_ = state; } 232 | 233 | // 更新滤波器的状态及协方差 234 | void update(const GlobalState& state, 235 | const Eigen::Matrix& covariance) { 237 | state_ = state; 238 | covariance_ = covariance; 239 | } 240 | 241 | // 初始化滤波器 242 | void initialization(double time, const V3D& rn, const V3D& vn, const Q4D& qbn, 243 | const V3D& ba, const V3D& bw) { 244 | state_ = GlobalState(rn, vn, qbn, ba, bw); 245 | time_ = time; 246 | flag_init_state_ = true; 247 | 248 | initializeCovariance(); 249 | } 250 | 251 | void initialization(double time, const V3D& rn, const V3D& vn, const Q4D& qbn, 252 | const V3D& ba, const V3D& bw, const V3D& acc, 253 | const V3D& gyr) { 254 | state_ = GlobalState(rn, vn, qbn, ba, bw); 255 | time_ = time; 256 | acc_last = acc; 257 | gyr_last = gyr; 258 | flag_init_imu_ = true; 259 | flag_init_state_ = true; 260 | 261 | initializeCovariance(); 262 | } 263 | 264 | void initialization(double time, const V3D& rn, const V3D& vn, const V3D& ba, 265 | const V3D& bw, double roll = 0.0, double pitch = 0.0, 266 | double yaw = 0.0) { 267 | state_ = GlobalState(rn, vn, rpy2Quat(V3D(roll, pitch, yaw)), ba, bw); 268 | time_ = time; 269 | flag_init_state_ = true; 270 | 271 | initializeCovariance(); 272 | } 273 | 274 | void initialization(double time, const V3D& rn, const V3D& vn, const V3D& ba, 275 | const V3D& bw, const V3D& acc, const V3D& gyr, 276 | double roll = 0.0, double pitch = 0.0, double yaw = 0.0) { 277 | state_ = GlobalState(rn, vn, rpy2Quat(V3D(roll, pitch, yaw)), ba, bw); 278 | time_ = time; 279 | acc_last = acc; 280 | gyr_last = gyr; 281 | flag_init_imu_ = true; 282 | flag_init_state_ = true; 283 | 284 | initializeCovariance(); 285 | } 286 | 287 | // 初始化协方差矩阵 288 | void initializeCovariance(int type = 0) { 289 | // 对角线上的矩阵元素 290 | double covX = pow(INIT_POS_STD(0), 2); 291 | double covY = pow(INIT_POS_STD(1), 2); 292 | double covZ = pow(INIT_POS_STD(2), 2); 293 | double covVx = pow(INIT_VEL_STD(0), 2); 294 | double covVy = pow(INIT_VEL_STD(1), 2); 295 | double covVz = pow(INIT_VEL_STD(2), 2); 296 | double covRoll = pow(deg2rad(INIT_ATT_STD(0)), 2); 297 | double covPitch = pow(deg2rad(INIT_ATT_STD(1)), 2); 298 | double covYaw = pow(deg2rad(INIT_ATT_STD(2)), 2); 299 | 300 | // 将元素平方 301 | V3D covPos = INIT_POS_STD.array().square(); 302 | V3D covVel = INIT_VEL_STD.array().square(); 303 | V3D covAcc = INIT_ACC_STD.array().square(); 304 | V3D covGyr = INIT_GYR_STD.array().square(); 305 | 306 | // ug: micro-gravity force -- 9.81/(10^6) 307 | double peba = pow(ACC_N * ug, 2); 308 | double pebg = pow(GYR_N * dph, 2); 309 | double pweba = pow(ACC_W * ugpsHz, 2); 310 | double pwebg = pow(GYR_W * dpsh, 2); 311 | V3D gra_cov(0.01, 0.01, 0.01); 312 | 313 | if (type == 0) { 314 | // Initialize using offline parameters 315 | covariance_.setZero(); 316 | covariance_.block<3, 3>(GlobalState::pos_, GlobalState::pos_) = 317 | covPos.asDiagonal(); // pos 318 | covariance_.block<3, 3>(GlobalState::vel_, GlobalState::vel_) = 319 | covVel.asDiagonal(); // vel 320 | covariance_.block<3, 3>(GlobalState::att_, GlobalState::att_) = 321 | V3D(covRoll, covPitch, covYaw).asDiagonal(); // att 322 | covariance_.block<3, 3>(GlobalState::acc_, GlobalState::acc_) = 323 | covAcc.asDiagonal(); // ba 324 | covariance_.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_) = 325 | covGyr.asDiagonal(); // bg 326 | covariance_.block<3, 3>(GlobalState::gra_, GlobalState::gra_) = 327 | gra_cov.asDiagonal(); // gravity 328 | } else if (type == 1) { 329 | // Inheritage previous covariance 330 | M3D vel_cov = 331 | covariance_.block<3, 3>(GlobalState::vel_, GlobalState::vel_); 332 | M3D acc_cov = 333 | covariance_.block<3, 3>(GlobalState::acc_, GlobalState::acc_); 334 | M3D gyr_cov = 335 | covariance_.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_); 336 | M3D gra_cov = 337 | covariance_.block<3, 3>(GlobalState::gra_, GlobalState::gra_); 338 | 339 | covariance_.setZero(); 340 | covariance_.block<3, 3>(GlobalState::pos_, GlobalState::pos_) = 341 | covPos.asDiagonal(); // pos 342 | covariance_.block<3, 3>(GlobalState::vel_, GlobalState::vel_) = 343 | vel_cov; // vel 344 | covariance_.block<3, 3>(GlobalState::att_, GlobalState::att_) = 345 | V3D(covRoll, covPitch, covYaw).asDiagonal(); // att 346 | covariance_.block<3, 3>(GlobalState::acc_, GlobalState::acc_) = acc_cov; 347 | covariance_.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_) = gyr_cov; 348 | covariance_.block<3, 3>(GlobalState::gra_, GlobalState::gra_) = gra_cov; 349 | } 350 | 351 | // 噪声? 352 | noise_.setZero(); // 12*12 353 | // asDiagonal()指将向量作为对角线构建对角矩阵 354 | noise_.block<3, 3>(0, 0) = V3D(peba, peba, peba).asDiagonal(); 355 | noise_.block<3, 3>(3, 3) = V3D(pebg, pebg, pebg).asDiagonal(); 356 | noise_.block<3, 3>(6, 6) = V3D(pweba, pweba, pweba).asDiagonal(); 357 | noise_.block<3, 3>(9, 9) = V3D(pwebg, pwebg, pwebg).asDiagonal(); 358 | } 359 | 360 | // 重置滤波器状态 361 | void reset(int type = 0) { 362 | if (type == 0) { 363 | state_.rn_.setZero(); 364 | state_.vn_ = state_.qbn_.inverse() * state_.vn_; 365 | state_.qbn_.setIdentity(); 366 | initializeCovariance(); 367 | } else if (type == 1) { 368 | state_.rn_.setZero(); 369 | state_.vn_ = state_.qbn_.inverse() * state_.vn_; 370 | state_.qbn_.setIdentity(); 371 | state_.gn_ = state_.qbn_.inverse() * state_.gn_; 372 | state_.gn_ = state_.gn_ * 9.81 / state_.gn_.norm(); 373 | initializeCovariance(1); 374 | } 375 | } 376 | 377 | void reset(V3D vn, V3D ba, V3D bw) { 378 | state_.setIdentity(); 379 | state_.vn_ = vn; 380 | state_.ba_ = ba; 381 | state_.bw_ = bw; 382 | initializeCovariance(); 383 | } 384 | 385 | // 返回初始化标志位 386 | inline bool isInitialized() { return flag_init_state_; } 387 | 388 | GlobalState state_; 389 | double time_; 390 | Eigen::Matrix 391 | F_; 392 | Eigen::Matrix 393 | jacobian_, covariance_; 394 | Eigen::Matrix 395 | noise_; 396 | 397 | V3D acc_last; // last acceleration measurement 398 | V3D gyr_last; // last gyroscope measurement 399 | 400 | bool flag_init_state_; 401 | bool flag_init_imu_; 402 | }; 403 | 404 | }; // namespace filter 405 | 406 | #endif // INCLUDE_KALMANFILTER_HPP_ 407 | -------------------------------------------------------------------------------- /include/LidarIris.h: -------------------------------------------------------------------------------- 1 | #ifndef _LIDAR_IRIS_H_ 2 | #define _LIDAR_IRIS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class LidarIris 12 | { 13 | public: 14 | struct FeatureDesc 15 | { 16 | cv::Mat1b img; 17 | cv::Mat1b T; 18 | cv::Mat1b M; 19 | }; 20 | 21 | LidarIris(int nscale, int minWaveLength, float mult, float sigmaOnf, int matchNum) : _nscale(nscale), 22 | _minWaveLength(minWaveLength), 23 | _mult(mult), 24 | _sigmaOnf(sigmaOnf), 25 | _matchNum(matchNum), 26 | vecList(flann::Index>(flann::KDTreeIndexParams(4))), 27 | // indicesBuffer(std::vector(matchNum)), 28 | // distsBuffer(std::vector(matchNum)), 29 | indices(flann::Matrix(new int[matchNum], 1, matchNum)), 30 | dists(flann::Matrix(new float[matchNum], 1, matchNum)) 31 | { 32 | } 33 | // LidarIris(const LidarIris &) = delete; 34 | // LidarIris &operator=(const LidarIris &) = delete; 35 | 36 | static cv::Mat1b GetIris(const pcl::PointCloud &cloud); 37 | // 38 | void UpdateFrame(const cv::Mat1b &frame, int frameIndex, float *matchDistance, int *matchIndex); 39 | // 40 | float Compare(const FeatureDesc &img1, const FeatureDesc &img2, int *bias = nullptr); 41 | // 42 | FeatureDesc GetFeature(const cv::Mat1b &src); 43 | FeatureDesc GetFeature(const cv::Mat1b &src, std::vector &vec); 44 | std::vector LogGaborFilter(const cv::Mat1f &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf); 45 | void GetHammingDistance(const cv::Mat1b &T1, const cv::Mat1b &M1, const cv::Mat1b &T2, const cv::Mat1b &M2, int scale, float &dis, int &bias); 46 | // 47 | static inline cv::Mat circRowShift(const cv::Mat &src, int shift_m_rows); 48 | static inline cv::Mat circColShift(const cv::Mat &src, int shift_n_cols); 49 | static cv::Mat circShift(const cv::Mat &src, int shift_m_rows, int shift_n_cols); 50 | 51 | private: 52 | void LoGFeatureEncode(const cv::Mat1b &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf, cv::Mat1b &T, cv::Mat1b &M); 53 | 54 | int _nscale; 55 | int _minWaveLength; 56 | float _mult; 57 | float _sigmaOnf; 58 | int _matchNum; 59 | 60 | flann::Index> vecList; 61 | std::vector featureList; 62 | std::vector frameIndexList; 63 | flann::Matrix indices; 64 | flann::Matrix dists; 65 | // std::vector indicesBuffer; 66 | // std::vector distsBuffer; 67 | }; 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /include/MapRingBuffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2014, Autonomous Systems Lab 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | * 27 | */ 28 | 29 | #ifndef INCLUDE_MAPRINGBUFFER_H_ 30 | #define INCLUDE_MAPRINGBUFFER_H_ 31 | 32 | #include 33 | #include 34 | 35 | template 36 | class MapRingBuffer { 37 | public: 38 | std::map measMap_; 39 | typename std::map::iterator itMeas_; 40 | 41 | int size; 42 | double maxWaitTime_; 43 | double minWaitTime_; 44 | 45 | MapRingBuffer() { 46 | maxWaitTime_ = 0.1; 47 | minWaitTime_ = 0.0; 48 | } 49 | 50 | virtual ~MapRingBuffer() {} 51 | 52 | bool allocate(const int sizeBuffer) { 53 | if (sizeBuffer <= 0) { 54 | return false; 55 | } else { 56 | size = sizeBuffer; 57 | return true; 58 | } 59 | } 60 | 61 | int getSize() { return measMap_.size(); } 62 | 63 | void addMeas(const Meas& meas, const double& t) { 64 | measMap_.insert(std::make_pair(t, meas)); 65 | 66 | // ensure the size of the map, and remove the last element 67 | if (measMap_.size() > size) { 68 | measMap_.erase(measMap_.begin()); 69 | } 70 | } 71 | 72 | void clear() { measMap_.clear(); } 73 | 74 | void clean(double t) { 75 | while (measMap_.size() >= 1 && measMap_.begin()->first <= t) { 76 | measMap_.erase(measMap_.begin()); 77 | } 78 | } 79 | 80 | bool getNextTime(double actualTime, double& nextTime) { 81 | itMeas_ = measMap_.upper_bound(actualTime); 82 | if (itMeas_ != measMap_.end()) { 83 | nextTime = itMeas_->first; 84 | return true; 85 | } else { 86 | return false; 87 | } 88 | } 89 | void waitTime(double actualTime, double& time) { 90 | double measurementTime = actualTime - maxWaitTime_; 91 | if (!measMap_.empty() && 92 | measMap_.rbegin()->first + minWaitTime_ > measurementTime) { 93 | measurementTime = measMap_.rbegin()->first + minWaitTime_; 94 | } 95 | if (time > measurementTime) { 96 | time = measurementTime; 97 | } 98 | } 99 | bool getLastTime(double& lastTime) { 100 | if (!measMap_.empty()) { 101 | lastTime = measMap_.rbegin()->first; 102 | return true; 103 | } else { 104 | return false; 105 | } 106 | } 107 | 108 | bool getFirstTime(double& firstTime) { 109 | if (!measMap_.empty()) { 110 | firstTime = measMap_.begin()->first; 111 | return true; 112 | } else { 113 | return false; 114 | } 115 | } 116 | 117 | bool getLastMeas(Meas& lastMeas) { 118 | if (!measMap_.empty()) { 119 | lastMeas = measMap_.rbegin()->second; 120 | return true; 121 | } else { 122 | return false; 123 | } 124 | } 125 | 126 | bool getLastLastMeas(Meas& lastlastMeas) { 127 | if (measMap_.size() >= 2) { 128 | auto itr = measMap_.rbegin(); 129 | itr++; 130 | lastlastMeas = itr->second; 131 | return true; 132 | } else { 133 | return false; 134 | } 135 | } 136 | 137 | bool getFirstMeas(Meas& firstMeas) { 138 | if (!measMap_.empty()) { 139 | firstMeas = measMap_.begin()->second; 140 | return true; 141 | } else { 142 | return false; 143 | } 144 | } 145 | 146 | bool hasMeasurementAt(double t) { return measMap_.count(t) > 0; } 147 | 148 | bool empty() { return measMap_.empty(); } 149 | 150 | void printContainer() { 151 | itMeas_ = measMap_.begin(); 152 | while (measMap_.size() >= 1 && itMeas_ != measMap_.end()) { 153 | std::cout << itMeas_->second << " "; 154 | itMeas_++; 155 | } 156 | } 157 | }; 158 | 159 | #endif // INCLUDE_MAPRINGBUFFER_H_ 160 | -------------------------------------------------------------------------------- /include/fftm/fftm.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _OPENCV_fftm_HPP_ 2 | #define _OPENCV_fftm_HPP_ 3 | #ifdef __cplusplus 4 | 5 | #include "opencv2/core.hpp" 6 | #include "opencv2/opencv.hpp" 7 | //----------------------------------------------------------------------------------------------------- 8 | // As input we need equal sized images, with the same aspect ratio, 9 | // scale difference should not exceed 1.8 times. 10 | //----------------------------------------------------------------------------------------------------- 11 | cv::RotatedRect FFTMatch(const cv::Mat& im0, const cv::Mat& im1); 12 | cv::RotatedRect LogPolarFFTTemplateMatch(cv::Mat& im0, cv::Mat& im1/*, double canny_threshold1=200, double canny_threshold2=100*/); 13 | #endif 14 | #endif 15 | 16 | // 2019.09.17: 17 | // remove canny -------------------------------------------------------------------------------- /include/gps_common/conversions.h: -------------------------------------------------------------------------------- 1 | /* Taken from utexas-art-ros-pkg:art_vehicle/applanix */ 2 | 3 | /* 4 | * Conversions between coordinate systems. 5 | * 6 | * Includes LatLong<->UTM. 7 | */ 8 | 9 | #ifndef _UTM_H 10 | #define _UTM_H 11 | 12 | /** @file 13 | 14 | @brief Universal Transverse Mercator transforms. 15 | 16 | Functions to convert (spherical) latitude and longitude to and 17 | from (Euclidean) UTM coordinates. 18 | 19 | @author Chuck Gantz- chuck.gantz@globalstar.com 20 | 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace gps_common 28 | { 29 | 30 | const double RADIANS_PER_DEGREE = M_PI/180.0; 31 | const double DEGREES_PER_RADIAN = 180.0/M_PI; 32 | 33 | // WGS84 Parameters 34 | const double WGS84_A = 6378137.0; // major axis 35 | const double WGS84_B = 6356752.31424518; // minor axis 36 | const double WGS84_F = 0.0033528107; // ellipsoid flattening 37 | const double WGS84_E = 0.0818191908; // first eccentricity 38 | const double WGS84_EP = 0.0820944379; // second eccentricity 39 | 40 | // UTM Parameters 41 | const double UTM_K0 = 0.9996; // scale factor 42 | const double UTM_FE = 500000.0; // false easting 43 | const double UTM_FN_N = 0.0; // false northing on north hemisphere 44 | const double UTM_FN_S = 10000000.0; // false northing on south hemisphere 45 | const double UTM_E2 = (WGS84_E*WGS84_E); // e^2 46 | const double UTM_E4 = (UTM_E2*UTM_E2); // e^4 47 | const double UTM_E6 = (UTM_E4*UTM_E2); // e^6 48 | const double UTM_EP2 = (UTM_E2/(1-UTM_E2)); // e'^2 49 | 50 | /** 51 | * Utility function to convert geodetic to UTM position 52 | * 53 | * Units in are floating point degrees (sign for east/west) 54 | * 55 | * Units out are meters 56 | */ 57 | static inline void UTM(double lat, double lon, double *x, double *y) 58 | { 59 | // constants 60 | const static double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256); 61 | const static double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024); 62 | const static double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024); 63 | const static double m3 = -(35*UTM_E6/3072); 64 | 65 | // compute the central meridian 66 | int cm = ((lon >= 0.0) 67 | ? ((int)lon - ((int)lon)%6 + 3) 68 | : ((int)lon - ((int)lon)%6 - 3)); 69 | 70 | // convert degrees into radians 71 | double rlat = lat * RADIANS_PER_DEGREE; 72 | double rlon = lon * RADIANS_PER_DEGREE; 73 | double rlon0 = cm * RADIANS_PER_DEGREE; 74 | 75 | // compute trigonometric functions 76 | double slat = sin(rlat); 77 | double clat = cos(rlat); 78 | double tlat = tan(rlat); 79 | 80 | // decide the false northing at origin 81 | double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S; 82 | 83 | double T = tlat * tlat; 84 | double C = UTM_EP2 * clat * clat; 85 | double A = (rlon - rlon0) * clat; 86 | double M = WGS84_A * (m0*rlat + m1*sin(2*rlat) 87 | + m2*sin(4*rlat) + m3*sin(6*rlat)); 88 | double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat); 89 | 90 | // compute the easting-northing coordinates 91 | *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A,3)/6 92 | + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A,5)/120); 93 | *y = fn + UTM_K0 * (M + V * tlat * (A*A/2 94 | + (5-T+9*C+4*C*C)*pow(A,4)/24 95 | + ((61-58*T+T*T+600*C-330*UTM_EP2) 96 | * pow(A,6)/720))); 97 | 98 | return; 99 | } 100 | 101 | 102 | /** 103 | * Determine the correct UTM letter designator for the 104 | * given latitude 105 | * 106 | * @returns 'Z' if latitude is outside the UTM limits of 84N to 80S 107 | * 108 | * Written by Chuck Gantz- chuck.gantz@globalstar.com 109 | */ 110 | static inline char UTMLetterDesignator(double Lat) 111 | { 112 | char LetterDesignator; 113 | 114 | if ((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X'; 115 | else if ((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W'; 116 | else if ((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V'; 117 | else if ((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U'; 118 | else if ((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T'; 119 | else if ((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S'; 120 | else if ((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R'; 121 | else if ((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q'; 122 | else if ((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P'; 123 | else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator = 'N'; 124 | else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator = 'M'; 125 | else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L'; 126 | else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K'; 127 | else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J'; 128 | else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H'; 129 | else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G'; 130 | else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F'; 131 | else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E'; 132 | else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D'; 133 | else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C'; 134 | // 'Z' is an error flag, the Latitude is outside the UTM limits 135 | else LetterDesignator = 'Z'; 136 | return LetterDesignator; 137 | } 138 | 139 | /** 140 | * Convert lat/long to UTM coords. Equations from USGS Bulletin 1532 141 | * 142 | * East Longitudes are positive, West longitudes are negative. 143 | * North latitudes are positive, South latitudes are negative 144 | * Lat and Long are in fractional degrees 145 | * 146 | * Written by Chuck Gantz- chuck.gantz@globalstar.com 147 | */ 148 | static inline void LLtoUTM(const double Lat, const double Long, 149 | double &UTMNorthing, double &UTMEasting, 150 | char* UTMZone) 151 | { 152 | double a = WGS84_A; 153 | double eccSquared = UTM_E2; 154 | double k0 = UTM_K0; 155 | 156 | double LongOrigin; 157 | double eccPrimeSquared; 158 | double N, T, C, A, M; 159 | 160 | //Make sure the longitude is between -180.00 .. 179.9 161 | double LongTemp = (Long+180)-int((Long+180)/360)*360-180; 162 | 163 | double LatRad = Lat*RADIANS_PER_DEGREE; 164 | double LongRad = LongTemp*RADIANS_PER_DEGREE; 165 | double LongOriginRad; 166 | int ZoneNumber; 167 | 168 | ZoneNumber = int((LongTemp + 180)/6) + 1; 169 | 170 | if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 ) 171 | ZoneNumber = 32; 172 | 173 | // Special zones for Svalbard 174 | if( Lat >= 72.0 && Lat < 84.0 ) 175 | { 176 | if( LongTemp >= 0.0 && LongTemp < 9.0 ) ZoneNumber = 31; 177 | else if( LongTemp >= 9.0 && LongTemp < 21.0 ) ZoneNumber = 33; 178 | else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35; 179 | else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37; 180 | } 181 | // +3 puts origin in middle of zone 182 | LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; 183 | LongOriginRad = LongOrigin * RADIANS_PER_DEGREE; 184 | 185 | //compute the UTM Zone from the latitude and longitude 186 | snprintf(UTMZone, 4, "%d%c", ZoneNumber, UTMLetterDesignator(Lat)); 187 | 188 | eccPrimeSquared = (eccSquared)/(1-eccSquared); 189 | 190 | N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad)); 191 | T = tan(LatRad)*tan(LatRad); 192 | C = eccPrimeSquared*cos(LatRad)*cos(LatRad); 193 | A = cos(LatRad)*(LongRad-LongOriginRad); 194 | 195 | M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64 - 5*eccSquared*eccSquared*eccSquared/256)*LatRad 196 | - (3*eccSquared/8 + 3*eccSquared*eccSquared/32 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad) 197 | + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 198 | - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad)); 199 | 200 | UTMEasting = (double)(k0*N*(A+(1-T+C)*A*A*A/6 201 | + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120) 202 | + 500000.0); 203 | 204 | UTMNorthing = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24 205 | + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720))); 206 | if(Lat < 0) 207 | UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere 208 | } 209 | 210 | static inline void LLtoUTM(const double Lat, const double Long, 211 | double &UTMNorthing, double &UTMEasting, 212 | std::string &UTMZone) { 213 | char zone_buf[] = {0, 0, 0, 0}; 214 | 215 | LLtoUTM(Lat, Long, UTMNorthing, UTMEasting, zone_buf); 216 | 217 | UTMZone = zone_buf; 218 | } 219 | 220 | 221 | /** 222 | * Converts UTM coords to lat/long. Equations from USGS Bulletin 1532 223 | * 224 | * East Longitudes are positive, West longitudes are negative. 225 | * North latitudes are positive, South latitudes are negative 226 | * Lat and Long are in fractional degrees. 227 | * 228 | * Written by Chuck Gantz- chuck.gantz@globalstar.com 229 | */ 230 | static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, 231 | const char* UTMZone, double& Lat, double& Long ) 232 | { 233 | double k0 = UTM_K0; 234 | double a = WGS84_A; 235 | double eccSquared = UTM_E2; 236 | double eccPrimeSquared; 237 | double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared)); 238 | double N1, T1, C1, R1, D, M; 239 | double LongOrigin; 240 | double mu, phi1Rad; 241 | double x, y; 242 | int ZoneNumber; 243 | char* ZoneLetter; 244 | 245 | x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude 246 | y = UTMNorthing; 247 | 248 | ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10); 249 | if((*ZoneLetter - 'N') < 0) 250 | { 251 | y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere 252 | } 253 | 254 | LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; //+3 puts origin in middle of zone 255 | 256 | eccPrimeSquared = (eccSquared)/(1-eccSquared); 257 | 258 | M = y / k0; 259 | mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256)); 260 | 261 | phi1Rad = mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu) 262 | + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu) 263 | +(151*e1*e1*e1/96)*sin(6*mu); 264 | 265 | N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad)); 266 | T1 = tan(phi1Rad)*tan(phi1Rad); 267 | C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad); 268 | R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5); 269 | D = x/(N1*k0); 270 | 271 | Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24 272 | +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720); 273 | Lat = Lat * DEGREES_PER_RADIAN; 274 | 275 | Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1) 276 | *D*D*D*D*D/120)/cos(phi1Rad); 277 | Long = LongOrigin + Long * DEGREES_PER_RADIAN; 278 | 279 | } 280 | 281 | static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting, 282 | std::string UTMZone, double& Lat, double& Long) { 283 | UTMtoLL(UTMNorthing, UTMEasting, UTMZone.c_str(), Lat, Long); 284 | } 285 | 286 | } // end namespace UTM 287 | 288 | #endif // _UTM_H 289 | -------------------------------------------------------------------------------- /include/integrationBase.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) 2016 Tong Qin 2 | // The Hong Kong University of Science and Technology 3 | 4 | #ifndef INCLUDE_INTEGRATIONBASE_H_ 5 | #define INCLUDE_INTEGRATIONBASE_H_ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | using namespace Eigen; 20 | using namespace filter; 21 | 22 | namespace integration { 23 | enum StateOrder { O_R = 0, O_P = 3, O_V = 6, O_BA = 9, O_BG = 12 }; 24 | 25 | enum NoiseOrder { O_AN = 0, O_GN = 3, O_AW = 6, O_GW = 9 }; 26 | 27 | const double ACC_N = 1e-4; 28 | const double GYR_N = 1e-4; 29 | const double ACC_W = 1e-8; 30 | const double GYR_W = 1e-8; 31 | 32 | // 预积分基类 33 | class IntegrationBase { 34 | public: 35 | IntegrationBase() = delete; 36 | IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, 37 | const Eigen::Vector3d &_linearized_ba, 38 | const Eigen::Vector3d &_linearized_bg) 39 | : acc_0{_acc_0}, 40 | gyr_0{_gyr_0}, 41 | linearized_acc{_acc_0}, 42 | linearized_gyr{_gyr_0}, 43 | linearized_ba{_linearized_ba}, 44 | linearized_bg{_linearized_bg}, 45 | jacobian{Eigen::Matrix::Identity()}, 46 | covariance{Eigen::Matrix::Zero()}, 47 | sum_dt{0.0}, 48 | delta_p{Eigen::Vector3d::Zero()}, 49 | delta_q{Eigen::Quaterniond::Identity()}, 50 | delta_v{Eigen::Vector3d::Zero()} 51 | 52 | {} 53 | 54 | // 将时间戳、加速度数据、陀螺仪数据压入buf 55 | void push_back(double dt, const Eigen::Vector3d &acc, 56 | const Eigen::Vector3d &gyr) { 57 | dt_buf.push_back(dt); 58 | acc_buf.push_back(acc); 59 | gyr_buf.push_back(gyr); 60 | // 计算预积分进行状态传递? 61 | propagate(dt, acc, gyr); 62 | } 63 | 64 | void midPointIntegration( 65 | double _dt, const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, 66 | const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1, 67 | const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, 68 | const Eigen::Vector3d &delta_v, const Eigen::Vector3d &linearized_ba, 69 | const Eigen::Vector3d &linearized_bg, Eigen::Vector3d &result_delta_p, 70 | Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v, 71 | Eigen::Vector3d &result_linearized_ba, 72 | Eigen::Vector3d &result_linearized_bg, bool update_jacobian) { 73 | // 将acc_0乘以上次的姿态误差状态delta_q变换到上一帧最可能的方向? 74 | Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); 75 | // 求两次角速度测量的中点 76 | Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; 77 | // 姿态量的误差状态? 78 | result_delta_q = 79 | delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, 80 | un_gyr(2) * _dt / 2); 81 | // 通过本帧的姿态误差状态矫正acc_1到最可能的方向? 82 | Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba); 83 | // 求两次加速度测量的中点 84 | Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); 85 | // 匀加速运动模型公式求position bias和velocity bias 86 | result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt; 87 | result_delta_v = delta_v + un_acc * _dt; 88 | 89 | // acceleration bias和gyroscope bias 90 | result_linearized_ba = linearized_ba; 91 | result_linearized_bg = linearized_bg; 92 | 93 | // 默认更新雅克比, 也就是imu误差模型的线性连续时间模型公式中的F 94 | if (update_jacobian) { 95 | // 不就是前面的un_gyr嘛 96 | Vector3d w_x = 97 | 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; // angular_velocity 98 | Vector3d a_0_x = 99 | _acc_0 - linearized_ba; // acceleration measurement - bias 100 | Vector3d a_1_x = _acc_1 - linearized_ba; 101 | Matrix3d R_w_x, R_a_0_x, R_a_1_x; 102 | 103 | // 将上面的3d vector经叉乘器转换为3*3矩阵 104 | R_w_x << 0, -w_x(2), w_x(1), // [w-b]x, cross product 105 | w_x(2), 0, -w_x(0), -w_x(1), w_x(0), 0; 106 | R_a_0_x << 0, -a_0_x(2), a_0_x(1), // [w-a]x 107 | a_0_x(2), 0, -a_0_x(0), -a_0_x(1), a_0_x(0), 0; 108 | R_a_1_x << 0, -a_1_x(2), a_1_x(1), a_1_x(2), 0, -a_1_x(0), -a_1_x(1), 109 | a_1_x(0), 0; 110 | 111 | // the order of a and theta is exchanged. and F = I + F*dt + 0.5*F^2*dt^2 112 | // 这里的预积分后面再推导 113 | MatrixXd F = MatrixXd::Zero(15, 15); 114 | F.block<3, 3>(GlobalState::pos_, GlobalState::pos_) = 115 | Matrix3d::Identity(); 116 | F.block<3, 3>(GlobalState::pos_, GlobalState::att_) = 117 | -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 118 | -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * 119 | (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt; 120 | F.block<3, 3>(GlobalState::pos_, GlobalState::vel_) = 121 | MatrixXd::Identity(3, 3) * _dt; 122 | F.block<3, 3>(GlobalState::pos_, GlobalState::acc_) = 123 | -0.25 * 124 | (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * 125 | _dt * _dt; 126 | F.block<3, 3>(GlobalState::pos_, GlobalState::gyr_) = 127 | -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 128 | -_dt; 129 | 130 | F.block<3, 3>(GlobalState::att_, GlobalState::att_) = 131 | Matrix3d::Identity() - R_w_x * _dt; 132 | F.block<3, 3>(GlobalState::att_, GlobalState::gyr_) = 133 | -1.0 * MatrixXd::Identity(3, 3) * _dt; 134 | 135 | F.block<3, 3>(GlobalState::vel_, GlobalState::att_) = 136 | -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 137 | -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * 138 | (Matrix3d::Identity() - R_w_x * _dt) * _dt; 139 | F.block<3, 3>(GlobalState::vel_, GlobalState::vel_) = 140 | Matrix3d::Identity(); 141 | F.block<3, 3>(GlobalState::vel_, GlobalState::acc_) = 142 | -0.5 * 143 | (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * 144 | _dt; 145 | F.block<3, 3>(GlobalState::vel_, GlobalState::gyr_) = 146 | -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt; 147 | 148 | F.block<3, 3>(GlobalState::acc_, GlobalState::acc_) = 149 | Matrix3d::Identity(); 150 | F.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_) = 151 | Matrix3d::Identity(); 152 | 153 | jacobian = F * jacobian; 154 | } 155 | 156 | /* 157 | Vector3d vk0 = _acc_0*dt; 158 | Vector3d ak0 = _gyr_0*dt; 159 | Vector3d vk1 = _acc_1*dt; 160 | Vector3d ak1 = _gyr_1*dt; 161 | 162 | Vector3d dv = vk1 + 0.5*ak1.cross(vk1) + 1.0/12*(ak0.cross(vk1) + 163 | vk0.cross(ak1)); Vector3d da = 0.5*(ak0+ak1); 164 | 165 | result_delta_q = delta_q * Quaterniond(1, da(0)/2, da(1)/2, da(2)/2); 166 | result_delta_v = delta_v + result_delta_q*dv; 167 | Vector3d aver_v = 0.5*(result_delta_v + delta_v); 168 | result_delta_p = delta_p + aver_v * _dt; 169 | 170 | result_linearized_ba = linearized_ba; 171 | result_linearized_bg = linearized_bg;*/ 172 | } 173 | 174 | // estimate alpha, beta, and gamma in eqa (5) 175 | void propagate(double _dt, const Eigen::Vector3d &_acc_1, 176 | const Eigen::Vector3d &_gyr_1) { 177 | dt = _dt; // 新的时间增量 178 | acc_1 = _acc_1; // 新的加速度计数据 179 | gyr_1 = _gyr_1; // 新的角速度 180 | // 误差状态 181 | Vector3d result_delta_p; // 预积分的position增量？ 182 | Quaterniond result_delta_q; // 预积分的attitude增量？ 183 | Vector3d result_delta_v; // 预积分的velocity增量？ 184 | Vector3d result_linearized_ba; // 预积分的acceleration bias增量？ 185 | Vector3d result_linearized_bg; // 预积分的gyroscope bias增量？ 186 | 187 | // 通过预积分更新误差状态? 188 | midPointIntegration(_dt, acc_0, gyr_0, acc_1, _gyr_1, delta_p, delta_q, 189 | delta_v, linearized_ba, linearized_bg, result_delta_p, 190 | result_delta_q, result_delta_v, result_linearized_ba, 191 | result_linearized_bg, 1); 192 | 193 | // checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v, 194 | // linearized_ba, linearized_bg); 195 | delta_p = result_delta_p; // position 196 | delta_q = result_delta_q; // attitude 197 | delta_v = result_delta_v; // velocity 198 | linearized_ba = result_linearized_ba; // acceleration bias 199 | linearized_bg = result_linearized_bg; // gyroscope bias 200 | delta_q.normalize(); // 姿态四元数归一化 201 | sum_dt += dt; 202 | acc_0 = acc_1; 203 | gyr_0 = gyr_1; 204 | } 205 | 206 | void setBa(const Eigen::Vector3d &ba) { linearized_ba = ba; } 207 | 208 | void setBg(const Eigen::Vector3d &bg) { linearized_bg = bg; } 209 | 210 | Eigen::Vector3d G; 211 | 212 | double dt; 213 | Eigen::Vector3d acc_0, gyr_0; 214 | Eigen::Vector3d acc_1, gyr_1; 215 | 216 | const Eigen::Vector3d linearized_acc, linearized_gyr; 217 | Eigen::Vector3d linearized_ba, linearized_bg; 218 | 219 | Eigen::Matrix jacobian, covariance; 220 | Eigen::Matrix step_jacobian; 221 | Eigen::Matrix step_V; 222 | Eigen::Matrix noise; 223 | 224 | double sum_dt; 225 | Eigen::Vector3d delta_p; 226 | Eigen::Quaterniond delta_q; 227 | Eigen::Vector3d delta_v; 228 | 229 | std::vector dt_buf; 230 | std::vector acc_buf; 231 | std::vector gyr_buf; 232 | }; 233 | } // namespace integration 234 | 235 | #endif // INCLUDE_INTEGRATIONBASE_H_ 236 | 237 | -------------------------------------------------------------------------------- /include/math_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_MATH_UTILS_H_ 2 | #define INCLUDE_MATH_UTILS_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | typedef Eigen::Vector3d V3D; 12 | typedef Eigen::Matrix3d M3D; 13 | typedef Eigen::VectorXd VXD; 14 | typedef Eigen::MatrixXd MXD; 15 | typedef Eigen::Quaterniond Q4D; 16 | 17 | namespace math_utils { 18 | 19 | template 20 | static int sign(Derived x) { 21 | if (x >= static_cast(0)) 22 | return 1; 23 | else 24 | return -1; 25 | } 26 | 27 | template 28 | static Type wrap_pi(Type x) { 29 | while (x >= Type(M_PI)) { 30 | x -= Type(2.0 * M_PI); 31 | } 32 | 33 | while (x < Type(-M_PI)) { 34 | x += Type(2.0 * M_PI); 35 | } 36 | return x; 37 | } 38 | 39 | static void enforceSymmetry(Eigen::MatrixXd &mat) { 40 | mat = 0.5 * (mat + mat.transpose()).eval(); 41 | } 42 | 43 | static Eigen::Quaterniond axis2Quat(const Eigen::Vector3d &axis, double theta) { 44 | Eigen::Quaterniond q; 45 | 46 | if (theta < 1e-10) { 47 | q.w() = 1.0; 48 | q.x() = q.y() = q.z() = 0; 49 | } 50 | 51 | double magnitude = sin(theta / 2.0f); 52 | 53 | q.w() = cos(theta / 2.0f); 54 | q.x() = axis(0) * magnitude; 55 | q.y() = axis(1) * magnitude; 56 | q.z() = axis(2) * magnitude; 57 | 58 | return q; 59 | } 60 | 61 | static Eigen::Quaterniond axis2Quat(const Eigen::Vector3d &vec) { 62 | Eigen::Quaterniond q; 63 | double theta = vec.norm(); 64 | 65 | if (theta < 1e-10) { 66 | q.w() = 1.0; 67 | q.x() = q.y() = q.z() = 0; 68 | return q; 69 | } 70 | 71 | Eigen::Vector3d tmp = vec / theta; 72 | return axis2Quat(tmp, theta); 73 | } 74 | 75 | static Eigen::Vector3d Quat2axis(const Eigen::Quaterniond &q) { 76 | double axis_magnitude = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z()); 77 | Eigen::Vector3d vec; 78 | vec(0) = q.x(); 79 | vec(1) = q.y(); 80 | vec(2) = q.z(); 81 | 82 | if (axis_magnitude >= 1e-10) { 83 | vec = vec / axis_magnitude; 84 | vec = vec * wrap_pi(2.0 * atan2(axis_magnitude, q.w())); 85 | } 86 | 87 | return vec; 88 | } 89 | 90 | template 91 | static Eigen::Matrix ypr2R( 92 | const Eigen::MatrixBase &ypr) { 93 | typedef typename Derived::Scalar Scalar_t; 94 | 95 | Scalar_t y = ypr(0); 96 | Scalar_t p = ypr(1); 97 | Scalar_t r = ypr(2); 98 | 99 | Eigen::Matrix Rz; 100 | Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1; 101 | 102 | Eigen::Matrix Ry; 103 | Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p); 104 | 105 | Eigen::Matrix Rx; 106 | Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r); 107 | 108 | return Rz * Ry * Rx; 109 | } 110 | 111 | // void setRPY(const Derived& roll, const Derived& pitch, const Derived& yaw) 112 | static Eigen::Quaterniond ypr2Quat(const Eigen::Vector3d &ypr) { 113 | double halfYaw = double(ypr(0)) * double(0.5); 114 | double halfPitch = double(ypr(1)) * double(0.5); 115 | double halfRoll = double(ypr(2)) * double(0.5); 116 | double cosYaw = cos(halfYaw); 117 | double sinYaw = sin(halfYaw); 118 | double cosPitch = cos(halfPitch); 119 | double sinPitch = sin(halfPitch); 120 | double cosRoll = cos(halfRoll); 121 | double sinRoll = sin(halfRoll); 122 | Eigen::Quaterniond Q; 123 | Q.x() = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; 124 | Q.y() = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; 125 | Q.z() = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; 126 | Q.w() = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; 127 | Q.normalized(); 128 | return Q; 129 | } 130 | 131 | static Eigen::Quaterniond rpy2Quat(const Eigen::Vector3d &rpy) { 132 | double halfYaw = double(rpy(2)) * double(0.5); 133 | double halfPitch = double(rpy(1)) * double(0.5); 134 | double halfRoll = double(rpy(0)) * double(0.5); 135 | double cosYaw = cos(halfYaw); 136 | double sinYaw = sin(halfYaw); 137 | double cosPitch = cos(halfPitch); 138 | double sinPitch = sin(halfPitch); 139 | double cosRoll = cos(halfRoll); 140 | double sinRoll = sin(halfRoll); 141 | Eigen::Quaterniond Q; 142 | Q.x() = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw; 143 | Q.y() = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw; 144 | Q.z() = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw; 145 | Q.w() = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw; 146 | Q.normalized(); 147 | return Q; 148 | } 149 | 150 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) { 151 | Eigen::Vector3d ypr; 152 | // ypr(1) = -asin(R(2,0)); //pitch 153 | ypr(1) = atan2(-R(2, 0), sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2))); 154 | ypr(2) = atan2(R(2, 1) / cos(ypr(1)), R(2, 2) / cos(ypr(1))); // roll 155 | ypr(0) = atan2(R(1, 0) / cos(ypr(1)), R(0, 0) / cos(ypr(1))); // yaw 156 | return ypr; 157 | } 158 | 159 | static Eigen::Vector3d Quat2ypr(const Eigen::Quaterniond &Q) { 160 | return R2ypr(Q.toRotationMatrix()); 161 | } 162 | 163 | template 164 | static Eigen::Matrix rpy2R( 165 | const Eigen::MatrixBase &rpy) { 166 | typedef typename Derived::Scalar Scalar_t; 167 | 168 | Scalar_t r = rpy(0); 169 | Scalar_t p = rpy(1); 170 | Scalar_t y = rpy(2); 171 | 172 | Eigen::Matrix Rz; 173 | Rz << cos(y), -sin(y), 0, sin(y), cos(y), 0, 0, 0, 1; 174 | 175 | Eigen::Matrix Ry; 176 | Ry << cos(p), 0., sin(p), 0., 1., 0., -sin(p), 0., cos(p); 177 | 178 | Eigen::Matrix Rx; 179 | Rx << 1., 0., 0., 0., cos(r), -sin(r), 0., sin(r), cos(r); 180 | 181 | return Rz * Ry * Rx; 182 | } 183 | 184 | static Eigen::Vector3d R2rpy(const Eigen::Matrix3d &R) { 185 | Eigen::Vector3d rpy; 186 | rpy(1) = atan2(-R(2, 0), sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2))); 187 | rpy(0) = atan2(R(2, 1) / cos(rpy(1)), R(2, 2) / cos(rpy(1))); // roll 188 | rpy(2) = atan2(R(1, 0) / cos(rpy(1)), R(0, 0) / cos(rpy(1))); // yaw 189 | return rpy; 190 | } 191 | 192 | static Eigen::Vector3d Q2rpy(const Eigen::Quaterniond &Q) { 193 | return R2rpy(Q.toRotationMatrix()); 194 | } 195 | 196 | template 197 | static Eigen::Matrix skew( 198 | const Eigen::MatrixBase &q) { 199 | Eigen::Matrix ans; 200 | ans << typename Derived::Scalar(0), -q(2), q(1), q(2), 201 | typename Derived::Scalar(0), -q(0), -q(1), q(0), 202 | typename Derived::Scalar(0); 203 | return ans; 204 | } 205 | 206 | template 207 | static Eigen::Quaternion positify( 208 | const Eigen::QuaternionBase &q) { 209 | // printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 210 | Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 211 | // printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 212 | return q.template w() >= (typename Derived::Scalar)(0.0) 213 | ? q 214 | : Eigen::Quaternion(-q.w(), -q.x(), 215 | -q.y(), -q.z()); 216 | // return q; 217 | } 218 | 219 | template 220 | static Eigen::Quaternion deltaQ( 221 | const Eigen::MatrixBase &theta) { 222 | typedef typename Derived::Scalar Scalar_t; 223 | 224 | Eigen::Quaternion dq; 225 | Eigen::Matrix half_theta = theta; 226 | half_theta /= static_cast(2.0); 227 | dq.w() = static_cast(1.0); 228 | dq.x() = half_theta.x(); 229 | dq.y() = half_theta.y(); 230 | dq.z() = half_theta.z(); 231 | return dq; 232 | } 233 | 234 | template 235 | static T rad2deg(const T &radians) { 236 | return radians * 180.0 / M_PI; 237 | } 238 | 239 | template 240 | static T deg2rad(const T °rees) { 241 | return degrees * M_PI / 180.0; 242 | } 243 | 244 | template 245 | static Eigen::MatrixBase rad2deg( 246 | const Eigen::MatrixBase radians) { 247 | return Eigen::MatrixBase(rad2deg(radians(0)), rad2deg(radians(1)), 248 | rad2deg(radians(2))); 249 | } 250 | 251 | template 252 | static Eigen::MatrixBase deg2rag( 253 | const Eigen::MatrixBase degrees) { 254 | return Eigen::MatrixBase(deg2rad(degrees(0)), deg2rad(degrees(1)), 255 | deg2rad(degrees(2))); 256 | } 257 | 258 | template 259 | static Eigen::Matrix Qleft( 260 | const Eigen::QuaternionBase &q) { 261 | Eigen::Quaternion qq = positify(q); 262 | Eigen::Matrix ans; 263 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 264 | ans.template block<3, 1>( 265 | 1, 0) = qq.vec(), 266 | ans.template block<3, 3>(1, 1) = 267 | qq.w() * 268 | Eigen::Matrix::Identity() + 269 | skew(qq.vec()); 270 | return ans; 271 | } 272 | 273 | template 274 | static Eigen::Matrix Qright( 275 | const Eigen::QuaternionBase &p) { 276 | Eigen::Quaternion pp = positify(p); 277 | Eigen::Matrix ans; 278 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 279 | ans.template block<3, 1>( 280 | 1, 0) = pp.vec(), 281 | ans.template block<3, 3>(1, 1) = 282 | pp.w() * 283 | Eigen::Matrix::Identity() - 284 | skew(pp.vec()); 285 | return ans; 286 | } 287 | 288 | template 289 | static Eigen::Matrix Rleft( 290 | const Eigen::MatrixBase axis) { 291 | typedef typename Derived::Scalar Scalar_t; 292 | 293 | Eigen::Matrix ans; 294 | Scalar_t theta = axis.norm(); 295 | Eigen::Matrix a = axis / theta; 296 | double s = sin(theta) / theta; 297 | double c = (1 - cos(theta)) / theta; 298 | ans = s * Eigen::Matrix::Identity() + 299 | (1 - s) * a * a.transpose() + c * skew(a); 300 | 301 | return ans; 302 | } 303 | 304 | static Eigen::Matrix Rinvleft( 305 | const Eigen::Matrix axis) { 306 | Eigen::Matrix ans; 307 | double theta = axis.norm(); 308 | 309 | if (theta < 1e-10) { 310 | ans.setIdentity(); 311 | return ans; 312 | } 313 | 314 | double half_theta = theta / 2.0; 315 | Eigen::Matrix a = axis / axis.norm(); 316 | double cot_half_theta = cos(half_theta) / sin(half_theta); 317 | double s = half_theta * cot_half_theta; 318 | ans = s * Eigen::Matrix::Identity() + 319 | (1.0 - s) * a * a.transpose() - half_theta * skew(a); 320 | return ans; 321 | } 322 | 323 | } // namespace math_utils 324 | 325 | #endif // INCLUDE_MATH_UTILS_H_ 326 | -------------------------------------------------------------------------------- /include/parameters.h: -------------------------------------------------------------------------------- 1 | // This file is part of LINS. 2 | // 3 | // Copyright (C) 2020 Chao Qin , 4 | // Robotics and Multiperception Lab (RAM-LAB ), 5 | // The Hong Kong University of Science and Technology 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | 18 | #ifndef INCLUDE_PARAMETERS_H_ 19 | #define INCLUDE_PARAMETERS_H_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include 52 | #include 53 | 54 | #include "cloud_msgs/cloud_info.h" 55 | 56 | #include 57 | 58 | #include 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | #include 65 | 66 | 67 | #define PI (3.14159265) 68 | 69 | using namespace std; 70 | 71 | //the following are UBUNTU/LINUX ONLY terminal color codes. 72 | #define RESET "\033[0m" 73 | #define BLACK "\033[30m" /* Black */ 74 | #define RED "\033[31m" /* Red */ 75 | #define GREEN "\033[32m" /* Green */ 76 | #define YELLOW "\033[33m" /* Yellow */ 77 | #define BLUE "\033[34m" /* Blue */ 78 | #define MAGENTA "\033[35m" /* Magenta */ 79 | #define CYAN "\033[36m" /* Cyan */ 80 | #define WHITE "\033[37m" /* White */ 81 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 82 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 83 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 84 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 85 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 86 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 87 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 88 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 89 | 90 | 91 | struct smoothness_t { 92 | float value; 93 | size_t ind; 94 | }; 95 | 96 | struct by_value { 97 | bool operator()(smoothness_t const &left, smoothness_t const &right) { 98 | return left.value < right.value; 99 | } 100 | }; 101 | 102 | /* 103 | * A point cloud type that has "ring" channel 104 | */ 105 | struct PointXYZIR 106 | { 107 | PCL_ADD_POINT4D 108 | PCL_ADD_INTENSITY; 109 | uint16_t ring; 110 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 111 | } EIGEN_ALIGN16; 112 | 113 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR, 114 | (float, x, x) (float, y, y) 115 | (float, z, z) (float, intensity, intensity) 116 | (uint16_t, ring, ring) 117 | ) 118 | 119 | /* 120 | * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) 121 | */ 122 | struct PointXYZIRPYT 123 | { 124 | PCL_ADD_POINT4D 125 | PCL_ADD_INTENSITY; 126 | float roll; 127 | float pitch; 128 | float yaw; 129 | double time; 130 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 131 | } EIGEN_ALIGN16; 132 | 133 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, 134 | (float, x, x) (float, y, y) 135 | (float, z, z) (float, intensity, intensity) 136 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 137 | (double, time, time) 138 | ) 139 | 140 | typedef PointXYZIRPYT PointTypePose; 141 | // -------------------------------------------------------------------------------------------------------- 142 | 143 | typedef pcl::PointXYZI PointType; 144 | 145 | typedef Eigen::Vector3d V3D; 146 | typedef Eigen::Matrix3d M3D; 147 | typedef Eigen::VectorXd VXD; 148 | typedef Eigen::MatrixXd MXD; 149 | typedef Eigen::Quaterniond Q4D; 150 | 151 | namespace parameter { 152 | 153 | /*!@EARTH COEFFICIENTS */ 154 | const double G0 = 9.81; // gravity 155 | const double deg = M_PI / 180.0; // degree 156 | const double rad = 180.0 / M_PI; // radian 157 | const double dph = deg / 3600.0; // degree per hour 158 | const double dpsh = deg / sqrt(3600.0); // degree per square-root hour 159 | const double mg = G0 / 1000.0; // mili-gravity force 160 | const double ug = mg / 1000.0; // micro-gravity force 161 | const double mgpsHz = mg / sqrt(1.0); // mili-gravity force per second 162 | const double ugpsHz = ug / sqrt(1.0); // micro-gravity force per second 163 | const double Re = 6378137.0; ///< WGS84 Equatorial radius in meters 164 | const double Rp = 6356752.31425; 165 | const double Ef = 1.0 / 298.257223563; 166 | const double Wie = 7.2921151467e-5; 167 | const double Ee = 0.0818191908425; 168 | const double EeEe = Ee * Ee; 169 | 170 | // IMU 内参 171 | const float accel_noise_sigma = 3.9939570888238808e-03; 172 | const float gyro_noise_sigma = 1.5636343949698187e-03; 173 | const float accel_bias_rw_sigma = 6.4356659353532566e-05; 174 | const float gyro_bias_rw_sigma = 3.5640318696367613e-05; 175 | const float imuGravity = 9.80511; 176 | 177 | /*!@SLAM COEFFICIENTS */ 178 | const bool loopClosureEnableFlag = true; 179 | const double mappingProcessInterval = 0.3; 180 | 181 | const float ang_res_x = 0.2; 182 | const float ang_res_y = 2.0; 183 | const float ang_bottom = 15.0 + 0.1; 184 | const int groundScanInd = 5; 185 | 186 | // HDL-32E 187 | // const int N_SCAN = 32; 188 | // const int Horizon_SCAN = 1800; 189 | // const float ang_res_x = 0.2; 190 | // const float ang_res_y = 1.33; 191 | // const float ang_bottom = 30.67; 192 | // const int groundScanInd = 7; 193 | 194 | // const float ang_res_x = 0.2; 195 | // const float ang_res_y = 0.427; 196 | // const float ang_bottom = 24.9; 197 | // const int groundScanInd = 50; 198 | 199 | const int systemDelay = 0; 200 | const float sensorMountAngle = 0.0; 201 | const float segmentTheta = 1.0472; 202 | const int segmentValidPointNum = 5; 203 | const int segmentValidLineNum = 3; 204 | const float segmentAlphaX = ang_res_x / 180.0 * M_PI; 205 | const float segmentAlphaY = ang_res_y / 180.0 * M_PI; 206 | const int edgeFeatureNum = 2; 207 | const int surfFeatureNum = 4; 208 | const int sectionsTotal = 6; 209 | const float surroundingKeyframeSearchRadius = 50.0; 210 | const int surroundingKeyframeSearchNum = 50; 211 | const float historyKeyframeSearchRadius = 10.0; 212 | const int historyKeyframeSearchNum = 25; 213 | const float historyKeyframeFitnessScore = 0.08; 214 | const float globalMapVisualizationSearchRadius = 1500.0; 215 | 216 | // !@ENABLE_CALIBRATION 217 | extern int CALIBARTE_IMU; 218 | extern int SHOW_CONFIGURATION; 219 | extern int AVERAGE_NUMS; 220 | 221 | // !@INITIAL_PARAMETERS 222 | extern double IMU_LIDAR_EXTRINSIC_ANGLE; 223 | extern double IMU_MISALIGN_ANGLE; 224 | 225 | // !@LIDAR_PARAMETERS 226 | extern int LINE_NUM; 227 | extern int SCAN_NUM; 228 | extern double SCAN_PERIOD; 229 | extern double EDGE_THRESHOLD; 230 | extern double SURF_THRESHOLD; 231 | extern double NEAREST_FEATURE_SEARCH_SQ_DIST; 232 | 233 | // !@TESTING 234 | extern int VERBOSE; 235 | extern int ICP_FREQ; 236 | extern int MAX_LIDAR_NUMS; 237 | extern int NUM_ITER; 238 | extern double LIDAR_SCALE; 239 | extern double LIDAR_STD; 240 | 241 | // !@SUB_TOPIC_NAME 242 | extern std::string IMU_TOPIC; 243 | extern std::string LIDAR_TOPIC; 244 | 245 | // !@PUB_TOPIC_NAME 246 | extern std::string LIDAR_ODOMETRY_TOPIC; 247 | extern std::string LIDAR_MAPPING_TOPIC; 248 | 249 | // !@KALMAN_FILTER 250 | extern double ACC_N; 251 | extern double ACC_W; 252 | extern double GYR_N; 253 | extern double GYR_W; 254 | extern V3D INIT_POS_STD; 255 | extern V3D INIT_VEL_STD; 256 | extern V3D INIT_ATT_STD; 257 | extern V3D INIT_ACC_STD; 258 | extern V3D INIT_GYR_STD; 259 | 260 | // !@INITIAL IMU BIASES 261 | extern V3D INIT_BA; 262 | extern V3D INIT_BW; 263 | 264 | // !@EXTRINSIC_PARAMETERS 265 | extern V3D INIT_TBL; 266 | extern Q4D INIT_RBL; 267 | 268 | extern double OriLon; 269 | extern double OriLat; 270 | extern double OriAlt; 271 | extern double OriYaw; 272 | extern double OriPitch; 273 | extern double OriRoll; 274 | extern double compensate_init_yaw; 275 | extern double compensate_init_pitch; 276 | extern double compensate_init_roll; 277 | extern double mappingCarYawPara; 278 | extern double InitPose_x; 279 | extern double InitPose_y; 280 | extern double InitPose_yaw; 281 | 282 | void readParameters(ros::NodeHandle& n); 283 | void readInitPose(ros::NodeHandle& n); 284 | 285 | void readV3D(cv::FileStorage* file, const std::string& name, V3D& vec_eigen); 286 | 287 | void readQ4D(cv::FileStorage* file, const std::string& name, Q4D& quat_eigen); 288 | 289 | enum StateOrder { 290 | O_R = 0, 291 | O_P = 3, 292 | }; 293 | 294 | } // namespace parameter 295 | 296 | #endif // INCLUDE_PARAMETERS_H_ 297 | -------------------------------------------------------------------------------- /include/sc_lego_loam/KDTreeVectorOfVectorsAdaptor.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com). 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in the 15 | * documentation and/or other materials provided with the distribution. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | *************************************************************************/ 28 | 29 | #pragma once 30 | 31 | #include "nanoflann.hpp" 32 | 33 | #include 34 | 35 | // ===== This example shows how to use nanoflann with these types of containers: ======= 36 | //typedef std::vector > my_vector_of_vectors_t; 37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include 38 | // ===================================================================================== 39 | 40 | 41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage. 42 | * The i'th vector represents a point in the state space. 43 | * 44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations. 45 | * \tparam num_t The type of the point coordinates (typically, double or float). 46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. 47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int) 48 | */ 49 | template 50 | struct KDTreeVectorOfVectorsAdaptor 51 | { 52 | typedef KDTreeVectorOfVectorsAdaptor self_t; 53 | typedef typename Distance::template traits::distance_t metric_t; 54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; 55 | 56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index. 57 | 58 | /// Constructor: takes a const ref to the vector of vectors object with the data points 59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat) 60 | { 61 | assert(mat.size() != 0 && mat[0].size() != 0); 62 | const size_t dims = mat[0].size(); 63 | if (DIM>0 && static_cast(dims) != DIM) 64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument"); 65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) ); 66 | index->buildIndex(); 67 | } 68 | 69 | ~KDTreeVectorOfVectorsAdaptor() { 70 | delete index; 71 | } 72 | 73 | const VectorOfVectorsType &m_data; 74 | 75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]). 76 | * Note that this is a short-cut method for index->findNeighbors(). 77 | * The user can also call index->... methods as desired. 78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface. 79 | */ 80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const 81 | { 82 | nanoflann::KNNResultSet resultSet(num_closest); 83 | resultSet.init(out_indices, out_distances_sq); 84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); 85 | } 86 | 87 | /** @name Interface expected by KDTreeSingleIndexAdaptor 88 | * @{ */ 89 | 90 | const self_t & derived() const { 91 | return *this; 92 | } 93 | self_t & derived() { 94 | return *this; 95 | } 96 | 97 | // Must return the number of data points 98 | inline size_t kdtree_get_point_count() const { 99 | return m_data.size(); 100 | } 101 | 102 | // Returns the dim'th component of the idx'th point in the class: 103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const { 104 | return m_data[idx][dim]; 105 | } 106 | 107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 110 | template 111 | bool kdtree_get_bbox(BBOX & /*bb*/) const { 112 | return false; 113 | } 114 | 115 | /** @} */ 116 | 117 | }; // end of KDTreeVectorOfVectorsAdaptor 118 | -------------------------------------------------------------------------------- /include/sc_lego_loam/Scancontext.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | // include flann before OpenCV 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "nanoflann.hpp" 29 | #include "KDTreeVectorOfVectorsAdaptor.h" 30 | 31 | #include "tictoc.h" 32 | 33 | #include "LidarIris.h" 34 | 35 | using namespace Eigen; 36 | using namespace nanoflann; 37 | 38 | using std::cout; 39 | using std::endl; 40 | using std::make_pair; 41 | 42 | using std::atan2; 43 | using std::cos; 44 | using std::sin; 45 | 46 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context) 47 | using KeyMat = std::vector >; 48 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >; 49 | 50 | 51 | // namespace SC2 52 | // { 53 | 54 | void coreImportTest ( void ); 55 | 56 | 57 | // sc param-independent helper functions 58 | float xy2theta( const float & _x, const float & _y ); 59 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ); 60 | std::vector eig2stdvec( MatrixXd _eigmat ); 61 | 62 | 63 | class SCManager 64 | { 65 | public: 66 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care. 67 | 68 | // 构建scan context 69 | Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down ); 70 | // 构建ring key 71 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ); 72 | // 构建sector key 73 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ); 74 | 75 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 76 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18) 77 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18) 78 | 79 | // User-side API 80 | // point cloud --> scan context 81 | void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ); 82 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw 83 | 84 | public: 85 | // hyper parameters () 86 | // 雷达高度 87 | const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0. 88 | 89 | // 角度分区 90 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18) 91 | // 距离分区 92 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18) 93 | // 最远距离 94 | const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18) 95 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR); 96 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING); 97 | 98 | // tree 99 | // 回环检测时排除最近的50帧关键帧 100 | const int NUM_EXCLUDE_RECENT = 50; // simply just keyframe gap, but node position distance-based exclusion is ok. 101 | // 进行kd搜索时选取最接近的10帧 102 | const int NUM_CANDIDATES_FROM_TREE = 5; // 10 is enough. (refer the IROS 18 paper) 103 | 104 | // loop thres 105 | // 检测频率 106 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // but may well work for same-direction-revisits, not for reverse-revisits 107 | // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness) 108 | const double SC_DIST_THRES = 0.0; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold 109 | 110 | // config 111 | // 构建KD树的周期 112 | const int TREE_MAKING_PERIOD_ = 50; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost 113 | int tree_making_period_conter = 0; 114 | 115 | // data 116 | std::vector polarcontexts_timestamp_; // optional. 117 | // 存储scan context 118 | std::vector polarcontexts_; 119 | // 存储ring key 120 | std::vector polarcontext_invkeys_; 121 | // 存储sector key 122 | std::vector polarcontext_vkeys_; 123 | 124 | std::vector IrisFeatures_; 125 | 126 | KeyMat polarcontext_invkeys_mat_; 127 | KeyMat polarcontext_invkeys_to_search_; 128 | std::unique_ptr polarcontext_tree_; 129 | 130 | // 不能在类内对其他类进行参数初始化 131 | // https://www.zhihu.com/question/329974084 132 | // LidarIris iris(4, 18, 1.6, 0.75, 50); //error: expected identifier before numeric constant 133 | LidarIris LidarIris_ = LidarIris(4, 18, 1.6, 0.75, 50); 134 | 135 | }; // SCManager 136 | 137 | // } // namespace SC2 138 | -------------------------------------------------------------------------------- /include/sc_lego_loam/tictoc.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 | #include 10 | #include 11 | 12 | class TicToc_ 13 | { 14 | public: 15 | TicToc_() 16 | { 17 | tic(); 18 | } 19 | 20 | TicToc_( bool _disp ) 21 | { 22 | disp_ = _disp; 23 | tic(); 24 | } 25 | 26 | void tic() 27 | { 28 | start = std::chrono::system_clock::now(); 29 | } 30 | 31 | void toc( std::string _about_task ) 32 | { 33 | end = std::chrono::system_clock::now(); 34 | std::chrono::duration elapsed_seconds = end - start; 35 | double elapsed_ms = elapsed_seconds.count() * 1000; 36 | 37 | if( disp_ ) 38 | { 39 | std::cout.precision(3); // 10 for sec, 3 for ms 40 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl; 41 | } 42 | } 43 | 44 | private: 45 | std::chrono::time_point start, end; 46 | bool disp_ = false; 47 | }; 48 | -------------------------------------------------------------------------------- /include/sensor_utils.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of LINS. 2 | // 3 | // Copyright (C) 2020 Chao Qin , 4 | // Robotics and Multiperception Lab (RAM-LAB ), 5 | // The Hong Kong University of Science and Technology 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | 18 | #ifndef INCLUDE_SENSOR_UTILS_HPP_ 19 | #define INCLUDE_SENSOR_UTILS_HPP_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | using namespace math_utils; 29 | using namespace parameter; 30 | 31 | namespace sensor_utils { 32 | 33 | // Sensor measurement class 34 | class Measurement { 35 | public: 36 | Measurement() {} 37 | virtual ~Measurement() {} 38 | }; 39 | 40 | class Imu : public Measurement { 41 | public: 42 | Imu() {} 43 | Imu(double time, const V3D& acc, const V3D& gyr) 44 | : time(time), acc(acc), gyr(gyr) {} 45 | ~Imu() {} 46 | double time; 47 | V3D acc; // accelerometer measurement (m^2/sec) 48 | V3D gyr; // gyroscope measurement (rad/s) 49 | }; 50 | 51 | class Gps : public Measurement { 52 | public: 53 | Gps() { 54 | time = 0.0; 55 | lat = 0.0; 56 | lon = 0.0; 57 | alt = 0.0; 58 | } 59 | Gps(double time, double lat, double lon, double alt) 60 | : time(time), lat(lat), lon(lon), alt(alt) {} 61 | ~Gps() {} 62 | int status; 63 | double time; 64 | double lat; 65 | double lon; 66 | double alt; 67 | inline V3D pn() const { return V3D(lat, lon, alt); } 68 | }; 69 | 70 | class Odometry : public Measurement { 71 | public: 72 | Odometry() { 73 | time = 0.0; 74 | rotation.setIdentity(); 75 | translation.setZero(); 76 | } 77 | Odometry(double time, const Q4D& rotation, const V3D& translation) 78 | : time(time), rotation(rotation), translation(translation) {} 79 | ~Odometry() {} 80 | 81 | Odometry inverse(const Odometry& odom) { 82 | Odometry inv; 83 | inv.time = odom.time; 84 | inv.rotation = odom.rotation.inverse(); 85 | inv.translation = -inv.rotation.toRotationMatrix() * odom.translation; 86 | return inv; 87 | } 88 | 89 | void boxPlus(const Odometry& increment) { 90 | translation = 91 | rotation.toRotationMatrix() * increment.translation + translation; 92 | rotation = rotation * increment.rotation; 93 | } 94 | 95 | Odometry boxMinus(const Odometry& odom) { 96 | Odometry res; 97 | res.translation = 98 | odom.rotation.inverse().toRotationMatrix() * translation - 99 | odom.rotation.inverse().toRotationMatrix() * odom.translation; 100 | res.rotation = odom.rotation.inverse() * rotation; 101 | return res; 102 | } 103 | 104 | double time; 105 | Q4D rotation; 106 | V3D translation; 107 | }; 108 | 109 | class EarthParams { 110 | public: 111 | EarthParams() {} 112 | ~EarthParams() {} 113 | 114 | static M3D getDrp(const V3D& pn) { 115 | M3D Drp = M3D::Zero(); 116 | double latitude = pn(0); 117 | double longitude = pn(1); 118 | double height = pn(2); 119 | double sq = 1 - EeEe * sin(latitude) * sin(latitude); 120 | double RNh = Re / sqrt(sq) + height; // (2.5) 121 | double RMh = RNh * (1 - EeEe) / sq + height; // (2.4) 122 | Drp << 0, 1.0 / RMh, 0, 1.0 / (RNh * cos(latitude)), 0, 0, 0, 0, 1; 123 | return Drp; 124 | } 125 | 126 | static M3D getDpr(const V3D& pn) { 127 | M3D Drp = getDrp(pn); 128 | return Drp.inverse(); 129 | } 130 | 131 | }; 132 | 133 | } // namespace sensor_utils 134 | 135 | #endif // INCLUDE_SENSOR_UTILS_HPP_ 136 | 137 | -------------------------------------------------------------------------------- /include/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc { 8 | public: 9 | TicToc() { tic(); } 10 | 11 | void tic() { start = std::chrono::system_clock::now(); } 12 | 13 | double toc() { 14 | end = std::chrono::system_clock::now(); 15 | std::chrono::duration elapsed_seconds = end - start; 16 | return elapsed_seconds.count() * 1000; 17 | } 18 | 19 | private: 20 | std::chrono::time_point start, end; 21 | }; 22 | -------------------------------------------------------------------------------- /include/utility.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_UTILITY_H_ 2 | #define INCLUDE_UTILITY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "cloud_msgs/cloud_info.h" 41 | 42 | #define PI 3.14159265 43 | 44 | using namespace std; 45 | 46 | typedef pcl::PointXYZI PointType; 47 | 48 | // VLP-16 49 | extern const int N_SCAN = 16; 50 | extern const int Horizon_SCAN = 1800; 51 | extern const float ang_res_x = 0.2; 52 | extern const float ang_res_y = 2.0; 53 | extern const float ang_bottom = 15.0 + 0.1; 54 | extern const int groundScanInd = 5; 55 | 56 | // HDL-32E 57 | // extern const int N_SCAN = 32; 58 | // extern const int Horizon_SCAN = 1800; 59 | // extern const float ang_res_x = 360.0/float(Horizon_SCAN); 60 | // extern const float ang_res_y = 41.33/float(N_SCAN-1); 61 | // extern const float ang_bottom = 30.67; 62 | // extern const int groundScanInd = 20; 63 | 64 | // Ouster OS1-64 65 | // extern const int N_SCAN = 64; 66 | // extern const int Horizon_SCAN = 1024; 67 | // extern const float ang_res_x = 360.0/float(Horizon_SCAN); 68 | // extern const float ang_res_y = 33.2/float(N_SCAN-1); 69 | // extern const float ang_bottom = 16.6+0.1; 70 | // extern const int groundScanInd = 15; 71 | 72 | extern const bool loopClosureEnableFlag = true; 73 | extern const double mappingProcessInterval = 0.3; 74 | 75 | extern const float scanPeriod = 0.1; 76 | extern const int systemDelay = 0; 77 | extern const int imuQueLength = 200; 78 | extern const string imuTopic = "/vehicle/imu/data_raw"; 79 | 80 | extern const float sensorMountAngle = 0.0; 81 | extern const float segmentTheta = 1.0472; 82 | extern const int segmentValidPointNum = 5; 83 | extern const int segmentValidLineNum = 3; 84 | extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI; 85 | extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI; 86 | 87 | extern const int edgeFeatureNum = 2; 88 | extern const int surfFeatureNum = 4; 89 | extern const int sectionsTotal = 6; 90 | extern const float edgeThreshold = 0.5; 91 | extern const float surfThreshold = 0.5; 92 | extern const float nearestFeatureSearchSqDist = 25; 93 | 94 | extern const float surroundingKeyframeSearchRadius = 50.0; 95 | extern const int surroundingKeyframeSearchNum = 50; 96 | 97 | extern const float historyKeyframeSearchRadius = 5.0; 98 | extern const int historyKeyframeSearchNum = 25; 99 | extern const float historyKeyframeFitnessScore = 0.3; 100 | 101 | extern const float globalMapVisualizationSearchRadius = 500.0; 102 | 103 | struct smoothness_t { 104 | float value; 105 | size_t ind; 106 | }; 107 | 108 | struct by_value { 109 | bool operator()(smoothness_t const &left, smoothness_t const &right) { 110 | return left.value < right.value; 111 | } 112 | }; 113 | 114 | /* 115 | * A point cloud type that has "ring" channel 116 | */ 117 | struct PointXYZIR 118 | { 119 | PCL_ADD_POINT4D 120 | PCL_ADD_INTENSITY; 121 | uint16_t ring; 122 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 123 | } EIGEN_ALIGN16; 124 | 125 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR, 126 | (float, x, x) (float, y, y) 127 | (float, z, z) (float, intensity, intensity) 128 | (uint16_t, ring, ring) 129 | ) 130 | 131 | /* 132 | * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) 133 | */ 134 | struct PointXYZIRPYT 135 | { 136 | PCL_ADD_POINT4D 137 | PCL_ADD_INTENSITY; 138 | float roll; 139 | float pitch; 140 | float yaw; 141 | double time; 142 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 143 | } EIGEN_ALIGN16; 144 | 145 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, 146 | (float, x, x) (float, y, y) 147 | (float, z, z) (float, intensity, intensity) 148 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 149 | (double, time, time) 150 | ) 151 | 152 | typedef PointXYZIRPYT PointTypePose; 153 | 154 | #endif // INCLUDE_UTILITY_H_ 155 | 156 | -------------------------------------------------------------------------------- /include/xmldom/XmlDomDocument.h: -------------------------------------------------------------------------------- 1 | /* 2 | XmlDomDocument.cpp 3 | 4 | DOM parsing class interfaces. 5 | 6 | ------------------------------------------ 7 | 8 | Copyright (c) 2013 Vic Hargrave 9 | 10 | Licensed under the Apache License, Version 2.0 (the "License"); 11 | you may not use this file except in compliance with the License. 12 | You may obtain a copy of the License at 13 | 14 | http://www.apache.org/licenses/LICENSE-2.0 15 | 16 | Unless required by applicable law or agreed to in writing, software 17 | distributed under the License is distributed on an "AS IS" BASIS, 18 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | See the License for the specific language governing permissions and 20 | limitations under the License. 21 | */ 22 | 23 | #ifndef __XmlDomDocument_h__ 24 | #define __XmlDomDocument_h__ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | using namespace std; 35 | using namespace xercesc; 36 | 37 | class XmlDomDocument 38 | { 39 | DOMDocument* m_doc; 40 | 41 | public: 42 | XmlDomDocument(const char* xmlfile); 43 | ~XmlDomDocument(); 44 | 45 | string getChildValue(const char* parentTag, int parentIndex, const char* childTag, int childIndex); 46 | string getChildAttribute(const char* parentTag, int parentIndex, const char* childTag, int childIndex, 47 | const char* attributeTag); 48 | int getRootElementCount(const char* rootElementTag); 49 | int getChildCount(const char* parentTag, int parentIndex, const char* childTag); 50 | 51 | private: 52 | XmlDomDocument(); 53 | XmlDomDocument(const XmlDomDocument&); 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /launch/ImuOdometry.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5449735522270203 9 | Tree Height: 874 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: ~ 14 | Name: Tool Properties 15 | Splitter Ratio: 0.5886790156364441 16 | - Class: rviz/Views 17 | Expanded: 18 | - /Current View1 19 | Name: Views 20 | Splitter Ratio: 0.5 21 | - Class: rviz/Time 22 | Experimental: false 23 | Name: Time 24 | SyncMode: 0 25 | SyncSource: GPS Trajectory 26 | - Class: rviz/Help 27 | Name: Help 28 | Preferences: 29 | PromptSaveOnExit: true 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 0; 0; 0 39 | Enabled: false 40 | Line Style: 41 | Line Width: 0.029999999329447746 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 1000 51 | Reference Frame: map 52 | Value: false 53 | - Class: rviz/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: false 58 | local_map: 59 | Value: true 60 | map: 61 | Value: true 62 | odom_fix: 63 | Value: true 64 | odom_fusion: 65 | Value: true 66 | rvizgps: 67 | Value: true 68 | Marker Scale: 10 69 | Name: TF 70 | Show Arrows: true 71 | Show Axes: true 72 | Show Names: true 73 | Tree: 74 | map: 75 | local_map: 76 | odom_fix: 77 | {} 78 | odom_fusion: 79 | {} 80 | rvizgps: 81 | {} 82 | Update Interval: 0 83 | Value: true 84 | - Class: rviz/Axes 85 | Enabled: false 86 | Length: 2 87 | Name: map 88 | Radius: 0.30000001192092896 89 | Reference Frame: map 90 | Value: false 91 | - Alpha: 0 92 | Autocompute Intensity Bounds: true 93 | Autocompute Value Bounds: 94 | Max Value: 5.575240265898174e-6 95 | Min Value: -1.9999639987945557 96 | Value: true 97 | Axis: Z 98 | Channel Name: intensity 99 | Class: rviz/PointCloud2 100 | Color: 59; 0; 255 101 | Color Transformer: Intensity 102 | Decay Time: 0 103 | Enabled: true 104 | Invert Rainbow: true 105 | Max Color: 85; 0; 255 106 | Max Intensity: 255 107 | Min Color: 255; 255; 255 108 | Min Intensity: 255 109 | Name: GPS Trajectory 110 | Position Transformer: XYZ 111 | Queue Size: 10 112 | Selectable: true 113 | Size (Pixels): 4 114 | Size (m): 2 115 | Style: Points 116 | Topic: /imuodometry/gps_history_position 117 | Unreliable: false 118 | Use Fixed Frame: true 119 | Use rainbow: false 120 | Value: true 121 | - Alpha: 1 122 | Autocompute Intensity Bounds: true 123 | Autocompute Value Bounds: 124 | Max Value: 10 125 | Min Value: -10 126 | Value: true 127 | Axis: Z 128 | Channel Name: intensity 129 | Class: rviz/PointCloud2 130 | Color: 255; 255; 255 131 | Color Transformer: Intensity 132 | Decay Time: 0 133 | Enabled: true 134 | Invert Rainbow: false 135 | Max Color: 255; 255; 255 136 | Max Intensity: 0 137 | Min Color: 0; 0; 0 138 | Min Intensity: 0 139 | Name: IMU odometry 140 | Position Transformer: XYZ 141 | Queue Size: 10 142 | Selectable: true 143 | Size (Pixels): 4 144 | Size (m): 3 145 | Style: Points 146 | Topic: /imu/odometry 147 | Unreliable: false 148 | Use Fixed Frame: true 149 | Use rainbow: true 150 | Value: true 151 | - Alpha: 1 152 | Autocompute Intensity Bounds: true 153 | Autocompute Value Bounds: 154 | Max Value: 0.3741946518421173 155 | Min Value: 0.0007519922801293433 156 | Value: true 157 | Axis: Z 158 | Channel Name: intensity 159 | Class: rviz/PointCloud2 160 | Color: 255; 255; 255 161 | Color Transformer: Intensity 162 | Decay Time: 0 163 | Enabled: true 164 | Invert Rainbow: false 165 | Max Color: 255; 0; 0 166 | Max Intensity: 255 167 | Min Color: 0; 255; 0 168 | Min Intensity: 255 169 | Name: Fusion Trajectory 170 | Position Transformer: XYZ 171 | Queue Size: 10 172 | Selectable: true 173 | Size (Pixels): 4 174 | Size (m): 4 175 | Style: Points 176 | Topic: /imuodometry/fusion_position 177 | Unreliable: false 178 | Use Fixed Frame: true 179 | Use rainbow: false 180 | Value: true 181 | - Alpha: 0.800000011920929 182 | Blocks: 3 183 | Class: rviz_plugins/AerialMapDisplay 184 | Draw Behind: false 185 | Enabled: false 186 | Name: AerialMapDisplay 187 | Object URI: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=pk.eyJ1IjoiZ2R1dC1reWxlIiwiYSI6ImNrbWp3eGMxZDBxNDAycGxuc2FtOHB2d2cifQ.OCU3Oa2l613UbI8Re2y-Gw 188 | Topic: /rvizgps 189 | Value: false 190 | Zoom: 17 191 | Enabled: true 192 | Global Options: 193 | Background Color: 255; 255; 255 194 | Default Light: true 195 | Fixed Frame: map 196 | Frame Rate: 30 197 | Name: root 198 | Tools: 199 | - Class: rviz/Interact 200 | Hide Inactive Objects: true 201 | - Class: rviz/FocusCamera 202 | - Class: rviz/Measure 203 | Value: true 204 | Views: 205 | Current: 206 | Class: rviz/Orbit 207 | Distance: 96.46292877197266 208 | Enable Stereo Rendering: 209 | Stereo Eye Separation: 0.05999999865889549 210 | Stereo Focal Distance: 1 211 | Swap Stereo Eyes: false 212 | Value: false 213 | Focal Point: 214 | X: 0 215 | Y: 0 216 | Z: 0 217 | Focal Shape Fixed Size: false 218 | Focal Shape Size: 0.05000000074505806 219 | Invert Z Axis: false 220 | Name: Current View 221 | Near Clip Distance: 0.009999999776482582 222 | Pitch: 0.7853981852531433 223 | Target Frame: odom_fix 224 | Value: Orbit (rviz) 225 | Yaw: 0.7853981852531433 226 | Saved: ~ 227 | Window Geometry: 228 | Displays: 229 | collapsed: false 230 | Height: 1025 231 | Help: 232 | collapsed: false 233 | Hide Left Dock: false 234 | Hide Right Dock: true 235 | QMainWindow State: 000000ff00000000fd00000004000000000000017c000003a7fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000800480065006c0070000000032c000000ba0000006e00fffffffb0000000a0049006d006100670065000000020f000000bb0000000000000000000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000039fc0100000003fb0000001600520061006e0067006500200049006d00610067006500000000000000073f0000000000000000fb0000000a0049006d0061006700650000000000000006100000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073e0000003efc0100000002fb0000000800540069006d006500000000000000073e000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005bb000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 236 | Selection: 237 | collapsed: false 238 | Time: 239 | collapsed: false 240 | Tool Properties: 241 | collapsed: false 242 | Views: 243 | collapsed: true 244 | Width: 1853 245 | X: 67 246 | Y: 27 247 | -------------------------------------------------------------------------------- /launch/exp_launch/GDUTdemo.gps: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 37716 3 | stamp: 4 | secs: 1596888000 5 | nsecs: 15865912 6 | frame_id: /rvizgps 7 | status: 8 | status: 0 9 | service: 1 10 | latitude: 23.0384348 11 | longitude: 113.3912863 12 | altitude: 4.67000007629395 13 | position_covariance: [3.9561210000000004, 0.0, 0.0, 0.0, 3.9561210000000004, 0.0, 0.0, 0.0, 7.650756] 14 | position_covariance_type: 2 15 | -------------------------------------------------------------------------------- /launch/exp_launch/KITTIdemo.gps: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 37716 3 | stamp: 4 | secs: 1596888000 5 | nsecs: 15865912 6 | frame_id: /rvizgps 7 | status: 8 | status: 0 9 | service: 1 10 | latitude: 48.9851176155 11 | longitude: 8.39399876679 12 | altitude: 4.67000007629395 13 | position_covariance: [3.9561210000000004, 0.0, 0.0, 0.0, 3.9561210000000004, 0.0, 0.0, 0.0, 7.650756] 14 | position_covariance_type: 2 15 | -------------------------------------------------------------------------------- /launch/exp_launch/KITTIdemo2.gps: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 37716 3 | stamp: 4 | secs: 1596888000 5 | nsecs: 15865912 6 | frame_id: /rvizgps 7 | status: 8 | status: 0 9 | service: 1 10 | latitude: 48.9721045445 11 | longitude: 8.47614699533 12 | altitude: 4.67000007629395 13 | position_covariance: [3.9561210000000004, 0.0, 0.0, 0.0, 3.9561210000000004, 0.0, 0.0, 0.0, 7.650756] 14 | position_covariance_type: 2 15 | -------------------------------------------------------------------------------- /launch/exp_launch/demo.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 | - /TF1/Tree1 10 | Splitter Ratio: 0.5 11 | Tree Height: 823 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 1 39 | Blocks: 3 40 | Class: rviz_plugins/AerialMapDisplay 41 | Draw Behind: false 42 | Enabled: true 43 | Name: AerialMapDisplay 44 | Object URI: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=pk.eyJ1Ijoib3BlbnN0cmVldG1hcCIsImEiOiJjaml5MjVyb3MwMWV0M3hxYmUzdGdwbzE4In0.q548FjhsSJzvXsGlPsFxAQ 45 | Topic: /gps/fix 46 | Value: true 47 | Zoom: 18 48 | - Class: rviz/TF 49 | Enabled: true 50 | Frame Timeout: 15 51 | Frames: 52 | All Enabled: true 53 | gps: 54 | Value: true 55 | map: 56 | Value: true 57 | Marker Scale: 200 58 | Name: TF 59 | Show Arrows: true 60 | Show Axes: true 61 | Show Names: true 62 | Tree: 63 | map: 64 | gps: 65 | {} 66 | Update Interval: 0 67 | Value: true 68 | - Alpha: 1 69 | Autocompute Intensity Bounds: true 70 | Autocompute Value Bounds: 71 | Max Value: 10 72 | Min Value: -10 73 | Value: true 74 | Axis: Z 75 | Channel Name: intensity 76 | Class: rviz/PointCloud2 77 | Color: 255; 255; 255 78 | Color Transformer: "" 79 | Decay Time: 0 80 | Enabled: true 81 | Invert Rainbow: false 82 | Max Color: 255; 255; 255 83 | Max Intensity: 4096 84 | Min Color: 0; 0; 0 85 | Min Intensity: 0 86 | Name: MapCloud 87 | Position Transformer: "" 88 | Queue Size: 10 89 | Selectable: true 90 | Size (Pixels): 3 91 | Size (m): 0.009999999776482582 92 | Style: Flat Squares 93 | Topic: /map_pub_topic 94 | Unreliable: false 95 | Use Fixed Frame: true 96 | Use rainbow: true 97 | Value: true 98 | - Alpha: 1 99 | Autocompute Intensity Bounds: true 100 | Autocompute Value Bounds: 101 | Max Value: 10 102 | Min Value: -10 103 | Value: true 104 | Axis: Z 105 | Channel Name: intensity 106 | Class: rviz/PointCloud2 107 | Color: 255; 255; 255 108 | Color Transformer: "" 109 | Decay Time: 0 110 | Enabled: true 111 | Invert Rainbow: false 112 | Max Color: 255; 255; 255 113 | Max Intensity: 4096 114 | Min Color: 0; 0; 0 115 | Min Intensity: 0 116 | Name: Velodyne 117 | Position Transformer: "" 118 | Queue Size: 10 119 | Selectable: true 120 | Size (Pixels): 3 121 | Size (m): 0.009999999776482582 122 | Style: Flat Squares 123 | Topic: /segmented_cloud 124 | Unreliable: false 125 | Use Fixed Frame: true 126 | Use rainbow: true 127 | Value: true 128 | Enabled: true 129 | Global Options: 130 | Background Color: 48; 48; 48 131 | Default Light: true 132 | Fixed Frame: map 133 | Frame Rate: 30 134 | Name: root 135 | Tools: 136 | - Class: rviz/Interact 137 | Hide Inactive Objects: true 138 | - Class: rviz/MoveCamera 139 | - Class: rviz/Select 140 | - Class: rviz/FocusCamera 141 | - Class: rviz/Measure 142 | - Class: rviz/SetInitialPose 143 | Theta std deviation: 0.2617993950843811 144 | Topic: /initialpose 145 | X std deviation: 0.5 146 | Y std deviation: 0.5 147 | - Class: rviz/SetGoal 148 | Topic: /move_base_simple/goal 149 | - Class: rviz/PublishPoint 150 | Single click: true 151 | Topic: /clicked_point 152 | Value: true 153 | Views: 154 | Current: 155 | Class: rviz/Orbit 156 | Distance: 775.6434326171875 157 | Enable Stereo Rendering: 158 | Stereo Eye Separation: 0.05999999865889549 159 | Stereo Focal Distance: 1 160 | Swap Stereo Eyes: false 161 | Value: false 162 | Focal Point: 163 | X: 13.735078811645508 164 | Y: -11.794296264648438 165 | Z: -84.21900177001953 166 | Focal Shape Fixed Size: false 167 | Focal Shape Size: 0.05000000074505806 168 | Invert Z Axis: false 169 | Name: Current View 170 | Near Clip Distance: 0.009999999776482582 171 | Pitch: 1.5697963237762451 172 | Target Frame: map 173 | Value: Orbit (rviz) 174 | Yaw: 4.7035627365112305 175 | Saved: ~ 176 | Window Geometry: 177 | Displays: 178 | collapsed: false 179 | Height: 1052 180 | Hide Left Dock: false 181 | Hide Right Dock: false 182 | QMainWindow State: 000000ff00000000fd000000040000000000000191000003c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000004cfc0100000002fb0000000800540069006d006500000000000000073d000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000490000003c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 183 | Selection: 184 | collapsed: false 185 | Time: 186 | collapsed: false 187 | Tool Properties: 188 | collapsed: false 189 | Views: 190 | collapsed: false 191 | Width: 1853 192 | X: 67 193 | Y: 0 194 | -------------------------------------------------------------------------------- /launch/exp_launch/run_port_exp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 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 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /launch/exp_launch/satellite.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 | - /TF1 10 | - /TF1/Frames1 11 | - /TF1/Tree1 12 | - /MapCloud1 13 | - /GPSOdometry1/Shape1 14 | Splitter Ratio: 0.5 15 | Tree Height: 823 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: GpsTrajectory 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Blocks: 3 44 | Class: rviz_plugins/AerialMapDisplay 45 | Draw Behind: false 46 | Enabled: true 47 | Name: AerialMapDisplay 48 | Object URI: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=pk.eyJ1IjoiZ2R1dC1reWxlIiwiYSI6ImNrbWp3eGMxZDBxNDAycGxuc2FtOHB2d2cifQ.OCU3Oa2l613UbI8Re2y-Gw 49 | Topic: /rvizgps 50 | Value: true 51 | Zoom: 17 52 | - Class: rviz/TF 53 | Enabled: true 54 | Frame Timeout: 15 55 | Frames: 56 | All Enabled: false 57 | local_map: 58 | Value: false 59 | map: 60 | Value: false 61 | odom_fix: 62 | Value: false 63 | odom_fusion: 64 | Value: true 65 | rvizgps: 66 | Value: false 67 | Marker Scale: 200 68 | Name: TF 69 | Show Arrows: true 70 | Show Axes: true 71 | Show Names: true 72 | Tree: 73 | map: 74 | local_map: 75 | odom_fix: 76 | {} 77 | odom_fusion: 78 | {} 79 | rvizgps: 80 | {} 81 | Update Interval: 0 82 | Value: true 83 | - Alpha: 0.8999999761581421 84 | Autocompute Intensity Bounds: true 85 | Autocompute Value Bounds: 86 | Max Value: 10 87 | Min Value: -10 88 | Value: true 89 | Axis: Z 90 | Channel Name: intensity 91 | Class: rviz/PointCloud2 92 | Color: 255; 255; 255 93 | Color Transformer: RGB8 94 | Decay Time: 0 95 | Enabled: true 96 | Invert Rainbow: false 97 | Max Color: 255; 255; 255 98 | Max Intensity: 255 99 | Min Color: 0; 0; 0 100 | Min Intensity: 2.071251153945923 101 | Name: MapCloud 102 | Position Transformer: XYZ 103 | Queue Size: 10 104 | Selectable: true 105 | Size (Pixels): 2 106 | Size (m): 0.20000000298023224 107 | Style: Points 108 | Topic: /laser_cloud_surround 109 | Unreliable: false 110 | Use Fixed Frame: true 111 | Use rainbow: true 112 | Value: true 113 | - Alpha: 1 114 | Autocompute Intensity Bounds: true 115 | Autocompute Value Bounds: 116 | Max Value: 10 117 | Min Value: -10 118 | Value: true 119 | Axis: Z 120 | Channel Name: intensity 121 | Class: rviz/PointCloud2 122 | Color: 255; 255; 255 123 | Color Transformer: Intensity 124 | Decay Time: 0 125 | Enabled: true 126 | Invert Rainbow: false 127 | Max Color: 255; 255; 255 128 | Max Intensity: 255 129 | Min Color: 0; 0; 0 130 | Min Intensity: 255 131 | Name: GpsTrajectory 132 | Position Transformer: XYZ 133 | Queue Size: 10 134 | Selectable: true 135 | Size (Pixels): 3 136 | Size (m): 1 137 | Style: Squares 138 | Topic: /gps_history_position 139 | Unreliable: false 140 | Use Fixed Frame: true 141 | Use rainbow: true 142 | Value: true 143 | - Alpha: 1 144 | Autocompute Intensity Bounds: true 145 | Autocompute Value Bounds: 146 | Max Value: 0.005383667070418596 147 | Min Value: 7.548852590844035e-5 148 | Value: true 149 | Axis: Z 150 | Channel Name: intensity 151 | Class: rviz/PointCloud2 152 | Color: 0; 0; 0 153 | Color Transformer: FlatColor 154 | Decay Time: 0 155 | Enabled: true 156 | Invert Rainbow: false 157 | Max Color: 0; 0; 0 158 | Max Intensity: 171 159 | Min Color: 0; 0; 0 160 | Min Intensity: 0 161 | Name: FusionTraj 162 | Position Transformer: XYZ 163 | Queue Size: 10 164 | Selectable: true 165 | Size (Pixels): 3 166 | Size (m): 1 167 | Style: Spheres 168 | Topic: /fusion_position 169 | Unreliable: false 170 | Use Fixed Frame: true 171 | Use rainbow: true 172 | Value: true 173 | - Angle Tolerance: 0.10000000149011612 174 | Class: rviz/Odometry 175 | Covariance: 176 | Orientation: 177 | Alpha: 0.5 178 | Color: 255; 255; 127 179 | Color Style: Unique 180 | Frame: Local 181 | Offset: 1 182 | Scale: 1 183 | Value: true 184 | Position: 185 | Alpha: 0.30000001192092896 186 | Color: 204; 51; 204 187 | Scale: 1 188 | Value: true 189 | Value: true 190 | Enabled: false 191 | Keep: 100 192 | Name: GPSOdometry 193 | Position Tolerance: 0.10000000149011612 194 | Shape: 195 | Alpha: 1 196 | Axes Length: 15 197 | Axes Radius: 2 198 | Color: 255; 25; 0 199 | Head Length: 0.30000001192092896 200 | Head Radius: 0.10000000149011612 201 | Shaft Length: 1 202 | Shaft Radius: 0.05000000074505806 203 | Value: Axes 204 | Topic: /fix_odom 205 | Unreliable: false 206 | Value: false 207 | Enabled: true 208 | Global Options: 209 | Background Color: 48; 48; 48 210 | Default Light: true 211 | Fixed Frame: map 212 | Frame Rate: 30 213 | Name: root 214 | Tools: 215 | - Class: rviz/Interact 216 | Hide Inactive Objects: true 217 | - Class: rviz/MoveCamera 218 | - Class: rviz/Select 219 | - Class: rviz/FocusCamera 220 | - Class: rviz/Measure 221 | - Class: rviz/SetInitialPose 222 | Theta std deviation: 0.2617993950843811 223 | Topic: /initialpose 224 | X std deviation: 0.5 225 | Y std deviation: 0.5 226 | - Class: rviz/SetGoal 227 | Topic: /move_base_simple/goal 228 | - Class: rviz/PublishPoint 229 | Single click: true 230 | Topic: /clicked_point 231 | Value: true 232 | Views: 233 | Current: 234 | Class: rviz/Orbit 235 | Distance: 404.0085144042969 236 | Enable Stereo Rendering: 237 | Stereo Eye Separation: 0.05999999865889549 238 | Stereo Focal Distance: 1 239 | Swap Stereo Eyes: false 240 | Value: false 241 | Focal Point: 242 | X: 40.762325286865234 243 | Y: -300.12054443359375 244 | Z: -0.5442525744438171 245 | Focal Shape Fixed Size: false 246 | Focal Shape Size: 0.05000000074505806 247 | Invert Z Axis: false 248 | Name: Current View 249 | Near Clip Distance: 0.009999999776482582 250 | Pitch: 1.5697963237762451 251 | Target Frame: vehicle 252 | Value: Orbit (rviz) 253 | Yaw: 4.063559055328369 254 | Saved: ~ 255 | Window Geometry: 256 | Displays: 257 | collapsed: false 258 | Height: 1052 259 | Hide Left Dock: false 260 | Hide Right Dock: true 261 | QMainWindow State: 000000ff00000000fd000000040000000000000191000003c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000004cfc0100000002fb0000000800540069006d006500000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005a6000003c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 262 | Selection: 263 | collapsed: false 264 | Time: 265 | collapsed: false 266 | Tool Properties: 267 | collapsed: false 268 | Views: 269 | collapsed: true 270 | Width: 1853 271 | X: 67 272 | Y: 0 273 | -------------------------------------------------------------------------------- /launch/imuOdometry.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 | -------------------------------------------------------------------------------- /launch/localization.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 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /launch/localization_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /AerialMapDisplay1 10 | - /TF1/Frames1 11 | - /TF1/Tree1 12 | - /GPSOdometry1/Shape1 13 | - /VehicleBaseLink1 14 | Splitter Ratio: 0.5432098507881165 15 | Tree Height: 790 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: Velodyne 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Toolbars: 38 | toolButtonStyle: 2 39 | Visualization Manager: 40 | Class: "" 41 | Displays: 42 | - Alpha: 0.5 43 | Blocks: 3 44 | Class: rviz_plugins/AerialMapDisplay 45 | Draw Behind: false 46 | Enabled: false 47 | Name: AerialMapDisplay 48 | Object URI: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=pk.eyJ1IjoiZ2R1dC1reWxlIiwiYSI6ImNrbWp3eGMxZDBxNDAycGxuc2FtOHB2d2cifQ.OCU3Oa2l613UbI8Re2y-Gw 49 | Topic: /rvizgps 50 | Value: false 51 | Zoom: 17 52 | - Class: rviz/TF 53 | Enabled: false 54 | Frame Timeout: 15 55 | Frames: 56 | All Enabled: false 57 | Marker Scale: 200 58 | Name: TF 59 | Show Arrows: true 60 | Show Axes: true 61 | Show Names: true 62 | Tree: 63 | {} 64 | Update Interval: 0 65 | Value: false 66 | - Alpha: 0.5 67 | Autocompute Intensity Bounds: true 68 | Autocompute Value Bounds: 69 | Max Value: 10 70 | Min Value: -10 71 | Value: true 72 | Axis: Z 73 | Channel Name: intensity 74 | Class: rviz/PointCloud2 75 | Color: 255; 255; 255 76 | Color Transformer: FlatColor 77 | Decay Time: 0 78 | Enabled: true 79 | Invert Rainbow: false 80 | Max Color: 255; 255; 255 81 | Max Intensity: 15.179499626159668 82 | Min Color: 0; 0; 0 83 | Min Intensity: -0.02574736624956131 84 | Name: MapCloud 85 | Position Transformer: XYZ 86 | Queue Size: 10 87 | Selectable: true 88 | Size (Pixels): 1 89 | Size (m): 0.20000000298023224 90 | Style: Points 91 | Topic: /map_pub_topic 92 | Unreliable: false 93 | Use Fixed Frame: true 94 | Use rainbow: true 95 | Value: true 96 | - Alpha: 1 97 | Autocompute Intensity Bounds: true 98 | Autocompute Value Bounds: 99 | Max Value: 42.091705322265625 100 | Min Value: -0.9562230110168457 101 | Value: true 102 | Axis: Z 103 | Channel Name: intensity 104 | Class: rviz/PointCloud2 105 | Color: 143; 89; 2 106 | Color Transformer: AxisColor 107 | Decay Time: 0 108 | Enabled: true 109 | Invert Rainbow: false 110 | Max Color: 255; 255; 255 111 | Max Intensity: 15.163800239562988 112 | Min Color: 0; 0; 0 113 | Min Intensity: 0.000699999975040555 114 | Name: Velodyne 115 | Position Transformer: XYZ 116 | Queue Size: 10 117 | Selectable: true 118 | Size (Pixels): 3 119 | Size (m): 1 120 | Style: Points 121 | Topic: /rslidar_points 122 | Unreliable: false 123 | Use Fixed Frame: true 124 | Use rainbow: true 125 | Value: true 126 | - Alpha: 1 127 | Autocompute Intensity Bounds: true 128 | Autocompute Value Bounds: 129 | Max Value: 10 130 | Min Value: -10 131 | Value: true 132 | Axis: Z 133 | Channel Name: intensity 134 | Class: rviz/PointCloud2 135 | Color: 255; 255; 255 136 | Color Transformer: Intensity 137 | Decay Time: 0 138 | Enabled: true 139 | Invert Rainbow: false 140 | Max Color: 255; 255; 255 141 | Max Intensity: 0 142 | Min Color: 0; 0; 0 143 | Min Intensity: 0 144 | Name: GpsTrajectory 145 | Position Transformer: XYZ 146 | Queue Size: 10 147 | Selectable: true 148 | Size (Pixels): 3 149 | Size (m): 1 150 | Style: Squares 151 | Topic: /gps_history_position 152 | Unreliable: false 153 | Use Fixed Frame: true 154 | Use rainbow: true 155 | Value: true 156 | - Alpha: 1 157 | Autocompute Intensity Bounds: true 158 | Autocompute Value Bounds: 159 | Max Value: 0.3591713607311249 160 | Min Value: 0.0012198002077639103 161 | Value: true 162 | Axis: Z 163 | Channel Name: intensity 164 | Class: rviz/PointCloud2 165 | Color: 0; 0; 0 166 | Color Transformer: FlatColor 167 | Decay Time: 0 168 | Enabled: true 169 | Invert Rainbow: false 170 | Max Color: 0; 0; 0 171 | Max Intensity: 171 172 | Min Color: 0; 0; 0 173 | Min Intensity: 0 174 | Name: LidarTrajectory 175 | Position Transformer: XYZ 176 | Queue Size: 10 177 | Selectable: true 178 | Size (Pixels): 3 179 | Size (m): 1 180 | Style: Spheres 181 | Topic: /key_pose_origin 182 | Unreliable: false 183 | Use Fixed Frame: true 184 | Use rainbow: true 185 | Value: true 186 | - Angle Tolerance: 0.10000000149011612 187 | Class: rviz/Odometry 188 | Covariance: 189 | Orientation: 190 | Alpha: 0.5 191 | Color: 255; 255; 127 192 | Color Style: Unique 193 | Frame: Local 194 | Offset: 1 195 | Scale: 1 196 | Value: true 197 | Position: 198 | Alpha: 0.30000001192092896 199 | Color: 204; 51; 204 200 | Scale: 1 201 | Value: true 202 | Value: true 203 | Enabled: false 204 | Keep: 100 205 | Name: GPSOdometry 206 | Position Tolerance: 0.10000000149011612 207 | Shape: 208 | Alpha: 1 209 | Axes Length: 15 210 | Axes Radius: 2 211 | Color: 255; 25; 0 212 | Head Length: 0.30000001192092896 213 | Head Radius: 0.10000000149011612 214 | Shaft Length: 1 215 | Shaft Radius: 0.05000000074505806 216 | Value: Axes 217 | Topic: /fix_odom 218 | Unreliable: false 219 | Value: false 220 | - Alpha: 1 221 | Autocompute Intensity Bounds: true 222 | Autocompute Value Bounds: 223 | Max Value: 10 224 | Min Value: -10 225 | Value: true 226 | Axis: Z 227 | Channel Name: intensity 228 | Class: rviz/PointCloud2 229 | Color: 255; 255; 255 230 | Color Transformer: FlatColor 231 | Decay Time: 0 232 | Enabled: true 233 | Invert Rainbow: false 234 | Max Color: 255; 255; 255 235 | Max Intensity: 15.179499626159668 236 | Min Color: 0; 0; 0 237 | Min Intensity: -0.024211399257183075 238 | Name: RecentPointCloud 239 | Position Transformer: XYZ 240 | Queue Size: 10 241 | Selectable: true 242 | Size (Pixels): 1 243 | Size (m): 0.20000000298023224 244 | Style: Points 245 | Topic: /recent_cloud 246 | Unreliable: false 247 | Use Fixed Frame: true 248 | Use rainbow: true 249 | Value: true 250 | - Class: rviz/Axes 251 | Enabled: true 252 | Length: 6 253 | Name: VehicleBaseLink 254 | Radius: 0.4000000059604645 255 | Reference Frame: vehicle 256 | Value: true 257 | Enabled: true 258 | Global Options: 259 | Background Color: 48; 48; 48 260 | Default Light: true 261 | Fixed Frame: pcl_map 262 | Frame Rate: 30 263 | Name: root 264 | Tools: 265 | - Class: rviz/Interact 266 | Hide Inactive Objects: true 267 | - Class: rviz/MoveCamera 268 | - Class: rviz/Select 269 | - Class: rviz/FocusCamera 270 | - Class: rviz/Measure 271 | - Class: rviz/SetInitialPose 272 | Theta std deviation: 0.2617993950843811 273 | Topic: /initialpose 274 | X std deviation: 0.5 275 | Y std deviation: 0.5 276 | - Class: rviz/SetGoal 277 | Topic: /move_base_simple/goal 278 | - Class: rviz/PublishPoint 279 | Single click: true 280 | Topic: /clicked_point 281 | Value: true 282 | Views: 283 | Current: 284 | Class: rviz/Orbit 285 | Distance: 106.45431518554688 286 | Enable Stereo Rendering: 287 | Stereo Eye Separation: 0.05999999865889549 288 | Stereo Focal Distance: 1 289 | Swap Stereo Eyes: false 290 | Value: false 291 | Focal Point: 292 | X: -0.14766842126846313 293 | Y: 0.028406409546732903 294 | Z: -0.15619538724422455 295 | Focal Shape Fixed Size: false 296 | Focal Shape Size: 0.05000000074505806 297 | Invert Z Axis: false 298 | Name: Current View 299 | Near Clip Distance: 0.009999999776482582 300 | Pitch: 1.5697963237762451 301 | Target Frame: vehicle 302 | Value: Orbit (rviz) 303 | Yaw: 1.5435594320297241 304 | Saved: ~ 305 | Window Geometry: 306 | Displays: 307 | collapsed: false 308 | Height: 1025 309 | Hide Left Dock: false 310 | Hide Right Dock: false 311 | QMainWindow State: 000000ff00000000fd000000040000000000000197000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000110000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000004cfc0100000002fb0000000800540069006d006500000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000048a000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 312 | Selection: 313 | collapsed: false 314 | Time: 315 | collapsed: false 316 | Tool Properties: 317 | collapsed: false 318 | Views: 319 | collapsed: false 320 | Width: 1853 321 | X: 67 322 | Y: 27 323 | -------------------------------------------------------------------------------- /launch/staticTFpub.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /lins.kdev4: -------------------------------------------------------------------------------- 1 | [Project] 2 | Manager=KDevCMakeManager 3 | Name=lins 4 | -------------------------------------------------------------------------------- /log.md: -------------------------------------------------------------------------------- 1 | ### 修改日志 2 | 3 | 2020/10/02 4 | 5 | 添加sensorgps的数据到mapping程序中，无清洗添加至因子图进行融合，经测试通过√ 6 | 7 | 2020/10/02 8 | 9 | 由于我们使用的GPS组合导航模块包含姿态测量（发现咱们这玩意三个旋转轴的旋转方向都与笛卡尔直角坐标系相反，这个得注意），因此我们不适用GTSAM的GPS factor，而是采用pose3 factor。 10 | 回环矫正方面，发现sc-lego-loam的sc检测后添加的回环约束因子是当前帧与建图第一帧的约束关系，这不合理，应该是当前帧与闭环匹配帧，已修改 11 | 因子图的噪声方面感觉设置得不合理，需要后续的实验和调节。 12 | 13 | 2020/10/03 14 | 添加了纯定位模块，其中出现了问题： 15 | 16 | > 问题描述：在做localization时发现地图栅格分配不足，我们直接拿了LOAM的栅格数，那个是对应小尺度地图的，所以我们现在要把`laserCloudNum`修改得足够大。 17 | 18 | 问题已解决，将地图总栅格数修改大一些： 19 | 20 | ```c++ 21 | // 大cube就是附近地图 22 | // 大cube各边上小cube个数的一半 23 | int laserCloudCenWidth = 20; 24 | int laserCloudCenHeight = 20; 25 | int laserCloudCenDepth = 10; 26 | // 多少个小cube组成大cube 27 | const int laserCloudWidth = 41; 28 | const int laserCloudHeight = 41; 29 | const int laserCloudDepth = 21; 30 | // 大cube含有小cube的个数 31 | const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; // 32 | ``` 33 | 34 | 已经测试 35 | 36 | 2020/10/04 37 | 38 | 了解了use_sim_time的作用，我们的rviz指定初始位置也需要一个时钟源，因此如果要调试rviz设置情况的话，play rosbag的时候应该只播放时钟： 39 | 40 | ```shell 41 | rosbag play 2020-09-28-20-46-00.bag --clock --topics 42 | ``` 43 | 44 | 明天设定一下map和camera_init的方向情况，然后尝试使用rviz设定`transformTobeMapped`的值 45 | 46 | 47 | 48 | 2020/10/06 49 | 50 | 尝试修改复杂的`transformAssociateToMap()`函数，使用`Eigen`的`Quaternion`进行简化运算 51 | 52 | 该函数的作用：计算连续两个Lidar odometry数据（`transformBefMapped`是上一次的里程计，`transformSum`是本次的里程计）间的位姿变换，然后将这个里程计估计出的帧间位姿变换附加到`transformTobeMapped`，然后作为优化的初值。 53 | 54 | 修改思路：`transformTobeMapped`,`transformBefMapped`和`transformSum`都转成`Quaternion`或齐次变换矩阵形式，然后进行姿态运算，最后再进行逆变换 55 | 56 | 修改成功，新的函数为`transformAssociateToMap_()`，但是发现一个问题，从四元数转欧拉角时得到的数值跟原函数求得的数值不一致，但是组合起来的旋转作用又是一致的，还需要进一步探索。保守起见还是用回原来的函数。 57 | 58 | 59 | 60 | 2020/10/07 61 | 62 | Localization module已添加上手动（rviz）指定初值，主要是手动修改`transformAftMapped`，注意不是`transformTobeMapped`，具体原因可以溯回到`transformAssociateToMap()`，`transformTobeMapped`是基于`transformAftMapped`的 63 | 64 | 65 | 66 | 2020/11/01 67 | 68 | 在mapping的launch文件中添加了保存地图的选项，这样不用每次都保存地图浪费调试时间 69 | 70 | 71 | 72 | 2020/11/05 73 | 74 | 修正了Localization 和mapping module中激光雷达和IMU的topic名固定为`velodyne_points`的问题，现在改成了每次都从config文件中读取，这样比较灵活 75 | 76 | 重新加入了全局地图发布线程，每10秒发布一次，方便测试 77 | 78 | 在`transform_fusion_node`中加了转WGS84的function，可以发布出来，但是与原始的GPS数据一起放在openstreet中，发现偏差较大，需要查找原因 79 | 80 | 81 | 82 | 2020/11/11 83 | 84 | 修正了transform_fusion_node /map转WGS84时出现误差的问题（由于在launch文件中传递的动态参数与mapLocalizaiton有相同，也就是有冲突，因此将两个节点需要用到的参数改成launch全局参数，具体使用方法参考`花里胡哨笔记`） 85 | 86 | 87 | 88 | 2020/11/18 89 | 90 | 使用新的bag建图后，在进行localization时发现GPS的轨迹与Lidar定位出来的轨迹不一致，但是经过再三确认在建图时明明紧紧跟随着GPS的轨迹，非常迷惑 91 | 92 | 解决方法：原来我们在localization时的GPS起始点使用的是bag的第一个数据，但是我们在建图程序中选取的不是bag的第一个数据，而是与第一个keyframe时间戳匹配的GPS数据，因此我们下次再在新的map中定位时，必须选取第一个keyframe对应的GPS数据，而不是bag的第一个数据。 93 | 94 | 95 | 96 | 2020/11/28 97 | 98 | 根据浩源导航程序的需求，在`transform_fusion_node.cpp`中加入了**发布经过融合后的定位数据**，消息类型为`sleipnir_msgs/gps4control`，经过功能包测试可以使用，还没部署到车上。 99 | 100 | 101 | 102 | 2020/11/29 103 | 104 | 经过试车测试，从建筑物出来时GPS的跳变影响比较大，需要设置一个缓冲队列，然后还可以适当增加激光雷达的置信度，还有个想法是当从建筑物出来时，需要设置一个GPS的信赖域，如果离当前定位数据太远的话就认为GPS还没恢复 105 | 106 | 107 | 108 | 2020/11/30 109 | 110 | 在实车测试时发现当车辆从建筑物出来后搜索到GPS信号的短暂时间内，GPS信号恢复时会发生突变，这时GPS的状态是良好的，但是他的定位数据还没恢复完成，这会将定位数据融合结果“扯“到很远，因此加了一个GPS的缓冲队列用于避免这种情况，而且还额外添加了判断当前位置与GPS定位的偏差程度，如果偏差太远的话也会认为GPS不正常。在融合Lidar因子时，加了一个选项可以选择使用一元因子还是传统的二元因子，一元因子的话噪声固定，可能会更加稳定。 111 | 112 | 2020/12/05 113 | 114 | 这一版调整了GPS切换策略，经过实车测试可行，但是还未经浩源的轨迹跟踪程序测试。 115 | 116 | 在测试过程中发现，车辆经过室内外交界处时会发生定位的轻微跳变，应该是因为此时GPS失效，只有Odometry因子起作用，这需要再融一个传感器信息去约束这种跳变。感觉还需要加上约束因素，因此我打算将IMU预积分项作为约束因子，添加到后端中，也就是二次融合IMU数据，从而起到平滑的作用。为了方便使用GTSAM中的IMU预积分因子，我将`lidar_mapping_node.cpp`和`mapLocalization.cpp`中的GTSAM因子标签改成了`Symbol`形式，位姿存在**X(a)**中，其中要注意的是，**a**需要>=0。 117 | 118 | 119 | 120 | 2020/12/12 121 | 122 | 我在建图程序中加了IMU预积分因子，测试通过，但是感觉改进效果不明显，有待挖掘 123 | 124 | 在Localization里加上IMU预积分因子时还不太行，我觉得原因是因为我们程序中设了初始位姿为0，但是GPS因子一开始将位姿扯到远处，导致因子图中的速度状态飞了，明天再想想怎么改，先下班了。 125 | 126 | 127 | 128 | 2020/12/13 129 | 130 | localization程序中，当车辆停在原地或者运动幅度很小时会一直抖动，问题应该出在了融合方面，当车辆行驶时会生成Keypoint，就会被GPS约束在某个点，但是停在原地或者运动幅度很小时不会运行`saveKeyFramesAndFactor`，这时输入到`transform_fusion_node`的`aftMappedTrans`没有融合GPS而是完全由点云匹配生成，但是呢上一个被GPS约束后的点不是点云匹配最佳的位置，因此会被拉扯到点云匹配最佳处，因此造成了位置反复横跳。 131 | 132 | 已解决：LEGO-LOAM建图是需要回环，为了避免关键帧过多，他通过距离对关键点进行稀疏化，但是在localization中不需要回环，因此将距离判断去掉，每一次后端优化都加入到GTSAM中进行融合，这样就不会反复横跳。但是有个疑问，长时间的积累，因子图会不会内存爆了？需要设置个滑动窗口吗？ 133 | 134 | 答：不需要，知乎回答：[增量式smooth and mapping 是不是伪需求啊？](https://www.zhihu.com/question/279027808) 135 | 136 | 137 | 138 | 2021/03/11 139 | 140 | 在`transform_fusion_node`文件中，将定位结果转换成GPS经纬度信息发布出去。但是经过坐标系统一后，发现规划处获取得到的定位结果与我这显示的定位结果存在偏差，经过测试发现是存在以建图原点为圆心的微量旋转（如0.003 rad），明明数据来源都是来自定位这里，但是为什么会不一致呢？目前还没找到原因（很大可能是来自传感器外参），因此我这里加了一个可调节参数用来调整该参数，如代码： 141 | 142 | ```cpp 143 | // get the GPS origination 144 | // 我们车上使用的星网宇达GPS接收器，他的航向角方向与笛卡尔直角坐标系相反 145 | Eigen::Vector3d InitEulerAngle=Eigen::Vector3d((-OriYaw+90.0)*deg+compensate_init_yaw+unknow_yaw_para, -OriPitch*deg, OriRoll*deg); 146 | cout<<"unknow_yaw_para: "< 2 | 3 | lins 4 | 0.0.0 5 | The lins package 6 | 7 | 8 | charles 9 | 10 | 11 | 12 | 13 | 14 | TODO 15 | 16 | 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 | catkin 50 | cloud_msgs 51 | sleipnir_msgs 52 | cv_bridge 53 | geometry_msgs 54 | image_transport 55 | nav_msgs 56 | pcl_conversions 57 | pcl_ros 58 | roscpp 59 | rospy 60 | sensor_msgs 61 | std_msgs 62 | tf 63 | cloud_msgs 64 | sleipnir_msgs 65 | cv_bridge 66 | geometry_msgs 67 | image_transport 68 | nav_msgs 69 | pcl_conversions 70 | pcl_ros 71 | roscpp 72 | rospy 73 | sensor_msgs 74 | std_msgs 75 | tf 76 | cloud_msgs 77 | sleipnir_msgs 78 | cv_bridge 79 | geometry_msgs 80 | image_transport 81 | nav_msgs 82 | pcl_conversions 83 | pcl_ros 84 | roscpp 85 | rospy 86 | sensor_msgs 87 | std_msgs 88 | tf 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /pic/GPSdetector1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/GPSdetector1.png -------------------------------------------------------------------------------- /pic/GPSdetector2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/GPSdetector2.png -------------------------------------------------------------------------------- /pic/IMU_GPS_LIDAR_Traj.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/IMU_GPS_LIDAR_Traj.png -------------------------------------------------------------------------------- /pic/global_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/global_graph.png -------------------------------------------------------------------------------- /pic/kitti_mapping_evo1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/kitti_mapping_evo1.png -------------------------------------------------------------------------------- /pic/kitti_mapping_evo2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/kitti_mapping_evo2.png -------------------------------------------------------------------------------- /pic/kitti_mapping_with_gps.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/kitti_mapping_with_gps.png -------------------------------------------------------------------------------- /pic/kitti_mapping_without_gps.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/kitti_mapping_without_gps.png -------------------------------------------------------------------------------- /pic/loop compared.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/loop compared.png -------------------------------------------------------------------------------- /pic/vehicle and sensor mounting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/vehicle and sensor mounting.png -------------------------------------------------------------------------------- /pic/融合建图效果2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GDUT-Kyle/lins-gps-iris/292e93fde51451c84b5705625bc54bebd27e1183/pic/融合建图效果2.png -------------------------------------------------------------------------------- /src/LidarIris.cpp: -------------------------------------------------------------------------------- 1 | #include "LidarIris.h" 2 | #include "fftm/fftm.hpp" 3 | 4 | using namespace Eigen; 5 | 6 | cv::Mat1b LidarIris::GetIris(const pcl::PointCloud &cloud) 7 | { 8 | // LiDAR-Iris Image Representation 9 | // 对点云投影进行栅格分割后保存于的矩阵 10 | cv::Mat1b IrisMap = cv::Mat1b::zeros(80, 360); 11 | 12 | // 16-line 13 | for (pcl::PointXYZI p : cloud.points) 14 | { 15 | float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]); 16 | float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 15; 17 | float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180; 18 | int Q_dis = std::min(std::max((int)floor(dis), 0), 79); 19 | int Q_arc = std::min(std::max((int)floor(arc / 4.0f), 0), 7); 20 | int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), 359); 21 | IrisMap.at(Q_dis, Q_yaw) |= (1 << Q_arc); 22 | } 23 | 24 | 25 | // 64-line 26 | // 遍历点云的点 27 | // for (pcl::PointXYZI p : cloud.points) 28 | // { 29 | // // 点离雷达底座的距离 30 | // float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]); 31 | // float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 24; 32 | // float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180; 33 | // int Q_dis = std::min(std::max((int)floor(dis), 0), 79); 34 | // int Q_arc = std::min(std::max((int)floor(arc / 4.0f), 0), 7); 35 | // int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), 359); 36 | // IrisMap.at(Q_dis, Q_yaw) |= (1 << Q_arc); 37 | // } 38 | return IrisMap; 39 | } 40 | 41 | void LidarIris::UpdateFrame(const cv::Mat1b &frame, int frameIndex, float *matchDistance, int *matchIndex) 42 | { 43 | // first: calc feature 44 | std::vector vec; 45 | auto feature = GetFeature(frame, vec); 46 | flann::Matrix queries(vec.data(), 1, vec.size()); 47 | if (featureList.size() == 0) 48 | { 49 | if (matchDistance) 50 | *matchDistance = NAN; 51 | if (matchIndex) 52 | *matchIndex = -1; 53 | vecList.buildIndex(queries); 54 | } 55 | else 56 | { 57 | // second: search in database 58 | vecList.knnSearch(queries, indices, dists, _matchNum, flann::SearchParams(32)); 59 | //thrid: calc matches 60 | std::vector dis(_matchNum); 61 | for (int i = 0; i < _matchNum; i++) 62 | { 63 | dis[i] = Compare(feature, featureList[indices[0][i]]); 64 | } 65 | int minIndex = std::min_element(dis.begin(), dis.end()) - dis.begin(); 66 | if (matchDistance) 67 | *matchDistance = dis[minIndex]; 68 | if (matchIndex) 69 | *matchIndex = frameIndexList[indices[0][minIndex]]; 70 | // forth: add frame to database 71 | vecList.addPoints(queries); 72 | } 73 | featureList.push_back(feature); 74 | frameIndexList.push_back(frameIndex); 75 | } 76 | 77 | float LidarIris::Compare(const LidarIris::FeatureDesc &img1, const LidarIris::FeatureDesc &img2, int *bias) 78 | { 79 | // 快速傅里叶变换模板匹配找到图像间的偏移 80 | auto firstRect = FFTMatch(img2.img, img1.img); 81 | int firstShift = firstRect.center.x - img1.img.cols / 2; 82 | float dis1; 83 | int bias1; 84 | // 根据匹配的偏移量变换后计算汉明距离 85 | GetHammingDistance(img1.T, img1.M, img2.T, img2.M, firstShift, dis1, bias1); 86 | // 87 | auto T2x = circShift(img2.T, 0, 180); 88 | auto M2x = circShift(img2.M, 0, 180); 89 | auto img2x = circShift(img2.img, 0, 180); 90 | // 91 | auto secondRect = FFTMatch(img2x, img1.img); 92 | int secondShift = secondRect.center.x - img1.img.cols / 2; 93 | float dis2 = 0; 94 | int bias2 = 0; 95 | GetHammingDistance(img1.T, img1.M, T2x, M2x, secondShift, dis2, bias2); 96 | // 97 | if (dis1 < dis2) 98 | { 99 | if (bias) 100 | *bias = bias1; 101 | return dis1; 102 | } 103 | else 104 | { 105 | if (bias) 106 | *bias = (bias2 + 180) % 360; 107 | return dis2; 108 | } 109 | } 110 | 111 | // Log-Gabor filter:https://xuewenyuan.github.io/posts/2016/05/blog-post-1/ 112 | 113 | // Parameters: 114 | // nscale: number of wavelet scales(4) 115 | // minWaveLength: wavelength of smallest scale filter(18) 116 | // mult: scaling factor between successive filters.(1.6) 117 | // sigmaOnf: Ratio of the standard deviation of the Gaussian describing(0.75) 118 | // the log Gabor filter's transfer function in the frequency 119 | // domain to the filter center frequency. 120 | std::vector LidarIris::LogGaborFilter(const cv::Mat1f &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf) 121 | { 122 | // iris的行数 123 | int rows = src.rows; 124 | // 列数 125 | int cols = src.cols; 126 | cv::Mat2f filtersum = cv::Mat2f::zeros(1, cols); 127 | // 存储不同尺度下的特征图 128 | std::vector EO(nscale); 129 | // logGabor内核长度偶数化 130 | int ndata = cols; 131 | if (ndata % 2 == 1) 132 | ndata--; 133 | // 滤波器内核 134 | cv::Mat1f logGabor = cv::Mat1f::zeros(1, ndata); 135 | cv::Mat2f result = cv::Mat2f::zeros(rows, ndata); 136 | cv::Mat1f radius = cv::Mat1f::zeros(1, ndata / 2 + 1); 137 | radius.at(0, 0) = 1; 138 | for (int i = 1; i < ndata / 2 + 1; i++) 139 | { 140 | radius.at(0, i) = i / (float)ndata; 141 | } 142 | // 波长 143 | double wavelength = minWaveLength; 144 | // 遍历所有尺度 145 | for (int s = 0; s < nscale; s++) 146 | { 147 | // Centre frequency of filter. 148 | // 滤波器中心频率 149 | double fo = 1.0 / wavelength; 150 | double rfo = fo / 0.5; 151 | // Log Gabor function(频域!!) 好像是预构建一个1D的卷积核 152 | cv::Mat1f temp; //(radius.size()); 153 | cv::log(radius / fo, temp); 154 | cv::pow(temp, 2, temp); 155 | cv::exp((-temp) / (2 * log(sigmaOnf) * log(sigmaOnf)), temp); 156 | temp.copyTo(logGabor.colRange(0, ndata / 2 + 1)); 157 | // Log Gabor function 158 | 159 | // Log Gabor filter 160 | logGabor.at(0, 0) = 0; 161 | cv::Mat2f filter; 162 | cv::Mat1f filterArr[2] = {logGabor, cv::Mat1f::zeros(logGabor.size())}; 163 | cv::merge(filterArr, 2, filter); 164 | filtersum = filtersum + filter; 165 | // 对iris的每一行进行log-gabor滤波器内核卷积? 166 | for (int r = 0; r < rows; r++) 167 | { 168 | cv::Mat2f src2f; 169 | // 以iris的行作为对象进行滤波操作 170 | cv::Mat1f srcArr[2] = {src.row(r).clone(), cv::Mat1f::zeros(1, src.cols)}; 171 | cv::merge(srcArr, 2, src2f); 172 | // 《学习OpenCV（中文版）》-- P200 173 | // 这里需要了解卷积定理公式,实际操作是卷积,但是它先将其转到频域上,再进行频谱相乘,再转回时域 174 | // 离散傅里叶变换 175 | cv::dft(src2f, src2f); 176 | // MulSpectrums 是对于两张频谱图中每个元素的乘法。在频域上进行log-gabor滤波 177 | cv::mulSpectrums(src2f, filter, src2f, 0); 178 | // 离散傅里叶逆变换 179 | cv::idft(src2f, src2f); 180 | // 滤波后将结果合成到result 181 | src2f.copyTo(result.row(r)); 182 | } 183 | // 完成一个尺度下的log-gabor滤波,将result放到EO中 184 | EO[s] = result.clone(); 185 | // 改变滤波器波长,也就是中心频率fo 186 | wavelength *= mult; 187 | } 188 | filtersum = circShift(filtersum, 0, cols / 2); 189 | return EO; 190 | } 191 | 192 | void LidarIris::LoGFeatureEncode(const cv::Mat1b &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf, cv::Mat1b &T, cv::Mat1b &M) 193 | { 194 | cv::Mat1f srcFloat; 195 | src.convertTo(srcFloat, CV_32FC1); 196 | auto list = LogGaborFilter(srcFloat, nscale, minWaveLength, mult, sigmaOnf); 197 | std::vector Tlist(nscale * 2), Mlist(nscale * 2); // bool图像 198 | // list.size()==尺度数? 199 | for (int i = 0; i < list.size(); i++) 200 | { 201 | cv::Mat1f arr[2]; 202 | cv::split(list[i], arr); 203 | // 记录实部和虚部,相当于相位? 204 | Tlist[i] = arr[0] > 0; // 实部 判断是否>0,若是,Tlist[i]=true, else Tlist[i]=false 205 | Tlist[i + nscale] = arr[1] > 0; // 虚部 206 | cv::Mat1f m; 207 | // 计算幅值 208 | cv::magnitude(arr[0], arr[1], m); 209 | Mlist[i] = m < 0.0001; 210 | Mlist[i + nscale] = m < 0.0001; 211 | } 212 | // 按行合并? 213 | cv::vconcat(Tlist, T); 214 | cv::vconcat(Mlist, M); 215 | } 216 | 217 | LidarIris::FeatureDesc LidarIris::GetFeature(const cv::Mat1b &src) 218 | { 219 | // 创建特征描述子 220 | FeatureDesc desc; 221 | // 将iris image存于特征结构体 222 | desc.img = src; 223 | // 傅里叶变换-(转到频域)->log-gabor滤波器-(转回时域)->傅里叶逆变换 224 | LoGFeatureEncode(src, _nscale, _minWaveLength, _mult, _sigmaOnf, desc.T, desc.M); 225 | return desc; 226 | } 227 | 228 | LidarIris::FeatureDesc LidarIris::GetFeature(const cv::Mat1b &src, std::vector &vec) 229 | { 230 | cv::Mat1f temp; 231 | src.convertTo(temp, CV_32FC1); 232 | cv::reduce((temp != 0) / 255, temp, 1, cv::REDUCE_AVG); 233 | vec = temp.isContinuous() ? temp : temp.clone(); 234 | return GetFeature(src); 235 | } 236 | 237 | void LidarIris::GetHammingDistance(const cv::Mat1b &T1, const cv::Mat1b &M1, const cv::Mat1b &T2, const cv::Mat1b &M2, int scale, float &dis, int &bias) 238 | { 239 | dis = NAN; 240 | bias = -1; 241 | for (int shift = scale - 2; shift <= scale + 2; shift++) 242 | { 243 | cv::Mat1b T1s = circShift(T1, 0, shift); 244 | cv::Mat1b M1s = circShift(M1, 0, shift); 245 | cv::Mat1b mask = M1s | M2; 246 | int MaskBitsNum = cv::sum(mask / 255)[0]; 247 | int totalBits = T1s.rows * T1s.cols - MaskBitsNum; 248 | cv::Mat1b C = T1s ^ T2; 249 | C = C & ~mask; 250 | int bitsDiff = cv::sum(C / 255)[0]; 251 | if (totalBits == 0) 252 | { 253 | dis = NAN; 254 | } 255 | else 256 | { 257 | float currentDis = bitsDiff / (float)totalBits; 258 | if (currentDis < dis || std::isnan(dis)) 259 | { 260 | dis = currentDis; 261 | bias = shift; 262 | } 263 | } 264 | } 265 | return; 266 | } 267 | 268 | inline cv::Mat LidarIris::circRowShift(const cv::Mat &src, int shift_m_rows) 269 | { 270 | if (shift_m_rows == 0) 271 | return src.clone(); 272 | shift_m_rows %= src.rows; 273 | int m = shift_m_rows > 0 ? shift_m_rows : src.rows + shift_m_rows; 274 | if(src.rows - m ==0) 275 | return src.clone(); 276 | cv::Mat dst(src.size(), src.type()); 277 | src(cv::Range(src.rows - m, src.rows), cv::Range::all()).copyTo(dst(cv::Range(0, m), cv::Range::all())); 278 | src(cv::Range(0, src.rows - m), cv::Range::all()).copyTo(dst(cv::Range(m, src.rows), cv::Range::all())); 279 | return dst; 280 | } 281 | 282 | inline cv::Mat LidarIris::circColShift(const cv::Mat &src, int shift_n_cols) 283 | { 284 | if (shift_n_cols == 0) 285 | return src.clone(); 286 | shift_n_cols %= src.cols; 287 | int n = shift_n_cols > 0 ? shift_n_cols : src.cols + shift_n_cols; 288 | if(src.cols - n ==0) 289 | return src.clone(); 290 | cv::Mat dst(src.size(), src.type()); 291 | // 1.Range是OpenCV中新加入的一个类，该类有两个关键的变量start和end； 292 | // 2.Range对象可以用来表示矩阵的多个连续的行或者多个连续的列 293 | // 3.Range表示范围从start到end，包含start，但不包含end； 294 | // 4.Range类还提供了一个静态方法all()，这个方法的作用如同Matlab中的“：”,表示所有的行或者所有的列 295 | // // 1.创建一个单位阵 296 | // Mat A= Mat::eye(10, 10, CV_32S); 297 | // // 2.提取第1到3列（不包括3） 298 | // Mat B = A(Range::all(),Range(1,3)); 299 | // // 3.提取B的第5至9行（不包括9） 300 | // C= B(Range(5,9),Range::all()); 301 | src(cv::Range::all(), cv::Range(src.cols - n, src.cols)).copyTo(dst(cv::Range::all(), cv::Range(0, n))); 302 | src(cv::Range::all(), cv::Range(0, src.cols - n)).copyTo(dst(cv::Range::all(), cv::Range(n, src.cols))); 303 | return dst; 304 | } 305 | 306 | cv::Mat LidarIris::circShift(const cv::Mat &src, int shift_m_rows, int shift_n_cols) 307 | { 308 | // 循环行列变换 309 | return circColShift(circRowShift(src, shift_m_rows), shift_n_cols); 310 | } -------------------------------------------------------------------------------- /src/fftm.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/core.hpp" 2 | #include "opencv2/opencv.hpp" 3 | 4 | using namespace std; 5 | using namespace cv; 6 | 7 | //---------------------------------------------------------- 8 | // Recombinate image quaters 9 | //---------------------------------------------------------- 10 | void Recomb(Mat &src, Mat &dst) 11 | { 12 | int cx = src.cols >> 1; 13 | int cy = src.rows >> 1; 14 | Mat tmp; 15 | tmp.create(src.size(), src.type()); 16 | src(Rect(0, 0, cx, cy)).copyTo(tmp(Rect(cx, cy, cx, cy))); 17 | src(Rect(cx, cy, cx, cy)).copyTo(tmp(Rect(0, 0, cx, cy))); 18 | src(Rect(cx, 0, cx, cy)).copyTo(tmp(Rect(0, cy, cx, cy))); 19 | src(Rect(0, cy, cx, cy)).copyTo(tmp(Rect(cx, 0, cx, cy))); 20 | dst = tmp; 21 | } 22 | //---------------------------------------------------------- 23 | // 2D Forward FFT 24 | //---------------------------------------------------------- 25 | void ForwardFFT(Mat &Src, Mat *FImg, bool do_recomb = true) 26 | { 27 | int M = getOptimalDFTSize(Src.rows); 28 | int N = getOptimalDFTSize(Src.cols); 29 | Mat padded; 30 | copyMakeBorder(Src, padded, 0, M - Src.rows, 0, N - Src.cols, BORDER_CONSTANT, Scalar::all(0)); 31 | Mat planes[] = { Mat_(padded), Mat::zeros(padded.size(), CV_32F) }; 32 | Mat complexImg; 33 | merge(planes, 2, complexImg); 34 | dft(complexImg, complexImg); 35 | split(complexImg, planes); 36 | planes[0] = planes[0](Rect(0, 0, planes[0].cols & -2, planes[0].rows & -2)); 37 | planes[1] = planes[1](Rect(0, 0, planes[1].cols & -2, planes[1].rows & -2)); 38 | if (do_recomb) 39 | { 40 | Recomb(planes[0], planes[0]); 41 | Recomb(planes[1], planes[1]); 42 | } 43 | planes[0] /= float(M*N); 44 | planes[1] /= float(M*N); 45 | FImg[0] = planes[0].clone(); 46 | FImg[1] = planes[1].clone(); 47 | } 48 | //---------------------------------------------------------- 49 | // 2D inverse FFT 50 | //---------------------------------------------------------- 51 | void InverseFFT(Mat *FImg, Mat &Dst, bool do_recomb = true) 52 | { 53 | if (do_recomb) 54 | { 55 | Recomb(FImg[0], FImg[0]); 56 | Recomb(FImg[1], FImg[1]); 57 | } 58 | Mat complexImg; 59 | merge(FImg, 2, complexImg); 60 | idft(complexImg, complexImg); 61 | split(complexImg, FImg); 62 | Dst = FImg[0].clone(); 63 | } 64 | //----------------------------------------------------------------------------------------------------- 65 | // 66 | //----------------------------------------------------------------------------------------------------- 67 | void highpass(Size sz, Mat& dst) 68 | { 69 | Mat a = Mat(sz.height, 1, CV_32FC1); 70 | Mat b = Mat(1, sz.width, CV_32FC1); 71 | 72 | float step_y = CV_PI / sz.height; 73 | float val = -CV_PI*0.5; 74 | 75 | for (int i = 0; i < sz.height; ++i) 76 | { 77 | a.at(i) = cos(val); 78 | val += step_y; 79 | } 80 | 81 | val = -CV_PI*0.5; 82 | float step_x = CV_PI / sz.width; 83 | for (int i = 0; i < sz.width; ++i) 84 | { 85 | b.at(i) = cos(val); 86 | val += step_x; 87 | } 88 | 89 | Mat tmp = a*b; 90 | dst = (1.0 - tmp).mul(2.0 - tmp); 91 | } 92 | //----------------------------------------------------------------------------------------------------- 93 | // 94 | //----------------------------------------------------------------------------------------------------- 95 | float logpolar(Mat& src, Mat& dst) 96 | { 97 | float radii = src.cols; 98 | float angles = src.rows; 99 | Point2f center(src.cols / 2, src.rows / 2); 100 | float d = norm(Vec2f(src.cols - center.x, src.rows - center.y)); 101 | float log_base = pow(10.0, log10(d) / radii); 102 | float d_theta = CV_PI / (float)angles; 103 | float theta = CV_PI / 2.0; 104 | float radius = 0; 105 | Mat map_x(src.size(), CV_32FC1); 106 | Mat map_y(src.size(), CV_32FC1); 107 | for (int i = 0; i < angles; ++i) 108 | { 109 | for (int j = 0; j < radii; ++j) 110 | { 111 | radius = pow(log_base, float(j)); 112 | float x = radius * sin(theta) + center.x; 113 | float y = radius * cos(theta) + center.y; 114 | map_x.at(i, j) = x; 115 | map_y.at(i, j) = y; 116 | } 117 | theta += d_theta; 118 | } 119 | remap(src, dst, map_x, map_y, cv::INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 120 | return log_base; 121 | } 122 | //----------------------------------------------------------------------------------------------------- 123 | // As input we need equal sized images, with the same aspect ratio, 124 | // scale difference should not exceed 1.8 times. 125 | //----------------------------------------------------------------------------------------------------- 126 | RotatedRect LogPolarFFTTemplateMatch(Mat& im0, Mat& im1/*, double canny_threshold1, double canny_threshold2*/) 127 | { 128 | // Accept 1 or 3 channel CV_8U, CV_32F or CV_64F images. 129 | CV_Assert((im0.type() == CV_8UC1) || (im0.type() == CV_8UC3) || 130 | (im0.type() == CV_32FC1) || (im0.type() == CV_32FC3) || 131 | (im0.type() == CV_64FC1) || (im0.type() == CV_64FC3)); 132 | 133 | CV_Assert(im0.rows == im1.rows && im0.cols == im1.cols); 134 | 135 | CV_Assert(im0.channels() == 1 || im0.channels() == 3 || im0.channels() == 4); 136 | 137 | CV_Assert(im1.channels() == 1 || im1.channels() == 3 || im1.channels() == 4); 138 | 139 | //Mat im0_tmp = im0.clone(); 140 | //Mat im1_tmp = im1.clone(); 141 | if (im0.channels() == 3) 142 | { 143 | cvtColor(im0, im0, cv::COLOR_BGR2GRAY); 144 | } 145 | 146 | if (im0.channels() == 4) 147 | { 148 | cvtColor(im0, im0, cv::COLOR_BGRA2GRAY); 149 | } 150 | 151 | if (im1.channels() == 3) 152 | { 153 | cvtColor(im1, im1, cv::COLOR_BGR2GRAY); 154 | } 155 | 156 | if (im1.channels() == 4) 157 | { 158 | cvtColor(im1, im1, cv::COLOR_BGRA2GRAY); 159 | } 160 | 161 | if (im0.type() == CV_32FC1) 162 | { 163 | im0.convertTo(im0, CV_8UC1, 255.0); 164 | } 165 | 166 | if (im1.type() == CV_32FC1) 167 | { 168 | im1.convertTo(im1, CV_8UC1, 255.0); 169 | } 170 | 171 | if (im0.type() == CV_64FC1) 172 | { 173 | im0.convertTo(im0, CV_8UC1, 255.0); 174 | } 175 | 176 | if (im1.type() == CV_64FC1) 177 | { 178 | im1.convertTo(im1, CV_8UC1, 255.0); 179 | } 180 | 181 | 182 | // Canny(im0, im0, canny_threshold1, canny_threshold2); // you can change this 183 | // Canny(im1, im1, canny_threshold1, canny_threshold2); 184 | 185 | // Ensure both images are of CV_32FC1 type 186 | im0.convertTo(im0, CV_32FC1, 1.0 / 255.0); 187 | im1.convertTo(im1, CV_32FC1, 1.0 / 255.0); 188 | 189 | Mat F0[2], F1[2]; 190 | Mat f0, f1; 191 | // 前向快速傅里叶变换 192 | ForwardFFT(im0, F0); 193 | ForwardFFT(im1, F1); 194 | // 该函数用来计算虚数的幅值 195 | magnitude(F0[0], F0[1], f0); 196 | magnitude(F1[0], F1[1], f1); 197 | 198 | // Create filter 199 | // 高通滤波器? 200 | Mat h; 201 | highpass(f0.size(), h); 202 | 203 | // Apply it in freq domain 204 | // 在频域上进行高通滤波 205 | f0 = f0.mul(h); 206 | f1 = f1.mul(h); 207 | 208 | float log_base; 209 | Mat f0lp, f1lp; 210 | 211 | // 将图像映射到极坐标 212 | // 参考:https://blog.csdn.net/BeBuBu/article/details/102517138 213 | // 图像的尺寸的差异被旋换为对数极坐标平面内沿着p轴的位移，旋转差异被转换成对数极坐标平面沿θ轴的位移。 214 | log_base = logpolar(f0, f0lp); 215 | log_base = logpolar(f1, f1lp); 216 | 217 | // Find rotation and scale 218 | // 相位相关法（phase correclate）:用于检测两幅内容相同的图像之间的相对位移量(注意:这里的图像是LogPolar形式, 219 | // 因此是寻找旋转和尺度的关系) 220 | Point2d rotation_and_scale = cv::phaseCorrelate(f1lp, f0lp); 221 | 222 | float angle = 180.0 * rotation_and_scale.y / f0lp.rows; 223 | float scale = pow(log_base, rotation_and_scale.x); 224 | // -------------- 225 | if (scale > 1.8) 226 | { 227 | rotation_and_scale = cv::phaseCorrelate(f1lp, f0lp); 228 | angle = -180.0 * rotation_and_scale.y / f0lp.rows; 229 | scale = 1.0 / pow(log_base, rotation_and_scale.x); 230 | if (scale > 1.8) 231 | { 232 | cout << "Images are not compatible. Scale change > 1.8" << endl; 233 | return RotatedRect(); 234 | } 235 | } 236 | // -------------- 237 | if (angle < -90.0) 238 | { 239 | angle += 180.0; 240 | } 241 | else if (angle > 90.0) 242 | { 243 | angle -= 180.0; 244 | } 245 | 246 | // Now rotate and scale fragment back, then find translation 247 | // 现在旋转并缩放片段，然后找到偏移 248 | // Mat getRotationMatrix2D(Point2f center, double angle, double scale) 生成旋转矩阵 249 | // Point2f center：表示旋转的中心点 250 | // double angle：表示旋转的角度 251 | // double scale：图像缩放因子 252 | Mat rot_mat = getRotationMatrix2D(Point(im1.cols / 2, im1.rows / 2), angle, 1.0 / scale); 253 | 254 | // rotate and scale 255 | Mat im1_rs; 256 | // 对图像进行仿射变换,使得im1_rs和im0的尺度和旋转相同 257 | warpAffine(im1, im1_rs, rot_mat, im1.size()); 258 | 259 | // find translation 260 | // 相位相关法（phase correlate）可以用于检测两幅内容相同的图像之间的相对位移量。 261 | // 它是基于傅立叶变换的位移定理：一个平移过的函数的傅立叶变换仅仅是未平移函数的傅立 262 | // 叶变换与一个具有线性相位的指数因子的乘积，即空间域中的平移会造成频域中频谱的相移。 263 | // 计算两图像间的偏移(注意:这里是原图像,不是LogPolar) 264 | Point2d tr = cv::phaseCorrelate(im1_rs, im0); 265 | 266 | // compute rotated rectangle parameters 267 | RotatedRect rr; 268 | rr.center = tr + Point2d(im0.cols / 2, im0.rows / 2); 269 | rr.angle = -angle; 270 | rr.size.width = im1.cols / scale; 271 | rr.size.height = im1.rows / scale; 272 | 273 | //im0 = im0_tmp.clone(); 274 | //im1 = im1_tmp.clone(); 275 | 276 | return rr; 277 | } 278 | 279 | RotatedRect FFTMatch(const Mat& im0, const Mat& im1) 280 | { 281 | Mat im0_tmp = im0.clone(); 282 | Mat im1_tmp = im1.clone(); 283 | // 基于FFT的模板匹配 284 | return LogPolarFFTTemplateMatch(im0_tmp, im1_tmp); 285 | } 286 | -------------------------------------------------------------------------------- /src/lib/Scancontext.cpp: -------------------------------------------------------------------------------- 1 | #include "sc_lego_loam/Scancontext.h" 2 | 3 | // namespace SC2 4 | // { 5 | 6 | void coreImportTest (void) 7 | { 8 | cout << "scancontext lib is successfully imported." << endl; 9 | } // coreImportTest 10 | 11 | 12 | float rad2deg(float radians) 13 | { 14 | return radians * 180.0 / M_PI; 15 | } 16 | 17 | float deg2rad(float degrees) 18 | { 19 | return degrees * M_PI / 180.0; 20 | } 21 | 22 | 23 | float xy2theta( const float & _x, const float & _y ) 24 | { 25 | if ( _x >= 0 & _y >= 0) 26 | return (180/M_PI) * atan(_y / _x); 27 | 28 | if ( _x < 0 & _y >= 0) 29 | return 180 - ( (180/M_PI) * atan(_y / (-_x)) ); 30 | 31 | if ( _x < 0 & _y < 0) 32 | return 180 + ( (180/M_PI) * atan(_y / _x) ); 33 | 34 | if ( _x >= 0 & _y < 0) 35 | return 360 - ( (180/M_PI) * atan((-_y) / _x) ); 36 | } // xy2theta 37 | 38 | 39 | MatrixXd circshift( MatrixXd &_mat, int _num_shift ) 40 | { 41 | // shift columns to right direction 42 | assert(_num_shift >= 0); 43 | 44 | if( _num_shift == 0 ) 45 | { 46 | MatrixXd shifted_mat( _mat ); 47 | return shifted_mat; // Early return 48 | } 49 | 50 | // ring key向量右移_num_shift列 51 | MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() ); 52 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ ) 53 | { 54 | int new_location = (col_idx + _num_shift) % _mat.cols(); 55 | shifted_mat.col(new_location) = _mat.col(col_idx); 56 | } 57 | 58 | return shifted_mat; 59 | 60 | } // circshift 61 | 62 | 63 | std::vector eig2stdvec( MatrixXd _eigmat ) 64 | { 65 | std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() ); 66 | return vec; 67 | } // eig2stdvec 68 | 69 | 70 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ) 71 | { 72 | int num_eff_cols = 0; // i.e., to exclude all-nonzero sector 73 | double sum_sector_similarity = 0; 74 | for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ ) 75 | { 76 | VectorXd col_sc1 = _sc1.col(col_idx); 77 | VectorXd col_sc2 = _sc2.col(col_idx); 78 | 79 | if( col_sc1.norm() == 0 | col_sc2.norm() == 0 ) 80 | continue; // don't count this sector pair. 81 | 82 | double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm()); 83 | 84 | sum_sector_similarity = sum_sector_similarity + sector_similarity; 85 | num_eff_cols = num_eff_cols + 1; 86 | } 87 | 88 | double sc_sim = sum_sector_similarity / num_eff_cols; 89 | return 1.0 - sc_sim; 90 | 91 | } // distDirectSC 92 | 93 | // 论文公式(6)(7) 94 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2) 95 | { 96 | int argmin_vkey_shift = 0; 97 | double min_veky_diff_norm = 10000000; 98 | // calculate distances with all possible column-shifted scan contexts and find the minimum distance 99 | // 将ring key 逐次右移位再与当前scan context进行比较计算距离 100 | for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ ) 101 | { 102 | // ring key循环右移 103 | MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx); 104 | 105 | // 计算范数 106 | MatrixXd vkey_diff = _vkey1 - vkey2_shifted; 107 | 108 | double cur_diff_norm = vkey_diff.norm(); 109 | // 取最小的距离值 110 | if( cur_diff_norm < min_veky_diff_norm ) 111 | { 112 | argmin_vkey_shift = shift_idx; 113 | min_veky_diff_norm = cur_diff_norm; 114 | } 115 | } 116 | 117 | return argmin_vkey_shift; 118 | 119 | } // fastAlignUsingVkey 120 | 121 | 122 | std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 ) 123 | { 124 | // 1. fast align using variant key (not in original IROS18) 125 | // 计算两帧待匹配点云的sector key 126 | MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 ); 127 | MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 ); 128 | // 计算两scan context距离 --- 论文公式(6)(7) 129 | int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 ); 130 | 131 | // 对于小数而言，round()函数仅仅保留到整数位，即仅仅对小数点后一位四舍五入， 132 | const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 133 | std::vector shift_idx_search_space { argmin_vkey_shift }; 134 | for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ ) 135 | { 136 | // 在括号内+ _sc1.cols()是为了防止出现负数取模的情况, 然后获取最佳候选闭环帧附近几帧 137 | shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() ); 138 | shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() ); 139 | } 140 | // 将待搜索区域的索引按顺序排列 141 | std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end()); 142 | 143 | // 2. fast columnwise diff 144 | int argmin_shift = 0; 145 | double min_sc_dist = 10000000; 146 | // 遍历最佳候选闭环帧附近几帧 147 | for ( int num_shift: shift_idx_search_space ) 148 | { 149 | // 求scan context的距离,获取最近的那帧的索引和得分值 150 | MatrixXd sc2_shifted = circshift(_sc2, num_shift); 151 | double cur_sc_dist = distDirectSC( _sc1, sc2_shifted ); 152 | if( cur_sc_dist < min_sc_dist ) 153 | { 154 | argmin_shift = num_shift; 155 | min_sc_dist = cur_sc_dist; 156 | } 157 | } 158 | 159 | return make_pair(min_sc_dist, argmin_shift); 160 | 161 | } // distanceBtnScanContext 162 | 163 | 164 | MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down ) 165 | { 166 | TicToc_ t_making_desc; 167 | 168 | // 点云数量 169 | int num_pts_scan_down = _scan_down.points.size(); 170 | 171 | // main 172 | const int NO_POINT = -1000; 173 | // scan context 矩阵, 初值设为NO_POINT = -1000, 与论文不同 174 | MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR); 175 | 176 | SCPointType pt; 177 | float azim_angle, azim_range; // wihtin 2d plane 178 | int ring_idx, sctor_idx; 179 | // 遍历每个点 180 | for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++) 181 | { 182 | pt.x = _scan_down.points[pt_idx].x; 183 | pt.y = _scan_down.points[pt_idx].y; 184 | pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0). 185 | 186 | // xyz to ring, sector 187 | // 计算点投影到xy平面的极坐标 188 | azim_range = sqrt(pt.x * pt.x + pt.y * pt.y); 189 | azim_angle = xy2theta(pt.x, pt.y); 190 | 191 | // if range is out of roi, pass 192 | if( azim_range > PC_MAX_RADIUS ) 193 | continue; 194 | 195 | // 编码 --- 论文fig.1 196 | ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 ); 197 | sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 ); 198 | 199 | // taking maximum z 200 | // 取该block中拥有最大的z值的点 --- 论文公式(3) 201 | if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0 202 | desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin 203 | } 204 | 205 | // reset no points to zero (for cosine dist later) 206 | // 遍历scan context矩阵中的每个block, 空白block置零 207 | for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ ) 208 | for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ ) 209 | if( desc(row_idx, col_idx) == NO_POINT ) 210 | desc(row_idx, col_idx) = 0; 211 | 212 | t_making_desc.toc("PolarContext making"); 213 | 214 | return desc; 215 | } // SCManager::makeScancontext 216 | 217 | 218 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc ) 219 | { 220 | /* 221 | * summary: rowwise mean vector 222 | */ 223 | // 定义一个向量k用于存储ring key --- 论文公式(8) 224 | Eigen::MatrixXd invariant_key(_desc.rows(), 1); 225 | for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ ) 226 | { 227 | // 论文公式(9) 228 | Eigen::MatrixXd curr_row = _desc.row(row_idx); 229 | invariant_key(row_idx, 0) = curr_row.mean(); 230 | } 231 | 232 | return invariant_key; 233 | } // SCManager::makeRingkeyFromScancontext 234 | 235 | 236 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc ) 237 | { 238 | /* 239 | * summary: columnwise mean vector 240 | */ 241 | Eigen::MatrixXd variant_key(1, _desc.cols()); 242 | for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ ) 243 | { 244 | Eigen::MatrixXd curr_col = _desc.col(col_idx); 245 | variant_key(0, col_idx) = curr_col.mean(); 246 | } 247 | 248 | return variant_key; 249 | } // SCManager::makeSectorkeyFromScancontext 250 | 251 | 252 | void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down ) 253 | { 254 | // 点云帧转scan context, 返回编码好的矩阵 255 | Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 256 | // 提取ring key 257 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc ); 258 | // 类似提取ring key 259 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc ); 260 | // eigen::vector -> std::vector 261 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey ); 262 | 263 | polarcontexts_.push_back( sc ); 264 | polarcontext_invkeys_.push_back( ringkey ); 265 | polarcontext_vkeys_.push_back( sectorkey ); 266 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec ); 267 | 268 | cv::Mat1b li1 = LidarIris_.GetIris(_scan_down); 269 | 270 | LidarIris::FeatureDesc fd1 = LidarIris_.GetFeature(li1); 271 | 272 | IrisFeatures_.push_back(fd1); 273 | 274 | // cout < SCManager::detectLoopClosureID ( void ) 280 | { 281 | // cout<<"loop2"< result {loop_id, 0.0}; 298 | return result; // Early return 299 | } 300 | 301 | // tree_ reconstruction (not mandatory to make everytime) 302 | if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost 303 | { 304 | TicToc_ t_tree_construction; 305 | 306 | // 构建kd树 307 | polarcontext_invkeys_to_search_.clear(); 308 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ; 309 | 310 | polarcontext_tree_.reset(); 311 | polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ ); 312 | // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor) 313 | t_tree_construction.toc("Tree construction"); 314 | } 315 | tree_making_period_conter = tree_making_period_conter + 1; 316 | 317 | double min_dist = 10000000; // init with somthing large 318 | int nn_align = 0; 319 | int nn_idx = 0; 320 | 321 | // knn search 322 | // 存储候选闭环帧 323 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 324 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE ); 325 | 326 | TicToc_ t_tree_search; 327 | // 存储kd搜索结果 328 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE ); 329 | // 初始化 330 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] ); 331 | // 进行kd搜索,结果存于knnsearch_result(10帧闭环候选帧) 332 | polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 333 | t_tree_search.toc("Tree search"); 334 | 335 | /* 336 | * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance) 337 | */ 338 | TicToc_ t_calc_dist; 339 | // 遍历10个闭环候选帧 340 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ ) 341 | { 342 | // 当前闭环候选帧的scan context 343 | // MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ]; 344 | // 获取匹配到的最佳闭环帧其得分值(距离和方位角偏差) -- scancontext 345 | // std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 346 | 347 | auto iris_feature_candidate = IrisFeatures_[ candidate_indexes[candidate_iter_idx] ]; 348 | 349 | int bias; 350 | auto dis = LidarIris_.Compare(curr_iris_feature, iris_feature_candidate, &bias); 351 | 352 | // cout << "dis = " << dis << ", bias = " << bias << endl; 353 | 354 | // 匹配得分值 355 | // double candidate_dist = sc_dist_result.first; 356 | // 匹配闭环帧的索引 357 | // int candidate_align = sc_dist_result.second; 358 | 359 | // 再次在10个闭环候选帧中筛选 360 | if( dis < min_dist ) 361 | { 362 | min_dist = dis; 363 | nn_align = bias; 364 | 365 | nn_idx = candidate_indexes[candidate_iter_idx]; 366 | } 367 | } 368 | t_calc_dist.toc("Distance calc"); 369 | 370 | /* 371 | * loop threshold check 372 | */ 373 | if( min_dist < SC_DIST_THRES ) 374 | { 375 | loop_id = nn_idx; 376 | 377 | // std::cout.precision(3); 378 | // cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; 379 | // cout << "[Loop found] yaw diff: " << nn_align << " deg." << endl; 380 | } 381 | else 382 | { 383 | std::cout.precision(3); 384 | // cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl; 385 | // cout << "[Not loop] yaw diff: " << nn_align << " deg." << endl; 386 | } 387 | 388 | // To do: return also nn_align (i.e., yaw diff) 389 | float yaw_diff_rad = deg2rad(nn_align); 390 | // 返回闭环帧索引和方位角偏差 391 | std::pair result {loop_id, yaw_diff_rad}; 392 | 393 | return result; 394 | 395 | } // SCManager::detectLoopClosureID 396 | 397 | // } // namespace SC2 -------------------------------------------------------------------------------- /src/lib/XmlDomDocument.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | XmlDomDocument.cpp 3 | 4 | DOM parsing class definitions. 5 | 6 | ------------------------------------------ 7 | 8 | Copyright (c) 2013 Vic Hargrave 9 | 10 | Licensed under the Apache License, Version 2.0 (the "License"); 11 | you may not use this file except in compliance with the License. 12 | You may obtain a copy of the License at 13 | 14 | http://www.apache.org/licenses/LICENSE-2.0 15 | 16 | Unless required by applicable law or agreed to in writing, software 17 | distributed under the License is distributed on an "AS IS" BASIS, 18 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 19 | See the License for the specific language governing permissions and 20 | limitations under the License. 21 | */ 22 | 23 | #include 24 | #include "xmldom/XmlDomDocument.h" 25 | 26 | class XmlDomErrorHandler : public HandlerBase 27 | { 28 | public: 29 | void fatalError(const SAXParseException &exc) { 30 | printf("Fatal parsing error at line %d\n", (int)exc.getLineNumber()); 31 | exit(-1); 32 | } 33 | }; 34 | 35 | XercesDOMParser* parser = NULL; 36 | ErrorHandler* errorHandler = NULL; 37 | 38 | void createParser() 39 | { 40 | if (!parser) 41 | { 42 | XMLPlatformUtils::Initialize(); 43 | parser = new XercesDOMParser(); 44 | errorHandler = (ErrorHandler*) new XmlDomErrorHandler(); 45 | parser->setErrorHandler(errorHandler); 46 | } 47 | } 48 | 49 | XmlDomDocument::XmlDomDocument(const char* xmlfile) : m_doc(NULL) 50 | { 51 | createParser(); 52 | parser->parse(xmlfile); 53 | m_doc = parser->adoptDocument(); 54 | } 55 | 56 | XmlDomDocument::~XmlDomDocument() 57 | { 58 | if (m_doc) m_doc->release(); 59 | } 60 | 61 | string XmlDomDocument::getChildValue(const char* parentTag, int parentIndex, const char* childTag, int childIndex) 62 | { 63 | XMLCh* temp = XMLString::transcode(parentTag); 64 | DOMNodeList* list = m_doc->getElementsByTagName(temp); 65 | XMLString::release(&temp); 66 | 67 | DOMElement* parent = dynamic_cast(list->item(parentIndex)); 68 | DOMElement* child = 69 | dynamic_cast(parent->getElementsByTagName(XMLString::transcode(childTag))->item(childIndex)); 70 | string value; 71 | if (child) { 72 | char* temp2 = XMLString::transcode(child->getTextContent()); 73 | value = temp2; 74 | XMLString::release(&temp2); 75 | } 76 | else { 77 | value = ""; 78 | } 79 | return value; 80 | } 81 | 82 | string XmlDomDocument::getChildAttribute(const char* parentTag, int parentIndex, const char* childTag, int childIndex, 83 | const char* attributeTag) 84 | { 85 | XMLCh* temp = XMLString::transcode(parentTag); 86 | DOMNodeList* list = m_doc->getElementsByTagName(temp); 87 | XMLString::release(&temp); 88 | 89 | DOMElement* parent = dynamic_cast(list->item(parentIndex)); 90 | DOMElement* child = 91 | dynamic_cast(parent->getElementsByTagName(XMLString::transcode(childTag))->item(childIndex)); 92 | string value; 93 | if (child) { 94 | temp = XMLString::transcode(attributeTag); 95 | char* temp2 = XMLString::transcode(child->getAttribute(temp)); 96 | value = temp2; 97 | XMLString::release(&temp2); 98 | } 99 | else { 100 | value = ""; 101 | } 102 | return value; 103 | } 104 | 105 | int XmlDomDocument::getRootElementCount(const char* rootElementTag) 106 | { 107 | DOMNodeList* list = m_doc->getElementsByTagName(XMLString::transcode(rootElementTag)); 108 | return (int)list->getLength(); 109 | } 110 | 111 | int XmlDomDocument::getChildCount(const char* parentTag, int parentIndex, const char* childTag) 112 | { 113 | XMLCh* temp = XMLString::transcode(parentTag); 114 | DOMNodeList* list = m_doc->getElementsByTagName(temp); 115 | XMLString::release(&temp); 116 | 117 | DOMElement* parent = dynamic_cast(list->item(parentIndex)); 118 | DOMNodeList* childList = parent->getElementsByTagName(XMLString::transcode(childTag)); 119 | return (int)childList->getLength(); 120 | } 121 | -------------------------------------------------------------------------------- /src/lib/parameters.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of LINS. 2 | // 3 | // Copyright (C) 2020 Chao Qin , 4 | // Robotics and Multiperception Lab (RAM-LAB ), 5 | // The Hong Kong University of Science and Technology 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | 18 | #include 19 | 20 | namespace parameter { 21 | 22 | // !@ENABLE_CALIBRATION 23 | int CALIBARTE_IMU; 24 | int SHOW_CONFIGURATION; 25 | int AVERAGE_NUMS; 26 | 27 | // !@INITIAL_PARAMETERS 28 | double IMU_LIDAR_EXTRINSIC_ANGLE; 29 | double IMU_MISALIGN_ANGLE; 30 | 31 | // !@LIDAR_PARAMETERS 32 | int LINE_NUM; 33 | int SCAN_NUM; 34 | double SCAN_PERIOD; 35 | double EDGE_THRESHOLD; 36 | double SURF_THRESHOLD; 37 | double NEAREST_FEATURE_SEARCH_SQ_DIST; 38 | 39 | // !@TESTING 40 | int VERBOSE; 41 | int ICP_FREQ; 42 | int MAX_LIDAR_NUMS; 43 | int NUM_ITER; 44 | double LIDAR_SCALE; 45 | double LIDAR_STD; 46 | 47 | // !@SUB_TOPIC_NAME 48 | std::string IMU_TOPIC; 49 | std::string LIDAR_TOPIC; 50 | 51 | // !@PUB_TOPIC_NAME 52 | std::string LIDAR_ODOMETRY_TOPIC; 53 | std::string LIDAR_MAPPING_TOPIC; 54 | 55 | // !@KALMAN_FILTER 56 | double ACC_N; 57 | double ACC_W; 58 | double GYR_N; 59 | double GYR_W; 60 | V3D INIT_POS_STD; 61 | V3D INIT_VEL_STD; 62 | V3D INIT_ATT_STD; 63 | V3D INIT_ACC_STD; 64 | V3D INIT_GYR_STD; 65 | 66 | // !@INITIAL IMU BIASES 67 | V3D INIT_BA; 68 | V3D INIT_BW; 69 | 70 | // !@EXTRINSIC_PARAMETERS 71 | V3D INIT_TBL; 72 | Q4D INIT_RBL; 73 | 74 | double OriLon; 75 | double OriLat; 76 | double OriAlt; 77 | double OriYaw; 78 | double OriPitch; 79 | double OriRoll; 80 | double compensate_init_yaw; 81 | double compensate_init_pitch; 82 | double compensate_init_roll; 83 | double mappingCarYawPara; 84 | double InitPose_x; 85 | double InitPose_y; 86 | double InitPose_yaw; 87 | 88 | template 89 | T readParam(ros::NodeHandle& n, std::string name) { 90 | T ans; 91 | if (n.getParam(name, ans)) { 92 | // ROS_INFO_STREAM("Loaded " << name << ": " << ans); 93 | } else { 94 | ROS_ERROR_STREAM("Failed to load " << name); 95 | n.shutdown(); 96 | } 97 | return ans; 98 | } 99 | 100 | void readParameters(ros::NodeHandle& n) { 101 | std::string config_file; 102 | config_file = readParam(n, "config_file"); 103 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 104 | if (!fsSettings.isOpened()) { 105 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 106 | } 107 | 108 | CALIBARTE_IMU = fsSettings["calibrate_imu"]; 109 | SHOW_CONFIGURATION = fsSettings["show_configuration"]; 110 | AVERAGE_NUMS = fsSettings["average_nums"]; 111 | IMU_LIDAR_EXTRINSIC_ANGLE = fsSettings["imu_lidar_extrinsic_angle"]; 112 | IMU_MISALIGN_ANGLE = fsSettings["imu_misalign_angle"]; 113 | LINE_NUM = fsSettings["line_num"]; 114 | SCAN_NUM = fsSettings["scan_num"]; 115 | SCAN_PERIOD = fsSettings["scan_period"]; 116 | EDGE_THRESHOLD = fsSettings["edge_threshold"]; 117 | SURF_THRESHOLD = fsSettings["surf_threshold"]; 118 | NEAREST_FEATURE_SEARCH_SQ_DIST = fsSettings["nearest_feature_search_sq_dist"]; 119 | VERBOSE = fsSettings["verbose"]; 120 | ICP_FREQ = fsSettings["icp_freq"]; 121 | MAX_LIDAR_NUMS = fsSettings["max_lidar_nums"]; 122 | NUM_ITER = fsSettings["num_iter"]; 123 | LIDAR_SCALE = fsSettings["lidar_scale"]; 124 | LIDAR_STD = fsSettings["lidar_std"]; 125 | 126 | fsSettings["imu_topic"] >> IMU_TOPIC; 127 | fsSettings["lidar_topic"] >> LIDAR_TOPIC; 128 | fsSettings["lidar_odometry_topic"] >> LIDAR_ODOMETRY_TOPIC; 129 | fsSettings["lidar_mapping_topic"] >> LIDAR_MAPPING_TOPIC; 130 | 131 | ACC_N = fsSettings["acc_n"]; 132 | ACC_W = fsSettings["acc_w"]; 133 | GYR_N = fsSettings["gyr_n"]; 134 | GYR_W = fsSettings["gyr_w"]; 135 | 136 | readV3D(&fsSettings, "init_pos_std", INIT_POS_STD); 137 | readV3D(&fsSettings, "init_vel_std", INIT_VEL_STD); 138 | readV3D(&fsSettings, "init_att_std", INIT_ATT_STD); 139 | readV3D(&fsSettings, "init_acc_std", INIT_ACC_STD); 140 | readV3D(&fsSettings, "init_gyr_std", INIT_GYR_STD); 141 | 142 | readV3D(&fsSettings, "init_ba", INIT_BA); 143 | readV3D(&fsSettings, "init_bw", INIT_BW); 144 | readV3D(&fsSettings, "init_tbl", INIT_TBL); 145 | readQ4D(&fsSettings, "init_rbl", INIT_RBL); 146 | } 147 | 148 | void readInitPose(ros::NodeHandle& n) 149 | { 150 | std::string config_file; 151 | config_file = readParam(n, "init_pose"); 152 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); 153 | if (!fsSettings.isOpened()) { 154 | std::cerr << "ERROR: Wrong path to settings" << std::endl; 155 | } 156 | 157 | OriLon = fsSettings["OriLon"]; 158 | OriLat = fsSettings["OriLat"]; 159 | OriAlt = fsSettings["OriAlt"]; 160 | OriYaw = fsSettings["OriYaw"]; 161 | OriPitch = fsSettings["OriPitch"]; 162 | OriRoll = fsSettings["OriRoll"]; 163 | 164 | compensate_init_yaw = fsSettings["compensate_init_yaw"]; 165 | compensate_init_pitch = fsSettings["compensate_init_pitch"]; 166 | compensate_init_roll = fsSettings["compensate_init_roll"]; 167 | mappingCarYawPara = fsSettings["mappingCarYawPara"]; 168 | 169 | InitPose_x = fsSettings["InitPose_x"]; 170 | InitPose_y = fsSettings["InitPose_y"]; 171 | InitPose_yaw = fsSettings["InitPose_yaw"]; 172 | } 173 | 174 | void readV3D(cv::FileStorage* file, const std::string& name, 175 | V3D& vec_eigen) { 176 | cv::Mat vec_cv; 177 | (*file)[name] >> vec_cv; 178 | cv::cv2eigen(vec_cv, vec_eigen); 179 | } 180 | 181 | void readQ4D(cv::FileStorage* file, const std::string& name, 182 | Q4D& quat_eigen) { 183 | cv::Mat mat_cv; 184 | (*file)[name] >> mat_cv; 185 | M3D mat_eigen; 186 | cv::cv2eigen(mat_cv, mat_eigen); 187 | Q4D quat(mat_eigen); 188 | quat_eigen = quat.normalized(); 189 | } 190 | 191 | } // namespace parameter 192 | -------------------------------------------------------------------------------- /src/lins_fusion_node.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of LINS. 2 | // 3 | // Copyright (C) 2020 Chao Qin , 4 | // Robotics and Multiperception Lab (RAM-LAB ), 5 | // The Hong Kong University of Science and Technology 6 | // 7 | // Redistribution and use in source and binary forms, with or without 8 | // modification, are permitted provided that the following conditions are met: 9 | // 1. Redistributions of source code must retain the above copyright notice, 10 | // this list of conditions and the following disclaimer. 11 | // 2. Redistributions in binary form must reproduce the above copyright notice, 12 | // this list of conditions and the following disclaimer in the documentation 13 | // and/or other materials provided with the distribution. 14 | // 3. Neither the name of the copyright holder nor the names of its 15 | // contributors may be used to endorse or promote products derived from this 16 | // software without specific prior written permission. 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | int main(int argc, char** argv) { 24 | ros::init(argc, argv, "lins_fusion_node"); 25 | ros::NodeHandle nh; 26 | ros::NodeHandle pnh("~"); 27 | 28 | ROS_INFO("\033[1;32m---->\033[0m LINS Fusion Started."); 29 | 30 | parameter::readParameters(pnh); 31 | 32 | fusion::LinsFusion lins(nh, pnh); 33 | lins.run(); 34 | 35 | ros::spin(); 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /src/staticTFpub_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "parameters.h" 6 | #include "xmldom/XmlDomDocument.h" 7 | 8 | using namespace std; 9 | using namespace parameter; 10 | 11 | class staticTFpub 12 | { 13 | public: 14 | ros::NodeHandle pnh; 15 | ros::Subscriber imu_sub; 16 | tf::TransformBroadcaster pub_map_tf_pcl_map; 17 | tf::Transform map_tf_pcl_map; 18 | 19 | tf::TransformBroadcaster pub_map_tf_planning_map; 20 | tf::Transform map_tf_planning_map; 21 | 22 | string osm_file; 23 | 24 | shared_ptr doc; 25 | 26 | double mgrs_x = 0; 27 | double mgrs_y = 0; 28 | string xml_value; 29 | public: 30 | staticTFpub(ros::NodeHandle& nh) : pnh(nh) { 31 | imu_sub = pnh.subscribe(IMU_TOPIC, 100, &staticTFpub::ImuHandler, this); 32 | 33 | map_tf_pcl_map.setOrigin(tf::Vector3(InitPose_x, InitPose_y, 0.0)); 34 | tf::Quaternion q; 35 | q.setRPY(0, 0, InitPose_yaw); 36 | map_tf_pcl_map.setRotation(q); 37 | 38 | pnh.param("osm_file", osm_file, "osm.yaml"); 39 | 40 | doc = make_shared(osm_file.c_str()); 41 | 42 | if(doc) 43 | { 44 | for(int i=0; igetChildCount("node", 0, "tag"); i++) 45 | { 46 | if(doc->getChildAttribute("node", 0, "tag", i, "k") == "local_x") 47 | { 48 | mgrs_x = atof(doc->getChildAttribute("node", 0, "tag", i, "v").c_str()); 49 | } 50 | if(doc->getChildAttribute("node", 0, "tag", i, "k") == "local_y") 51 | { 52 | mgrs_y = atof(doc->getChildAttribute("node", 0, "tag", i, "v").c_str()); 53 | } 54 | } 55 | } 56 | 57 | map_tf_planning_map.setOrigin(tf::Vector3(mgrs_x, mgrs_y, 0.0)); 58 | q.setRPY(0, 0, 0); 59 | map_tf_planning_map.setRotation(q); 60 | } 61 | void ImuHandler(const sensor_msgs::Imu::ConstPtr& msg) 62 | { 63 | pub_map2pcl_map(msg->header.stamp); 64 | pub_map2planning_map(msg->header.stamp); 65 | } 66 | void pub_map2pcl_map(ros::Time timestamp) 67 | { 68 | pub_map_tf_pcl_map.sendTransform(tf::StampedTransform(map_tf_pcl_map, timestamp, "map", "pcl_map")); 69 | } 70 | void pub_map2planning_map(ros::Time timestamp) 71 | { 72 | pub_map_tf_planning_map.sendTransform(tf::StampedTransform(map_tf_planning_map, timestamp, "map", "planning_map")); 73 | } 74 | }; 75 | 76 | int main(int argc, char** argv) 77 | { 78 | ros::init(argc, argv, "staticTFpub_node"); 79 | ros::NodeHandle nh("~"); 80 | 81 | parameter::readInitPose(nh); 82 | 83 | staticTFpub staticTFpub_(nh); 84 | 85 | ros::Rate rate(500); // 10Hz 86 | 87 | while(ros::ok()) 88 | { 89 | rate.sleep(); 90 | ros::spinOnce(); 91 | // staticTFpub_.pub_map2pcl_map(); 92 | // staticTFpub_.pub_map2planning_map(); 93 | } 94 | 95 | return 0; 96 | } -------------------------------------------------------------------------------- /urdf/smartcar.urdf: -------------------------------------------------------------------------------- 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 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | --------------------------------------------------------------------------------