├── lidar_gnss_mapping ├── msg │ ├── gps_status.msg │ ├── gps_ins_msg.msg │ ├── odom_and_status.msg │ ├── odom_and_ctrl.msg │ ├── gpgga_msg.msg │ └── headinga_msg.msg ├── imgs │ ├── Campus3.png │ └── framework.png ├── include │ ├── loam_gtsam.h │ ├── math_process.hpp │ ├── pointType.h │ ├── gnss_process.hpp │ └── loopOptimize.h ├── launch │ └── lidar_gnss_mapping.launch ├── CMakeLists.txt ├── package.xml └── src │ ├── write_file.cpp │ ├── gps_ins_node.cpp │ ├── register_lidar_gps_new.cpp │ ├── rtk_registration.cpp │ └── rtk_odom.cpp ├── README.md └── LICENSE /lidar_gnss_mapping/msg/gps_status.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 status 3 | int32 sat_num -------------------------------------------------------------------------------- /lidar_gnss_mapping/msg/gps_ins_msg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | nav_msgs/Odometry odom 3 | int32 gps_status 4 | int32 sat_num -------------------------------------------------------------------------------- /lidar_gnss_mapping/imgs/Campus3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZhuangYanDLUT/lidar_gnss_mapping/HEAD/lidar_gnss_mapping/imgs/Campus3.png -------------------------------------------------------------------------------- /lidar_gnss_mapping/msg/odom_and_status.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | nav_msgs/Odometry odom 3 | int32 status 4 | int32 sat_num 5 | int32 ctrl_flag -------------------------------------------------------------------------------- /lidar_gnss_mapping/imgs/framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ZhuangYanDLUT/lidar_gnss_mapping/HEAD/lidar_gnss_mapping/imgs/framework.png -------------------------------------------------------------------------------- /lidar_gnss_mapping/msg/odom_and_ctrl.msg: -------------------------------------------------------------------------------- 1 | # Header header 2 | nav_msgs/Odometry odom 3 | int32 ctrl_flag 4 | float64 theta 5 | bool update 6 | int64 rtk_num 7 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/msg/gpgga_msg.msg: -------------------------------------------------------------------------------- 1 | 2 | Header header 3 | 4 | string head 5 | float64 timeUTC 6 | 7 | float64 latitude 8 | string northOrSouth 9 | float64 longitude 10 | string eastOrWest 11 | 12 | int16 GPSstatus 13 | int16 satelliteNum 14 | float64 HDOP 15 | float64 altitude 16 | string altitudeUnit 17 | 18 | float64 geiod 19 | string geiodUnit 20 | int16 rtkAge 21 | string rtkID 22 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/msg/headinga_msg.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string head 4 | 5 | string calculateStatus 6 | string locateStatus 7 | float64 baseLine 8 | float64 yawAngle 9 | float64 pitchAngle 10 | float64 yuLiu1 11 | float64 yawAngleError 12 | float64 pitchAngleError 13 | string rtkID 14 | int16 satelliteTrackNum 15 | int16 satelliteCalculateNum 16 | int16 satelliteHighNum 17 | int16 satelliteL2Num 18 | int16 yuLiu2 19 | int16 expandCalculateStatus 20 | int16 yuLiu3 21 | int16 signalMaskStatus 22 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/include/loam_gtsam.h: -------------------------------------------------------------------------------- 1 | #ifndef _LOAM_GTSAM_H_ 2 | #define _LOAM_GTSAM_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #endif 20 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/launch/lidar_gnss_mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/include/math_process.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __data_process_h__ 2 | #define __data_process_h__ 3 | 4 | #include 5 | #include 6 | #include 7 | typedef geometry_msgs::Point ROSPoi3d; 8 | typedef geometry_msgs::Vector3 ROSVec3d; 9 | void QuatMultiply(double w1, double x1, double y1, double z1, double w2, double x2, 10 | double y2, double z2, double &w3, double &x3, double &y3, 11 | double &z3) { 12 | w3 = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2; 13 | x3 = w1 * x2 + x1 * w2 + z1 * y2 - y1 * z2; 14 | y3 = w1 * y2 + y1 * w2 + x1 * z2 - z1 * x2; 15 | z3 = w1 * z2 + z1 * w2 + y1 * x2 - x1 * y2; 16 | } 17 | ROSVec3d VecMinus(ROSVec3d src1, ROSVec3d src2) { 18 | static ROSVec3d rst; 19 | rst.x = src1.x - src2.x; 20 | rst.y = src1.y - src2.y; 21 | rst.z = src1.z - src2.z; 22 | return rst; 23 | } 24 | ROSVec3d VecPlus(ROSVec3d src1, ROSVec3d src2) { 25 | static ROSVec3d rst; 26 | rst.x = src1.x + src2.x; 27 | rst.y = src1.y + src2.y; 28 | rst.z = src1.z + src2.z; 29 | return rst; 30 | } 31 | ROSPoi3d PoiMinus(ROSPoi3d src1, ROSPoi3d src2) { 32 | static ROSPoi3d rst; 33 | rst.x = src1.x - src2.x; 34 | rst.y = src1.y - src2.y; 35 | rst.z = src1.z - src2.z; 36 | return rst; 37 | } 38 | ROSPoi3d PoiPlus(ROSPoi3d src1, ROSPoi3d src2) { 39 | static ROSPoi3d rst; 40 | rst.x = src1.x + src2.x; 41 | rst.y = src1.y + src2.y; 42 | rst.z = src1.z + src2.z; 43 | return rst; 44 | } 45 | 46 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LiDAR GNSS mapping 2 | ## Framework 3 | ![image](https://github.com/ZhuangYanDLUT/lidar_gnss_mapping/blob/master/lidar_gnss_mapping/imgs/framework.png) 4 | The overall system framework for large-scale 3D map building in partially GNSS-denied scenes, which consists of two operating modes: `LiDAR-only mode` and `GNSS-LiDAR mode`. While working in GNSS-denied scenes, LiDAR odometry runs with high frequency and outputs estimations with local registration errors, and LiDAR mapping provides more accurate pose estimations with low frequency. 5 | While moving from GNSS-denied scenes to open scenes, 3D-M-Box is working in `GNSS-LiDAR mode`. The auto coordinate alignment algorithm is applied to align the coordinate system between LiDAR and GNSS by registering a group of poses obtained from LiDAR mapping and GNSS within a certain time window. Finally, GNSS-constrained LiDAR mapping outputs the pose and point clouds with high accuracy. 6 | ## Sample map 7 | ![image](https://github.com/ZhuangYanDLUT/lidar_gnss_mapping/blob/master/lidar_gnss_mapping/imgs/Campus3.png) 8 | The picture shows that 3D-M-Box can accomplish online pose estimation and map building in GNSS-denied scenes. 9 | ## Dependency 10 | **ROS** (tested with kinetic) 11 | **gtsam** (Georgia Tech Smoothing and Mapping library, 4.0.0-alpha2) 12 | ``` 13 | wget -O ~/Downloads/gtsam.zip https://github.com/borglab/gtsam/archive/4.0.0-alpha2.zip 14 | cd ~/Downloads/ && unzip gtsam.zip -d ~/Downloads/ 15 | mkdir build && cd build 16 | cmake .. 17 | sudo make install 18 | ``` 19 | ## How to build with catkin 20 | ``` 21 | cd ~/catkin_ws/src 22 | git clone https://github.com/ZhuangYanDLUT/lidar_gnss_mapping.git 23 | cd ~/catkin_ws 24 | catkin_make 25 | ``` 26 | ## Running 27 | `roslaunch lidar_gnss_mapping lidar_gnss_mapping.launch` 28 | In second terminal play sample data from [test.bag](https://pan.baidu.com/s/1KUwSCFyufw5symvmWUorYA) (the access code is `ri4b`) 29 | `rosbag play -s 35 test.bag` 30 | 31 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/include/pointType.h: -------------------------------------------------------------------------------- 1 | #ifndef _POINT_TYPE_H_ 2 | #define _POINT_TYPE_H_ 3 | 4 | 5 | #include 6 | 7 | 8 | #include 9 | #include 10 | //#include 11 | //#include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | using namespace std; 20 | 21 | typedef pcl::PointXYZI PointType; 22 | typedef pcl::PointXYZ CurPointType; 23 | ////////////////////////////////////////////// 24 | struct PointXYZIRPYT 25 | { 26 | PCL_ADD_POINT4D 27 | PCL_ADD_INTENSITY; 28 | float roll; 29 | float pitch; 30 | float yaw; 31 | double time; 32 | double index; 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 34 | } EIGEN_ALIGN16; 35 | 36 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, 37 | (float, x, x) (float, y, y) 38 | (float, z, z) (float, intensity, intensity) 39 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 40 | (double, time, time) (double, index, index) 41 | ) 42 | 43 | typedef PointXYZIRPYT PointTypePose; 44 | ////////////////////////////////////////////////// 45 | struct PointXYZRPY 46 | { 47 | PCL_ADD_POINT4D 48 | 49 | float roll; 50 | float pitch; 51 | float yaw; 52 | 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 54 | } EIGEN_ALIGN16; 55 | 56 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZRPY, 57 | (float, x, x) (float, y, y) 58 | (float, z, z) 59 | (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) 60 | 61 | ) 62 | 63 | typedef PointXYZRPY CurPointTypePose; 64 | typedef PointXYZIRPYT PointTypePose; 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lidar_gnss_mapping) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | nav_msgs 9 | pcl_ros 10 | roscpp 11 | message_generation 12 | sensor_msgs 13 | std_msgs 14 | ) 15 | find_package(OpenCV REQUIRED) 16 | find_package(PCL REQUIRED) 17 | find_package(GTSAM REQUIRED QUIET) 18 | 19 | ## Generate messages in the 'msg' folder 20 | add_message_files( 21 | FILES 22 | gps_ins_msg.msg 23 | gpgga_msg.msg 24 | odom_and_status.msg 25 | gps_status.msg 26 | odom_and_ctrl.msg 27 | headinga_msg.msg 28 | ) 29 | 30 | ## Generate added messages and services with any dependencies listed here 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs # Or other packages containing msgs 34 | nav_msgs 35 | ) 36 | 37 | catkin_package( 38 | # INCLUDE_DIRS include 39 | # LIBRARIES pose_integration 40 | # CATKIN_DEPENDS pcl_ros roscpp 41 | # DEPENDS system_lib 42 | ) 43 | 44 | ########### 45 | ## Build ## 46 | ########### 47 | 48 | include_directories( 49 | include 50 | ${catkin_INCLUDE_DIRS} 51 | ${OpenCV_INCLUDE_DIRS} 52 | ${PCL_INCLUDE_DIRS} 53 | ${GTSAM_INCLUDE_DIR} 54 | ) 55 | link_directories( 56 | include 57 | ${OpenCV_LIBRARY_DIRS} 58 | ${PCL_LIBRARY_DIRS} 59 | ${GTSAM_LIBRARY_DIRS} 60 | ) 61 | 62 | add_executable(${PROJECT_NAME}_gps_ins_node src/gps_ins_node.cpp) 63 | add_executable(${PROJECT_NAME}_rtk_registration src/rtk_registration.cpp) 64 | add_executable(${PROJECT_NAME}_rtk_odom src/rtk_odom.cpp) 65 | add_executable(${PROJECT_NAME}_rtk_mapping src/rtk_mapping.cpp) 66 | add_executable(${PROJECT_NAME}_register_lidar_gps_new src/register_lidar_gps_new.cpp) 67 | 68 | add_dependencies(${PROJECT_NAME}_gps_ins_node ${PROJECT_NAME}_gencpp) 69 | add_dependencies(${PROJECT_NAME}_rtk_registration ${PROJECT_NAME}_gencpp) 70 | add_dependencies(${PROJECT_NAME}_rtk_odom ${PROJECT_NAME}_gencpp) 71 | add_dependencies(${PROJECT_NAME}_rtk_mapping ${PROJECT_NAME}_gencpp) 72 | add_dependencies(${PROJECT_NAME}_register_lidar_gps_new ${PROJECT_NAME}_gencpp) 73 | 74 | target_link_libraries(${PROJECT_NAME}_gps_ins_node ${catkin_LIBRARIES}) 75 | target_link_libraries(${PROJECT_NAME}_rtk_registration ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 76 | target_link_libraries(${PROJECT_NAME}_rtk_odom ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 77 | target_link_libraries(${PROJECT_NAME}_rtk_mapping ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 78 | target_link_libraries(${PROJECT_NAME}_register_lidar_gps_new ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} gtsam) 79 | 80 | 81 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_gnss_mapping 4 | 0.0.0 5 | The lidar_gnss_mapping package 6 | 7 | GuojianHe 8 | 9 | 10 | 11 | 12 | 13 | TODO 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 | catkin 49 | nav_msgs 50 | pcl_ros 51 | roscpp 52 | sensor_msgs 53 | std_msgs 54 | nav_msgs 55 | pcl_ros 56 | roscpp 57 | sensor_msgs 58 | std_msgs 59 | nav_msgs 60 | pcl_ros 61 | roscpp 62 | sensor_msgs 63 | std_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/src/write_file.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include "rtk_slam/odom_and_ctrl.h" 16 | #include "rtk_slam/odom_and_status.h" 17 | #include "math_process.hpp" 18 | 19 | #include 20 | #include 21 | #include "pointType.h" 22 | 23 | pcl::PointCloud::Ptr laserCloudFullRes; 24 | 25 | std::string str_head = "/home/yxd/outdoor_ws/src/rtk_slam/launch/data/10/"; 26 | std::string str_end = "2.txt"; 27 | bool first_recv(true); 28 | bool first_loam_recv(true); 29 | bool first_optimize_recv(true); 30 | bool write_pointcloud_to_file_XYZI(string str,pcl::PointCloud::Ptr cloudIn){ 31 | std::ofstream of(str,std::ios::app); 32 | if(!of.is_open()){ 33 | std::cout<<"open file "<points.size();++i){ 37 | PointType *pointTmp; 38 | pointTmp = &cloudIn->points[i]; 39 | of<x<<" "<y<<" "<z<<" "<intensity<::Ptr cloudIn){ 45 | std::ofstream of(str,std::ios::app); 46 | if(!of.is_open()){ 47 | std::cout<<"open file "<points.size();++i){ 51 | PointType *pointTmp; 52 | pointTmp = &cloudIn->points[i]; 53 | of<x<<" "<y<<" "<z<()); 63 | pcl::fromROSMsg(*keyPoseIn,*laserCloudFullRes); //从点云数据类型转换为PointType 64 | std::string str = str_head + "KeyPose_XYZ_"+str_end; 65 | std::cout<<'\n'<<"write key pose to "<()); 74 | pcl::fromROSMsg(*loamPoseIn,*laserCloudFullRes); //从点云数据类型转换为PointType 75 | std::string str = str_head + "LoamPose_XYZ_"+str_end; 76 | std::cout<<'\n'<<"write loam pose to "<()); 85 | pcl::fromROSMsg(*optimizePoseIn,*laserCloudFullRes); //从点云数据类型转换为PointType 86 | std::string str = str_head + "Optimize_Pose_XYZ_"+str_end; 87 | std::cout<<'\n'<<"write optimize pose to "<header.stamp.toSec(); 94 | 95 | laserCloudFullRes.reset(new pcl::PointCloud()); 96 | 97 | pcl::fromROSMsg(*laserCloudFullRes2,*laserCloudFullRes); //从点云数据类型转换为PointType 98 | std::vector indices; 99 | pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes,indices);//去除点云中的无效点 100 | std::string str = str_head + "pointcoud"+str_end; 101 | write_pointcloud_to_file_XYZ(str,laserCloudFullRes); 102 | } 103 | int main(int argc,char**argv){ 104 | ros::init(argc,argv,"write_file"); 105 | ros::NodeHandle nh; 106 | ros::Subscriber subLaserCloudFullRes = 107 | nh.subscribe("/laser_cloud_surround_rtk", 2, laserCloudFullResHandler); 108 | //nh.advertise("/key_pose_origin", 2); 109 | ros::Subscriber subKeyPose = 110 | nh.subscribe("/key_pose_origin", 2, keyPoseHandler); 111 | 112 | ros::Subscriber subLoamPose = 113 | nh.subscribe("/loam_pose", 2, loamPoseHandler); 114 | 115 | ros::Subscriber subOptimizePoses = nh.subscribe("/optimize_pose",2,optimizePoseHandler); 116 | ros::Rate rate(10); 117 | while(ros::ok()){ 118 | ros::spinOnce(); 119 | rate.sleep(); 120 | } 121 | } -------------------------------------------------------------------------------- /lidar_gnss_mapping/include/gnss_process.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __data_process_hpp__ 2 | #define __data_process_hpp__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | using namespace Eigen; 12 | 13 | #ifndef __pi__ 14 | #define __pi__ 15 | const double PI = 3.1415926535898; 16 | #endif 17 | 18 | typedef geometry_msgs::Vector3 ROSVec3d; 19 | typedef geometry_msgs::Point ROSPoi3d; 20 | bool isFirstGPS(true); 21 | double first_Xe; 22 | double first_Ye; 23 | double first_Ze; 24 | 25 | //Geocentric latitude and longitude coordinates of the high-coordinate data is converted to Northeast day coordinate system coordinates 26 | void BLH2ENU(double lat, double lon, double alt, double &x, double &y, 27 | double &z, double sin_lat, double cos_lat, double sin_lon, 28 | double cos_lon) { 29 | lat = lat * M_PI / 180; // To rad. 30 | lon = lon * M_PI / 180; 31 | double f = 1 / 298.257223563; // WGS84 32 | double A_GNSS = 6378137.0; // WGS84 33 | double B_GNSS = A_GNSS * (1 - f); 34 | double e = sqrt(A_GNSS * A_GNSS - B_GNSS * B_GNSS) / A_GNSS; 35 | double N = A_GNSS / sqrt(1 - e * e * sin(lat) * sin(lat)); 36 | // To ECEF 37 | double Xe = (N + alt) * cos(lat) * cos(lon); 38 | double Ye = (N + alt) * cos(lat) * sin(lon); 39 | double Ze = (N * (1 - (e * e)) + alt) * sin(lat); 40 | if(isFirstGPS){ 41 | first_Xe = Xe; 42 | first_Ye = Ye; 43 | first_Ze = Ze; 44 | isFirstGPS = false; 45 | } 46 | Xe -= first_Xe; 47 | Ye -= first_Ye; 48 | Ze -= first_Ze; 49 | 50 | // To ENU 51 | x = -Xe * sin_lon + Ye * cos_lon; 52 | y = -Xe * sin_lat * cos_lon - Ye * sin_lat * sin_lon + Ze * cos_lat; 53 | z = Xe * cos_lat * cos_lon + Ye * cos_lat * sin_lon + Ze * sin_lat; 54 | } 55 | void AverageFilter(ROSPoi3d &inPos, ROSPoi3d &outPos, int num) { 56 | static int countIn(0); 57 | static const int pointNum = num; // 50点 58 | static float *smX = new float[pointNum]; 59 | static float *smY = new float[pointNum]; 60 | static float *smZ = new float[pointNum]; 61 | if (countIn < pointNum) { 62 | countIn++; 63 | smX[countIn - 1] = inPos.x; 64 | smY[countIn - 1] = inPos.y; 65 | smZ[countIn - 1] = inPos.z; 66 | outPos = inPos; 67 | return; 68 | } 69 | for (int i = 0; i < pointNum - 1; ++i) { 70 | smX[i] = smX[i + 1]; 71 | smY[i] = smY[i + 1]; 72 | smZ[i] = smZ[i + 1]; 73 | } 74 | smX[pointNum - 1] = inPos.x; 75 | smY[pointNum - 1] = inPos.y; 76 | smZ[pointNum - 1] = inPos.z; 77 | float sumX(0), sumY(0), sumZ(0); 78 | for (int i = 0; i < pointNum; ++i) { 79 | sumX += smX[i]; 80 | sumY += smY[i]; 81 | sumZ += smZ[i]; 82 | } 83 | outPos.x = sumX / pointNum; 84 | outPos.y = sumY / pointNum; 85 | outPos.z = sumZ / pointNum; 86 | } 87 | //Kalman filter 88 | void KalmanFun(const ROSVec3d &acc, ROSPoi3d &inPos, double dt, 89 | ROSPoi3d &outPos, int status) { 90 | 91 | static bool isFirstKal(true); 92 | static MatrixXd A(4, 4); 93 | static MatrixXd B(4, 2); 94 | static MatrixXd u(2, 1); 95 | static MatrixXd Q(4, 4); 96 | static MatrixXd H(2, 4); 97 | static MatrixXd R(2, 2); 98 | static MatrixXd X_pdct(4, 1); 99 | static MatrixXd Pk_pdct(4, 4); 100 | static MatrixXd K(4, 2); 101 | static MatrixXd Z_meas(2, 1); 102 | static MatrixXd X_evlt(4, 1); 103 | static MatrixXd Pk_evlt(4, 4); 104 | if (isFirstKal) { 105 | 106 | X_pdct.setZero(); 107 | Pk_pdct.setZero(); 108 | K.setZero(); 109 | Z_meas.setZero(); 110 | X_evlt.setZero(); 111 | Pk_evlt.setZero(); 112 | isFirstKal = false; 113 | } 114 | A << 1, 0, dt, 0, 0, 1, 0, dt, 0, 0, 1, 0, 0, 0, 0, 1; 115 | 116 | B << 0.5 * dt * dt, 0, 0, 0.5 * dt * dt, dt, 0, 0, dt; 117 | 118 | u << acc.x, acc.y; 119 | 120 | Q << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 121 | 0.01; 122 | 123 | H << 1, 0, 0, 0, 0, 1, 0, 124 | 0; 125 | if (status < 2) { 126 | R << 1, 0, 0, 1; 127 | } else if (status == 2) { 128 | R << 0.5, 0, 0, 0.5; 129 | } else { 130 | R << 0.01, 0, 0, 0.01; 131 | } 132 | 133 | 134 | X_pdct = A * X_evlt + B * u; 135 | Pk_pdct = A * Pk_evlt * A.transpose() + Q; 136 | 137 | MatrixXd tmp(2, 2); 138 | tmp = H * Pk_pdct * H.transpose() + R; 139 | K = Pk_pdct * H.transpose() * tmp.inverse(); 140 | 141 | Z_meas << inPos.x, inPos.y; 142 | X_evlt = X_pdct + K * (Z_meas - H * X_pdct); 143 | Pk_evlt = (MatrixXd::Identity(4, 4) - K * H) * Pk_pdct; 144 | 145 | outPos.x = X_evlt(0, 0); 146 | outPos.y = X_evlt(1, 0); 147 | outPos.z = inPos.z; 148 | } 149 | 150 | float GetMeanValue(const int num, const ROSPoi3d *que, const int &len, 151 | const int &cur_index) { 152 | float sum(0); 153 | for (int i = 0; i < num; ++i) { 154 | sum += que[(cur_index - i + len) % len].z; 155 | } 156 | return sum / num; 157 | } 158 | 159 | float GetVarianceValue(const int num, const ROSPoi3d *que, const int &len, 160 | const int &cur_index) { 161 | float mean = GetMeanValue(num, que, len, cur_index); 162 | float sum(0), differ(0); 163 | for (int i = 0; i < num; ++i) { 164 | differ = que[(cur_index - i + len) % len].z - mean; 165 | sum += differ * differ; 166 | } 167 | return sum / num; 168 | } 169 | 170 | void GetMeanAndVarianceValue(const int num, const ROSPoi3d *que, const int &len, 171 | const int &cur_index, float &mean, 172 | float &variance) { 173 | mean = GetMeanValue(num, que, len, cur_index); 174 | float sum(0), differ(0); 175 | for (int i = 0; i < num; ++i) { 176 | differ = que[(cur_index - i + len) % len].z - mean; 177 | sum += differ * differ; 178 | } 179 | variance = sum / num; 180 | } 181 | 182 | bool IsOutliers(ROSPoi3d *que, const int len, const int cur_index) { 183 | static int point_count(0); 184 | static float mean(0), variance(0); 185 | if (point_count < 40) { 186 | ++point_count; 187 | } else { 188 | GetMeanAndVarianceValue(40, que, len, cur_index, mean, variance); 189 | } 190 | // printf("%f %f\n", mean, variance); 191 | if (variance > 0.2) { 192 | return true; 193 | } else { 194 | return false; 195 | } 196 | } 197 | #endif -------------------------------------------------------------------------------- /lidar_gnss_mapping/include/loopOptimize.h: -------------------------------------------------------------------------------- 1 | #ifndef _LOOP_OPTIMIZE_H_ 2 | #define _LOOP_OPTIMIZE_H_ 3 | #include "loam_gtsam.h" 4 | #include "pointType.h" 5 | 6 | using namespace gtsam; 7 | class loamGtsam{ 8 | private: 9 | 10 | NonlinearFactorGraph gtSAMgraph; 11 | Values initialEstimate; 12 | Values optimizedEstimate; 13 | ISAM2 *isam; 14 | ISAM2Params parameters; 15 | Values isamCurrentEstimate; 16 | 17 | noiseModel::Diagonal::shared_ptr priorNoise; 18 | noiseModel::Diagonal::shared_ptr odometryNoise; 19 | noiseModel::Diagonal::shared_ptr constraintNoise; 20 | 21 | 22 | pcl::PointCloud::Ptr odomKeyPoses6D;//保存要优化的位姿 23 | pcl::PointCloud::Ptr cloudKeyPoses3D; 24 | PointTypePose laser_gps; 25 | PointType path_data; 26 | bool newdata; 27 | bool loopClosureEnableFlag; 28 | 29 | public: 30 | PointTypePose odometry; 31 | pcl::PointCloud::Ptr currentOdomKeyPoses6D;//校正后的位姿 32 | pcl::PointCloud::Ptr currentCloudKeyPoses3D; 33 | double odom_num; //保存odom数目 34 | loamGtsam(){ 35 | Vector Vector6(6); 36 | Vector6 << 1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-6; 37 | priorNoise = noiseModel::Diagonal::Variances(Vector6);//生成噪声 38 | odometryNoise = noiseModel::Diagonal::Variances(Vector6);//噪声定义，对角线矩阵 39 | //参数设置 40 | 41 | parameters.relinearizeThreshold = 0.01; 42 | parameters.relinearizeSkip = 1; 43 | //生成ISAM2类型 44 | isam = new ISAM2(parameters); 45 | newdata = false;//是否为新的数据成员变量 46 | loopClosureEnableFlag = false; 47 | odomKeyPoses6D.reset(new pcl::PointCloud()); //保存变量的数组 48 | cloudKeyPoses3D.reset(new pcl::PointCloud()); 49 | 50 | currentOdomKeyPoses6D.reset(new pcl::PointCloud()); 51 | currentCloudKeyPoses3D.reset(new pcl::PointCloud()); 52 | odom_num = 0; //初始设定需要优化的里程计的数目为零 53 | 54 | } 55 | //void recv_data(const PointTypePose &odometry){ 56 | //读取该类中odometry上的数据，并将其保存在要进行优化的数组中 57 | void recv_data(){ 58 | //std::cout<<"recv_data"<points.size(); //index 69 | 70 | odomKeyPoses6D->push_back(laser_gps); 71 | 72 | path_data.x = laser_gps.x; 73 | path_data.y = laser_gps.y; 74 | path_data.z = laser_gps.z; 75 | //std::cout<(0,gtsam::Pose3(R,t),priorNoise)); 95 | initialEstimate.insert(0, Pose3(Rot3::RzRyRx(laser_gps.yaw,laser_gps.roll,laser_gps.pitch), 96 | Point3(laser_gps.z,laser_gps.x,laser_gps.y))); 97 | gtSAMgraph.push_back(factor1); 98 | }else{ 99 | //std::cout<<"no first"<points.size()<<" * "<points.size()<<" * "<points[pose6Dsize-1].yaw,odomKeyPoses6D->points[pose6Dsize-1].roll, 104 | odomKeyPoses6D->points[pose6Dsize-1].pitch), 105 | Point3(odomKeyPoses6D->points[pose6Dsize-1].z,odomKeyPoses6D->points[pose6Dsize-1].x,odomKeyPoses6D->points[pose6Dsize-1].y)); 106 | 107 | gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(odomKeyPoses6D->points[pose6Dsize].yaw,odomKeyPoses6D->points[pose6Dsize].roll, 108 | odomKeyPoses6D->points[pose6Dsize].pitch), 109 | Point3(odomKeyPoses6D->points[pose6Dsize].z,odomKeyPoses6D->points[pose6Dsize].x,odomKeyPoses6D->points[pose6Dsize].y)); 110 | 111 | gtSAMgraph.add(BetweenFactor(odom_num-1,odom_num,poseFrom.between(poseTo), odometryNoise)); 112 | initialEstimate.insert(odom_num, Pose3(Rot3::RzRyRx(odomKeyPoses6D->points[pose6Dsize].yaw,odomKeyPoses6D->points[pose6Dsize].roll, 113 | odomKeyPoses6D->points[pose6Dsize].pitch), 114 | Point3(odomKeyPoses6D->points[pose6Dsize].z,odomKeyPoses6D->points[pose6Dsize].x,odomKeyPoses6D->points[pose6Dsize].y))); 115 | 116 | } 117 | odom_num++; 118 | // std::cout<<"befor updata"<update(gtSAMgraph, initialEstimate); 120 | isam->update(); 121 | // std::cout<<"end updata"<1 && odomKeyPoses6D->points[laser_gps.index-1].intensity == 3){//闭环优化 126 | //std::cout<<" laser_gps.intensity == 1 "<1){//闭环优化 128 | // std::cout<<"loop closure "<points[laser_gps.index].intensity<points[laser_gps.index-1].intensity<points[laser_gps.index-2].intensity<(odom_num-1,gtsam::Pose3(R,t),priorNoise)); 136 | 137 | gtSAMgraph.push_back(factor2); 138 | isam->update(gtSAMgraph); 139 | isam->update(); 140 | gtSAMgraph.resize(0); 141 | odom_num++; 142 | loopClosureEnableFlag = true; 143 | std::cout<<"loop closure !!! "<()); 151 | currentCloudKeyPoses3D.reset(new pcl::PointCloud()); 152 | odomKeyPoses6D.reset(new pcl::PointCloud()); //保存变量的数组 153 | cloudKeyPoses3D.reset(new pcl::PointCloud()); 154 | 155 | isamCurrentEstimate = isam->calculateEstimate();//得到正确的位姿估计 156 | for(int i=0; i(i); 160 | thisPose6D.x = latestEstimate.translation().y(); 161 | thisPose6D.y = latestEstimate.translation().z(); 162 | thisPose6D.z = latestEstimate.translation().x(); 163 | thisPose6D.roll = latestEstimate.rotation().pitch(); 164 | thisPose6D.pitch = latestEstimate.rotation().yaw(); 165 | thisPose6D.yaw = latestEstimate.rotation().roll(); 166 | thisPose3D.x = thisPose6D.x; 167 | thisPose3D.y = thisPose6D.y; 168 | thisPose3D.z = thisPose6D.z; 169 | currentOdomKeyPoses6D->push_back(thisPose6D); 170 | currentCloudKeyPoses3D->push_back(thisPose3D); 171 | } 172 | delete isam; 173 | isam = new ISAM2(parameters); 174 | odom_num = 0; 175 | 176 | return true; 177 | } 178 | return false; 179 | } 180 | void run(){ 181 | //recv_data(odometry); 182 | recv_data(); 183 | path_optimize(); 184 | save_result(); 185 | } 186 | 187 | }; 188 | #endif 189 | 190 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/src/gps_ins_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "gnss_process.hpp" 19 | #include "lidar_gnss_mapping/odom_and_status.h" 20 | #include "lidar_gnss_mapping/gpgga_msg.h" 21 | 22 | #ifndef __pi__ 23 | #define __pi__ 24 | const double PI = 3.141592654; 25 | #endif 26 | 27 | typedef geometry_msgs::Vector3 ROSVec3d; 28 | typedef geometry_msgs::Point ROSPoi3d; 29 | 30 | bool newGps(false); 31 | double gpsInTime; 32 | double lat, lon, alt; 33 | double last_lat, last_lon, last_alt; 34 | int status, satNum; 35 | 36 | bool newImu(false); 37 | bool firstImu(true); 38 | double imuInTime; 39 | ROSVec3d imuInAcc; 40 | ROSVec3d imuInAngle; 41 | geometry_msgs::Quaternion orien; 42 | 43 | const int imuQueLen(1000); 44 | double imuTime[imuQueLen] = {0.0}; 45 | ROSVec3d imuRPY[imuQueLen]; 46 | ROSVec3d imuAcc[imuQueLen]; 47 | ROSVec3d imuVelo[imuQueLen]; 48 | ROSPoi3d imuShift[imuQueLen]; 49 | ROSVec3d imuRPYVel[imuQueLen]; 50 | int imuNew(0); 51 | 52 | const int gpsQueLen(100); 53 | double gpsTime[gpsQueLen] = {0.0}; 54 | ROSPoi3d gpsShift[gpsQueLen]; 55 | int gpsNew(0); 56 | 57 | const int gpsAftQueLen(200); 58 | double gpsAftTime[gpsAftQueLen]; 59 | ROSPoi3d gpsAftShift[gpsAftQueLen]; 60 | ROSVec3d gpsAftVelo[gpsAftQueLen]; 61 | int gpsAftNew(0); 62 | 63 | ROSPoi3d firstPoint; 64 | ROSVec3d firstRPY; 65 | bool isFirst(true); 66 | bool haveNoGpsData(true); 67 | double timeDiff(0.0); 68 | double sinLat, cosLat, sinLon, cosLon; 69 | 70 | ROSVec3d newestStableRpy; 71 | ROSPoi3d newestStablePoi; 72 | 73 | ros::Publisher *pubOdom(NULL), *pubOdom1(NULL); 74 | ros::Publisher *pubOdomAndStatus(NULL); 75 | ros::Publisher *pubPoint(NULL); 76 | // float resetloam[6] = {0}; 77 | bool new_delta_theta(false); 78 | geometry_msgs::Quaternion geoQuat; 79 | // void loamDeltaTheta(const nav_msgs::Odometry odom){ 80 | // new_delta_theta = true; 81 | // resetloam[0] = odom.pose.pose.orientation.x; //roll 82 | // resetloam[1] = odom.pose.pose.orientation.y; //pitch 83 | // resetloam[2] = odom.pose.pose.orientation.z; //yaw 84 | 85 | // resetloam[3] = odom.pose.pose.position.x; //x 86 | // resetloam[4] = odom.pose.pose.position.y; //y 87 | // resetloam[5] = odom.pose.pose.position.z; //z 88 | // } 89 | void GPSCallback(const lidar_gnss_mapping::gpgga_msg::ConstPtr &gpsIn) { 90 | gpsInTime = gpsIn->header.stamp.toSec(); 91 | lat = gpsIn->latitude; // Unit: degree 92 | lon = gpsIn->longitude;// 93 | alt = gpsIn->altitude; // Unit: metre 94 | status = gpsIn->GPSstatus; 95 | satNum = gpsIn->satelliteNum; 96 | newGps = true; 97 | } 98 | void IMUCallback(const sensor_msgs::Imu::ConstPtr &imuIn) { 99 | imuInTime = imuIn->header.stamp.toSec(); 100 | orien = imuIn->orientation; 101 | double roll, pitch, yaw; 102 | tf::Quaternion orientation; 103 | tf::quaternionMsgToTF(imuIn->orientation, orientation); 104 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); //rad 105 | 106 | double accX = (imuIn->linear_acceleration.x + 9.81 * sin(pitch)); 107 | double accY = (imuIn->linear_acceleration.y - 9.81 * sin(roll) * cos(pitch)); 108 | double accZ = (imuIn->linear_acceleration.z - 9.81 * cos(roll) * cos(pitch)); 109 | 110 | imuNew = (imuNew + 1) % imuQueLen; 111 | imuTime[imuNew] = imuInTime; 112 | imuRPY[imuNew].x = roll; 113 | imuRPY[imuNew].y = pitch; 114 | imuRPY[imuNew].z = yaw; 115 | imuRPYVel[imuNew].x = imuIn->angular_velocity.x; 116 | imuRPYVel[imuNew].y = imuIn->angular_velocity.y; 117 | imuRPYVel[imuNew].z = imuIn->angular_velocity.z; 118 | 119 | // printf("%f %f %f\n", imuRPY[imuNew].x, imuRPY[imuNew].y, imuRPY[imuNew].z); 120 | imuAcc[imuNew].x = accX; 121 | imuAcc[imuNew].y = accY; 122 | imuAcc[imuNew].z = accZ; 123 | newImu = true; 124 | } 125 | 126 | int main(int argc, char **argv) { 127 | ros::init(argc, argv, "gps_ins_node"); 128 | ros::NodeHandle nh; 129 | ros::Subscriber subGPS = 130 | nh.subscribe("/gnss/data", 1000, GPSCallback); 131 | ros::Subscriber subIMU = 132 | nh.subscribe("/imu/data", 1000, IMUCallback); 133 | // ros::Subscriber subDeltaTheta = nh.subscribe("/delta_theta",100,loamDeltaTheta); 134 | pubOdom = new ros::Publisher( 135 | nh.advertise("/gps_ins_odom", 1000)); 136 | pubOdomAndStatus = new ros::Publisher( 137 | nh.advertise("/odom_and_status", 1000)); 138 | 139 | ros::Rate rate(300); 140 | while (ros::ok()) { 141 | ros::spinOnce(); 142 | if (newGps && newImu) { 143 | newImu = false; 144 | newGps = false; 145 | //init 146 | if (isFirst) { 147 | if (status !=4 && status !=5) { // No GPS signal 148 | nav_msgs::Odometry odom; 149 | odom.header.stamp = ros::Time().fromSec(gpsAftTime[gpsAftNew]); 150 | odom.header.frame_id = "/camera_init"; 151 | odom.pose.pose.position.x = 0.0; 152 | odom.pose.pose.position.y = 0.0; 153 | odom.pose.pose.position.z = 0.0; 154 | odom.pose.pose.orientation = orien; 155 | pubOdom->publish(odom); 156 | 157 | lidar_gnss_mapping::odom_and_status odom_status; 158 | odom_status.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 159 | odom_status.header.frame_id = "/camera_init"; 160 | odom_status.odom.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 161 | odom_status.odom.header.frame_id = "/camera_init"; 162 | odom_status.odom.pose.pose.position.x = 0.0; 163 | odom_status.odom.pose.pose.position.y = 0.0; 164 | odom_status.odom.pose.pose.position.z = 0.0; 165 | odom_status.odom.pose.pose.orientation = orien; 166 | odom_status.status = 0; 167 | odom_status.sat_num = satNum; 168 | pubOdomAndStatus->publish(odom_status); 169 | } else { 170 | //use the pose obtained from the latest GNSS and IMU to initialize 171 | std::cout<<"GNSS init "<publish(odom); 239 | 240 | static lidar_gnss_mapping::odom_and_status odom_status; 241 | odom_status.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 242 | odom_status.header.frame_id = "/camera_init"; 243 | odom_status.odom.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 244 | odom_status.odom.header.frame_id = "/camera_init"; 245 | odom_status.odom.pose.pose.position = gpsAftShift[gpsAftNew]; 246 | odom_status.odom.pose.pose.orientation = orien; 247 | odom_status.status = status; 248 | odom_status.sat_num = satNum; 249 | pubOdomAndStatus->publish(odom_status); 250 | if (haveNoGpsData) 251 | haveNoGpsData = false; 252 | }else{ 253 | continue; 254 | } 255 | }else{ 256 | static nav_msgs::Odometry odom; 257 | odom.header.stamp = ros::Time().fromSec(gpsAftTime[gpsAftNew]); 258 | odom.header.frame_id = "/camera_init"; 259 | odom.pose.pose.position = gpsAftShift[gpsAftNew]; 260 | odom.pose.pose.orientation = orien; 261 | pubOdom->publish(odom); 262 | 263 | static lidar_gnss_mapping::odom_and_status odom_status; 264 | odom_status.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 265 | odom_status.header.frame_id = "/camera_init"; 266 | odom_status.odom.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 267 | odom_status.odom.header.frame_id = "/camera_init"; 268 | odom_status.odom.pose.pose.position = gpsAftShift[gpsAftNew]; 269 | odom_status.odom.pose.pose.orientation = orien; 270 | odom_status.status = status; 271 | odom_status.sat_num = satNum; 272 | pubOdomAndStatus->publish(odom_status); 273 | } 274 | } 275 | //Interpolate GNSS data using imu data 276 | if (!newGps && newImu) { 277 | if (haveNoGpsData) 278 | continue; 279 | newImu = false; 280 | double roll = imuRPY[imuNew].x; 281 | double pitch = imuRPY[imuNew].y; 282 | double yaw = imuRPY[imuNew].z; 283 | double accX = imuAcc[imuNew].x; 284 | double accY = imuAcc[imuNew].y; 285 | double accZ = imuAcc[imuNew].z; 286 | //Convert acceleration to world coordinate system(ENU) 287 | double x1 = accX; 288 | double y1 = cos(roll) * accY - sin(roll) * accZ; 289 | double z1 = sin(roll) * accY + cos(roll) * accZ; 290 | 291 | double x2 = cos(pitch) * x1 + sin(pitch) * z1; 292 | double y2 = y1; 293 | double z2 = -sin(pitch) * x1 + cos(pitch) * z1; 294 | 295 | accX = cos(yaw) * x2 - sin(yaw) * y2; 296 | accY = sin(yaw) * x2 + cos(yaw) * y2; 297 | accZ = z2; 298 | 299 | int imuBack = (imuNew - 1 + imuQueLen) % imuQueLen; 300 | double timeDiff = imuTime[imuNew] - imuTime[imuBack]; 301 | if (timeDiff < 0.02) { // nearly==0.01sec 302 | 303 | imuShift[imuNew].x = imuShift[imuBack].x + 304 | imuVelo[imuBack].x * timeDiff + 305 | accX * timeDiff * timeDiff / 2; 306 | imuShift[imuNew].y = imuShift[imuBack].y + 307 | imuVelo[imuBack].y * timeDiff + 308 | accY * timeDiff * timeDiff / 2; 309 | imuShift[imuNew].z = imuShift[imuBack].z + 310 | imuVelo[imuBack].z * timeDiff + 311 | accZ * timeDiff * timeDiff / 2; 312 | imuVelo[imuNew].x = imuVelo[imuBack].x + accX * timeDiff; 313 | imuVelo[imuNew].y = imuVelo[imuBack].y + accY * timeDiff; 314 | imuVelo[imuNew].z = imuVelo[imuBack].z + accZ * timeDiff; 315 | } 316 | 317 | static nav_msgs::Odometry odom; 318 | odom.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 319 | odom.header.frame_id = "/camera_init"; 320 | odom.pose.pose.position = imuShift[imuNew]; 321 | odom.pose.pose.orientation = orien; 322 | pubOdom->publish(odom); 323 | 324 | static lidar_gnss_mapping::odom_and_status odom_status; 325 | odom_status.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 326 | odom_status.header.frame_id = "/camera_init"; 327 | odom_status.odom.header.stamp = ros::Time().fromSec(imuTime[imuNew]); 328 | odom_status.odom.header.frame_id = "/camera_init"; 329 | odom_status.odom.pose.pose.position = imuShift[imuNew]; 330 | odom_status.odom.pose.pose.orientation = orien; 331 | odom_status.status = status; 332 | odom_status.sat_num = satNum; 333 | pubOdomAndStatus->publish(odom_status); 334 | } 335 | rate.sleep(); 336 | } 337 | delete pubOdom; 338 | delete pubOdomAndStatus; 339 | return 0; 340 | } -------------------------------------------------------------------------------- /lidar_gnss_mapping/src/register_lidar_gps_new.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | #include "lidar_gnss_mapping/odom_and_ctrl.h" 19 | #include "lidar_gnss_mapping/odom_and_status.h" 20 | #include "math_process.hpp" 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include "loopOptimize.h" 28 | 29 | using namespace gtsam; 30 | /* 31 | laser_gps.intensity = 0 GNSS data available 32 | laser_gps.intensity = 1 GNSS-Constrained LiDAR-Mapping switch to LiDAR-only (GNSS data) 33 | laser_gps.intensity = 2 LiDAR only modle 34 | laser_gps.intensity = 3 GNSS-Constrained LiDAR-Mapping switch to LiDAR-only (LiDAR-only data) 35 | */ 36 | 37 | pcl::PointCloud::Ptr odomKeyPoses6D; //(x y z pitch yaw roll) 38 | pcl::PointCloud::Ptr odomBeforOptimizePoses6D; 39 | pcl::PointCloud::Ptr odomAfterOptimizePoses6D; 40 | pcl::PointCloud::Ptr cloudKeyPoses3D; 41 | pcl::PointCloud::Ptr optimizePose3D; 42 | pcl::PointCloud::Ptr loamPose3D; 43 | PointTypePose laser_gps_test; 44 | 45 | std::deque::Ptr>recentLidarCloud; 46 | std::dequetimeLaser; 47 | std::dequerecentOdom; 48 | std::dequetimeOdometry; 49 | pcl::PointCloud::Ptr laserCloudFullRes; 50 | pcl::PointCloud::Ptr globalMapKeyFrames; 51 | pcl::PointCloud::Ptr pubGlobalMap; 52 | 53 | ros::Publisher pubLaserCloudSurround_rtk; 54 | ros::Publisher pubLoamDeltaTheta; 55 | 56 | 57 | bool newdata(false); 58 | bool begindata(false); 59 | int reset_flag_from_fusion = 0; 60 | int begin_current_id = 0; //the first index when switching from GNSS modle to lidar only 61 | int end_loam_index = 0; //the last index in lidar only modle 62 | double timeLaserOdometry = 0; 63 | double timeLaserCloudFullRes = 0; 64 | float position_s = 0.9; 65 | float angle_s = 0.0; 66 | 67 | vectorupdate_theta; 68 | vectorbegin_time; 69 | vectorend_time; 70 | double final_theta=0.0; 71 | double latest_theta = 0.0; 72 | double test_angle = 0; 73 | 74 | int charges = 0; 75 | int count_index = 0; 76 | PointType end_curretn; 77 | 78 | loamGtsam LG; 79 | double delta_theta = 0; 80 | int begin_index=3,end_index = 0;//坐标系对齐第一个index 81 | 82 | bool write_pointcloud_to_file(string str,pcl::PointCloud::Ptr cloudIn){ 83 | std::ofstream of(str,std::ios::app); 84 | if(!of.is_open()){ 85 | std::cout<<"open file "<points.size();++i){ 89 | PointType *pointTmp; 90 | pointTmp = &cloudIn->points[i]; 91 | of<x<<" "<y<<" "<z<<" "<intensity<::Ptr transformPointCloud(pcl::PointCloud::Ptr cloudIn, PointTypePose* transformIn){ 97 | 98 | pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud()); 99 | 100 | PointType *pointFrom; 101 | PointType pointTo; 102 | 103 | int cloudSize = cloudIn->points.size(); 104 | cloudOut->resize(cloudSize); 105 | for (int i = 0; i < cloudSize; ++i){ 106 | 107 | pointFrom = &cloudIn->points[i]; 108 | float x1 = cos(transformIn->yaw) * pointFrom->x - sin(transformIn->yaw) * pointFrom->y; 109 | float y1 = sin(transformIn->yaw) * pointFrom->x + cos(transformIn->yaw)* pointFrom->y; 110 | float z1 = pointFrom->z; 111 | 112 | float x2 = x1; 113 | float y2 = cos(transformIn->roll) * y1 - sin(transformIn->roll) * z1; 114 | float z2 = sin(transformIn->roll) * y1 + cos(transformIn->roll)* z1; 115 | 116 | pointTo.x = cos(transformIn->pitch) * x2 + sin(transformIn->pitch) * z2 + transformIn->x; 117 | pointTo.y = y2 + transformIn->y; 118 | pointTo.z = -sin(transformIn->pitch) * x2 + cos(transformIn->pitch) * z2 + transformIn->z; 119 | pointTo.intensity = pointFrom->intensity; 120 | 121 | cloudOut->points[i] = pointTo; 122 | } 123 | 124 | return cloudOut; 125 | } 126 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2){ 127 | timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); 128 | laserCloudFullRes.reset(new pcl::PointCloud()); 129 | pcl::PointCloud::Ptr laserCloudTmp; 130 | laserCloudTmp.reset(new pcl::PointCloud()); 131 | 132 | pcl::fromROSMsg(*laserCloudFullRes2,*laserCloudFullRes); 133 | for(auto i = 0;ipoints.size();i++){ 134 | PointType tmp= laserCloudFullRes->points[i]; 135 | if(sqrt(tmp.x*tmp.x + tmp.y*tmp.y + tmp.z*tmp.z)<2.5){ 136 | continue; 137 | }else{ 138 | laserCloudTmp->push_back(tmp); 139 | } 140 | } 141 | std::vector indices; 142 | pcl::removeNaNFromPointCloud(*laserCloudTmp,*laserCloudTmp,indices); 143 | recentLidarCloud.push_back(laserCloudTmp); 144 | timeLaser.push_back(timeLaserCloudFullRes); //timestamp 145 | } 146 | 147 | void transe(float q0, float q1, float q2, float q3,Eigen::Matrix3d &m){ 148 | 149 | m(0,0) = 1-2*q2*q2 - 2*q3*q3; 150 | m(0,1) = 2*q1*q2 + 2*q0*q3; 151 | m(0,2) = 2*q1*q3 - 2*q0*q2; 152 | // 153 | m(1,0) = 2*q1*q2 - 2*q0*q3; 154 | m(1,1) = 1-2*q1*q1 - 2*q3*q3; 155 | m(1,2) = 2*q2*q3 + 2*q0*q1; 156 | // 157 | m(2,0) = 2*q1*q3 + 2*q0*q2; 158 | m(2,1) = 2*q2*q3 - 2*q0*q1; 159 | m(2,2) = 1 - 2*q1*q1 - 2*q2*q2; 160 | } 161 | void registerLidarGpsCallback( 162 | const lidar_gnss_mapping::odom_and_ctrl::ConstPtr &laserOdometry){ 163 | newdata = true; 164 | timeLaserOdometry = laserOdometry->odom.header.stamp.toSec(); 165 | timeOdometry.push_back(timeLaserOdometry); 166 | reset_flag_from_fusion = laserOdometry->ctrl_flag; 167 | laser_gps_test.x = laserOdometry->odom.pose.pose.position.x; 168 | laser_gps_test.y = laserOdometry->odom.pose.pose.position.y; 169 | laser_gps_test.z = laserOdometry->odom.pose.pose.position.z; 170 | laser_gps_test.time = laserOdometry->odom.header.stamp.toSec(); 171 | double roll, pitch, yaw; 172 | geometry_msgs::Quaternion geoQuat = laserOdometry->odom.pose.pose.orientation; 173 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)) 174 | .getRPY(roll, pitch, yaw); 175 | laser_gps_test.roll = -pitch; 176 | laser_gps_test.pitch = -yaw; 177 | laser_gps_test.yaw = roll; 178 | laser_gps_test.intensity = laserOdometry->ctrl_flag; 179 | laser_gps_test.index = odomKeyPoses6D->points.size(); 180 | odomKeyPoses6D->push_back(laser_gps_test); 181 | 182 | PointType thisPose3D; 183 | thisPose3D.x = laser_gps_test.x; 184 | thisPose3D.y = laser_gps_test.y; 185 | thisPose3D.z = laser_gps_test.z; 186 | thisPose3D.intensity = laser_gps_test.index; 187 | cloudKeyPoses3D->push_back(thisPose3D); 188 | PointType KeyPoint; 189 | 190 | int laser_index = 0; 191 | int odom_index = 0; 192 | double theta = 0; 193 | 194 | if(laserOdometry->update == true){ 195 | final_theta = 0; 196 | 197 | update_theta.push_back(laserOdometry->theta); 198 | begin_time.push_back(cloudKeyPoses3D->points.size()); 199 | 200 | } 201 | else if(final_theta == 0){ 202 | final_theta = laserOdometry->theta; 203 | begin_time.push_back(cloudKeyPoses3D->points.size()); 204 | 205 | end_index = begin_time[begin_time.size()-1]; 206 | int theta_index = 0; 207 | pubGlobalMap.reset(new pcl::PointCloud()); 208 | 209 | for(auto i = end_index-update_theta.size();ipoints[i].intensity !=1){ 211 | odomKeyPoses6D->points[i].pitch -= (final_theta - update_theta[theta_index]); 212 | theta_index++; 213 | for(auto k=0;k()); 216 | *globalMapKeyFrames = *transformPointCloud(recentLidarCloud[k],&odomKeyPoses6D->points[i]); 217 | *pubGlobalMap += *globalMapKeyFrames; 218 | break; 219 | } 220 | } 221 | } 222 | } 223 | sensor_msgs::PointCloud2 cloudMsgTemp; 224 | globalMapKeyFrames.reset(new pcl::PointCloud()); 225 | pcl::toROSMsg(*pubGlobalMap, cloudMsgTemp); 226 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaser[timeLaser.size()-1]); 227 | cloudMsgTemp.header.frame_id = "/camera_init"; 228 | pubLaserCloudSurround_rtk.publish(cloudMsgTemp); 229 | begin_time.clear(); 230 | update_theta.clear(); 231 | count_index = end_index; 232 | 233 | } 234 | if(odomKeyPoses6D->points[laser_gps_test.index].intensity == 0 && final_theta!=0 && odomKeyPoses6D->points.size()>end_index+1){ 235 | for(auto i=0;i::Ptr pointCloudTmp = recentLidarCloud[i]; 240 | globalMapKeyFrames.reset(new pcl::PointCloud()); 241 | 242 | *globalMapKeyFrames = *transformPointCloud(pointCloudTmp,&odomKeyPoses6D->points[odomKeyPoses6D->points.size()-1]); 243 | 244 | sensor_msgs::PointCloud2 cloudMsgTemp; 245 | pcl::toROSMsg(*globalMapKeyFrames,cloudMsgTemp); 246 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaser[i]); 247 | cloudMsgTemp.header.frame_id = "/camera_init"; 248 | pubLaserCloudSurround_rtk.publish(cloudMsgTemp); 249 | break; 250 | } 251 | } 252 | globalMapKeyFrames.reset(new pcl::PointCloud()); 253 | } 254 | 255 | laser_index = 0; 256 | //////////////////////////////////////////////////////////////////////////////////////////////////////////// 257 | if(laser_gps_test.index > 0 && odomKeyPoses6D->points[laser_gps_test.index].intensity == 2 258 | && begindata == false){ 259 | begin_current_id = laser_gps_test.index; 260 | begindata = true; 261 | } 262 | if(laser_gps_test.index > 0 && (laser_gps_test.intensity == 2 || 263 | laser_gps_test.intensity == 3)){ 264 | odomBeforOptimizePoses6D->push_back(laser_gps_test); 265 | } 266 | if(laser_gps_test.index >0 && laser_gps_test.intensity == 1){ 267 | int end_index = odomBeforOptimizePoses6D->points.size(); 268 | for(int i = 0;ipoints.size();++i){ 269 | odomAfterOptimizePoses6D->push_back(odomBeforOptimizePoses6D->points[i]); 270 | } 271 | 272 | double delta_x_sum = (laser_gps_test.x - odomAfterOptimizePoses6D->points[end_index-1].x) * position_s; 273 | double delta_y_sum = (laser_gps_test.y - odomAfterOptimizePoses6D->points[end_index-1].y) * position_s; 274 | double delta_z_sum = (laser_gps_test.z - odomAfterOptimizePoses6D->points[end_index-1].z) * position_s; 275 | 276 | double rang_distance_all = 0.0; 277 | for(int i = 1;ipoints.size(); ++i){ 278 | rang_distance_all += sqrt((odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[i-1].x)* 279 | (odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[i-1].x) + 280 | (odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[i-1].y)* 281 | (odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[i-1].y) + 282 | (odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[i-1].z)* 283 | (odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[i-1].z)); 284 | } 285 | double current_l = 0.0; 286 | std::vector v_z; 287 | std::vector v_x; 288 | std::vector v_y; 289 | for(int i = 1;ipoints.size(); ++i){ 290 | current_l += sqrt((odomBeforOptimizePoses6D->points[i].x - odomBeforOptimizePoses6D->points[i-1].x)* 291 | (odomBeforOptimizePoses6D->points[i].x - odomBeforOptimizePoses6D->points[i-1].x) + 292 | (odomBeforOptimizePoses6D->points[i].y - odomBeforOptimizePoses6D->points[i-1].y)* 293 | (odomBeforOptimizePoses6D->points[i].y - odomBeforOptimizePoses6D->points[i-1].y) + 294 | (odomBeforOptimizePoses6D->points[i].z - odomBeforOptimizePoses6D->points[i-1].z)* 295 | (odomBeforOptimizePoses6D->points[i].z - odomBeforOptimizePoses6D->points[i-1].z)); 296 | odomAfterOptimizePoses6D->points[i].x += delta_x_sum*(current_l/rang_distance_all); 297 | odomAfterOptimizePoses6D->points[i].y += delta_y_sum*(current_l/rang_distance_all); 298 | odomAfterOptimizePoses6D->points[i].z += delta_z_sum*(current_l/rang_distance_all); 299 | } 300 | pcl::PointCloud::Ptr origin_pose(new pcl::PointCloud()); 301 | pcl::PointCloud::Ptr recent_pose(new pcl::PointCloud()); 302 | 303 | origin_pose.reset(new pcl::PointCloud()); 304 | recent_pose.reset(new pcl::PointCloud()); 305 | double tmp_origin_x = odomBeforOptimizePoses6D->points[0].x; 306 | double tmp_origin_y = odomBeforOptimizePoses6D->points[0].y; 307 | double tmp_origin_z = odomBeforOptimizePoses6D->points[0].z; 308 | double delta_pitch = 0; 309 | double dist = 0; 310 | std::vectorv_dist; 311 | Eigen::Affine3f correctionCameraFrame; 312 | for(int i = 0;ipoints.size();++i){ 313 | pcl::PointXYZI tmp_origin,tmp_recent; 314 | tmp_origin.x = odomBeforOptimizePoses6D->points[i].x - tmp_origin_x; 315 | tmp_origin.y = 0; 316 | tmp_origin.z = odomBeforOptimizePoses6D->points[i].z - tmp_origin_z; 317 | 318 | tmp_recent.x = odomAfterOptimizePoses6D->points[i].x - tmp_origin_x; 319 | tmp_recent.y = 0; 320 | tmp_recent.z = odomAfterOptimizePoses6D->points[i].z - tmp_origin_z; 321 | origin_pose->push_back(tmp_origin); 322 | recent_pose->push_back(tmp_recent); 323 | dist = sqrt((tmp_origin.x-tmp_recent.x)*(tmp_origin.x-tmp_recent.x)+ 324 | (tmp_origin.y-tmp_recent.y)*(tmp_origin.y-tmp_recent.y)+ 325 | (tmp_origin.z-tmp_recent.z)*(tmp_origin.z-tmp_recent.z)); 326 | v_dist.push_back(dist); 327 | } 328 | //icp 329 | pcl::IterativeClosestPoint icp; 330 | icp.setMaxCorrespondenceDistance(dist*0.8); 331 | icp.setMaximumIterations(500); 332 | icp.setTransformationEpsilon(1e-10); 333 | icp.setEuclideanFitnessEpsilon(0.01); 334 | icp.setRANSACIterations(0); 335 | icp.setInputSource(origin_pose); 336 | icp.setInputTarget(recent_pose); 337 | pcl::PointCloud::Ptr unused_result_icp(new pcl::PointCloud()); 338 | icp.align(*unused_result_icp);//ICP 339 | 340 | float x,y,z,roll,pitch,yaw; 341 | if(icp.hasConverged() == true){ 342 | correctionCameraFrame = icp.getFinalTransformation(); 343 | pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw); 344 | test_angle = pitch; 345 | }else{ 346 | std::cout<<"ICP ERROR "<points.size() ; 349 | 350 | if(yaw != 0 || pitch != 0 || roll != 0){ 351 | double xx = laser_gps_test.x - odomAfterOptimizePoses6D->points[end_index-1].x; 352 | double yy = laser_gps_test.y - odomAfterOptimizePoses6D->points[end_index-1].y; 353 | double zz = laser_gps_test.z - odomAfterOptimizePoses6D->points[end_index-1].z; 354 | 355 | double distance_all = 0.0; 356 | for(int i = 1;ipoints.size(); ++i){ 357 | distance_all += sqrt((odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[i-1].x)* 358 | (odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[i-1].x) + 359 | (odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[i-1].y)* 360 | (odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[i-1].y) + 361 | (odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[i-1].z)* 362 | (odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[i-1].z)); 363 | } 364 | double test_detal_distance; 365 | double detal_distance_all = 0.0; 366 | for(int i = 1;ipoints.size(); ++i){ 367 | test_detal_distance = sqrt((odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[i-1].x)* 368 | (odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[i-1].x) + 369 | (odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[i-1].y)* 370 | (odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[i-1].y) + 371 | (odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[i-1].z)* 372 | (odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[i-1].z)); 373 | 374 | detal_distance_all += test_detal_distance; 375 | double yaw_tmp = yaw ; 376 | double pitch_tmp = pitch ; 377 | double roll_tmp = roll ; 378 | float postw,posti,postj,postk; 379 | postw = cos(roll_tmp/2.0)*cos(pitch_tmp/2.0)*cos(yaw_tmp/2.0) + sin(roll_tmp/2.0)*sin(pitch_tmp/2.0)*sin(yaw_tmp/2.0); 380 | posti = sin(roll_tmp/2.0)*cos(pitch_tmp/2.0)*cos(yaw_tmp/2.0) - cos(roll_tmp/2.0)*sin(pitch_tmp/2.0)*sin(yaw_tmp/2.0); 381 | postj = cos(roll_tmp/2.0)*sin(pitch_tmp/2.0)*cos(yaw_tmp/2.0) + sin(roll_tmp/2.0)*cos(pitch_tmp/2.0)*sin(yaw_tmp/2.0); 382 | postk = cos(roll_tmp/2.0)*cos(pitch_tmp/2.0)*sin(yaw_tmp/2.0) - sin(roll_tmp/2.0)*sin(pitch_tmp/2.0)*cos(yaw_tmp/2.0); 383 | Eigen::Matrix3d Matrix_err(3,3); 384 | Eigen::Matrix3d error_inv(3,3); 385 | 386 | transe(postw,posti,postj,postk,Matrix_err); 387 | error_inv = Matrix_err.inverse(); 388 | Eigen::Matrix3d Matrix_err_xyz(3,3); 389 | Matrix_err_xyz(0,0)=odomAfterOptimizePoses6D->points[i].x - odomAfterOptimizePoses6D->points[0].x; 390 | Matrix_err_xyz(0,1)=odomAfterOptimizePoses6D->points[i].y - odomAfterOptimizePoses6D->points[0].y; 391 | Matrix_err_xyz(0,2)=odomAfterOptimizePoses6D->points[i].z - odomAfterOptimizePoses6D->points[0].z; 392 | 393 | Eigen::Matrix3d Matrix_xyz(3,3); 394 | 395 | Matrix_xyz = error_inv * Matrix_err_xyz; 396 | odomAfterOptimizePoses6D->points[i].yaw += yaw_tmp; 397 | odomAfterOptimizePoses6D->points[i].pitch += pitch_tmp; 398 | odomAfterOptimizePoses6D->points[i].roll += roll_tmp; 399 | 400 | } 401 | } 402 | for(int i=0;ipoints.size(); ++i){ 403 | LG.odometry.x = odomAfterOptimizePoses6D->points[i].x; 404 | LG.odometry.y = odomAfterOptimizePoses6D->points[i].y; 405 | LG.odometry.z = odomAfterOptimizePoses6D->points[i].z; 406 | LG.odometry.time = odomAfterOptimizePoses6D->points[i].time; 407 | LG.odometry.roll = odomAfterOptimizePoses6D->points[i].roll; 408 | LG.odometry.pitch = odomAfterOptimizePoses6D->points[i].pitch; 409 | LG.odometry.yaw = odomAfterOptimizePoses6D->points[i].yaw; 410 | LG.odometry.intensity = odomAfterOptimizePoses6D->points[i].intensity; 411 | LG.recv_data(); 412 | LG.path_optimize(); 413 | } 414 | LG.odometry.x = laser_gps_test.x; 415 | LG.odometry.y = laser_gps_test.y; 416 | LG.odometry.z = laser_gps_test.z; 417 | LG.odometry.time = laser_gps_test.time; 418 | 419 | LG.odometry.roll = odomAfterOptimizePoses6D->points[odomAfterOptimizePoses6D->points.size()-1].roll; 420 | LG.odometry.pitch = odomAfterOptimizePoses6D->points[odomAfterOptimizePoses6D->points.size()-1].pitch; 421 | LG.odometry.yaw = odomAfterOptimizePoses6D->points[odomAfterOptimizePoses6D->points.size()-1].yaw; 422 | LG.odometry.intensity = laser_gps_test.intensity; 423 | LG.recv_data(); 424 | LG.path_optimize(); 425 | int tmp_time_index = 0; 426 | if(LG.save_result()){ 427 | int j=0; 428 | for(int i=0;ipoints.size();++i){ 429 | j = i+begin_current_id; 430 | PointType thisPose; 431 | thisPose.x = LG.currentOdomKeyPoses6D->points[i].x; 432 | thisPose.y = LG.currentOdomKeyPoses6D->points[i].y; 433 | thisPose.z = LG.currentOdomKeyPoses6D->points[i].z; 434 | 435 | optimizePose3D->push_back(thisPose); 436 | thisPose.x = cloudKeyPoses3D->points[j].x; 437 | thisPose.y = cloudKeyPoses3D->points[j].y; 438 | thisPose.z = cloudKeyPoses3D->points[j].z; 439 | loamPose3D->push_back(thisPose); 440 | cloudKeyPoses3D->points[j].x = LG.currentOdomKeyPoses6D->points[i].x; 441 | cloudKeyPoses3D->points[j].y = LG.currentOdomKeyPoses6D->points[i].y; 442 | cloudKeyPoses3D->points[j].z = LG.currentOdomKeyPoses6D->points[i].z; 443 | odomKeyPoses6D->points[j].x = LG.currentOdomKeyPoses6D->points[i].x; 444 | odomKeyPoses6D->points[j].y = LG.currentOdomKeyPoses6D->points[i].y; 445 | odomKeyPoses6D->points[j].z = LG.currentOdomKeyPoses6D->points[i].z; 446 | odomKeyPoses6D->points[j].roll = LG.currentOdomKeyPoses6D->points[i].roll; 447 | odomKeyPoses6D->points[j].pitch = LG.currentOdomKeyPoses6D->points[i].pitch; 448 | odomKeyPoses6D->points[j].yaw = LG.currentOdomKeyPoses6D->points[i].yaw; 449 | if(odomKeyPoses6D->points[j].intensity != 1){ 450 | for(auto k=0;k()); 453 | *globalMapKeyFrames = *transformPointCloud(recentLidarCloud[k],&odomKeyPoses6D->points[j]); 454 | *pubGlobalMap += *globalMapKeyFrames; 455 | break; 456 | } 457 | } 458 | } 459 | } 460 | sensor_msgs::PointCloud2 cloudMsgTemp; 461 | pcl::toROSMsg(*pubGlobalMap, cloudMsgTemp); 462 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaser[timeLaser.size()-1]); 463 | cloudMsgTemp.header.frame_id = "/camera_init"; 464 | pubLaserCloudSurround_rtk.publish(cloudMsgTemp); 465 | globalMapKeyFrames.reset(new pcl::PointCloud()); 466 | begin_index = begin_current_id+LG.currentOdomKeyPoses6D->points.size(); 467 | nav_msgs::Odometry odom; 468 | odom.header.frame_id = "/camera_init"; 469 | odom.pose.pose.orientation.x = odomKeyPoses6D->points[LG.currentOdomKeyPoses6D->points.size()-1+begin_current_id].roll - roll; 470 | odom.pose.pose.orientation.y = odomKeyPoses6D->points[LG.currentOdomKeyPoses6D->points.size()-1+begin_current_id].pitch - pitch; 471 | odom.pose.pose.orientation.z = odomKeyPoses6D->points[LG.currentOdomKeyPoses6D->points.size()-1+begin_current_id].yaw - yaw; 472 | odom.pose.pose.position.x = odomKeyPoses6D->points[LG.currentOdomKeyPoses6D->points.size()-1+begin_current_id].x; 473 | odom.pose.pose.position.y = odomKeyPoses6D->points[LG.currentOdomKeyPoses6D->points.size()-1+begin_current_id].y; 474 | odom.pose.pose.position.z = odomKeyPoses6D->points[LG.currentOdomKeyPoses6D->points.size()-1+begin_current_id].z; 475 | 476 | pubLoamDeltaTheta.publish(odom); 477 | begindata = false; 478 | odomBeforOptimizePoses6D.reset(new pcl::PointCloud()); 479 | odomAfterOptimizePoses6D.reset(new pcl::PointCloud()); 480 | for(int k = 0;k()); 494 | odomBeforOptimizePoses6D.reset(new pcl::PointCloud()); 495 | odomAfterOptimizePoses6D.reset(new pcl::PointCloud()); 496 | cloudKeyPoses3D.reset(new pcl::PointCloud()); 497 | optimizePose3D.reset(new pcl::PointCloud()); 498 | loamPose3D.reset(new pcl::PointCloud()); 499 | globalMapKeyFrames.reset(new pcl::PointCloud()); 500 | 501 | ros::Subscriber subLaserCloudFullRes = 502 | nh.subscribe("/velodyne_cloud_3", 2, laserCloudFullResHandler); 503 | ros::Subscriber subRegisterLidarGps = 504 | nh.subscribe("/laser_and_odom_init",20,registerLidarGpsCallback); 505 | 506 | ros::Publisher pubKeyPoses = nh.advertise("/key_pose_origin", 2); 507 | ros::Publisher pubOptimizePoses = nh.advertise("/optimize_pose",2); 508 | ros::Publisher pubLoamPoses = nh.advertise("/loam_pose",2); 509 | pubLaserCloudSurround_rtk = nh.advertise("/laser_cloud_surround_rtk", 2); 510 | pubLoamDeltaTheta = nh.advertise("/delta_theta",2); 511 | 512 | ros::Rate rate(10); 513 | double odom_num = 0; 514 | bool first_write_keyPose(true); 515 | while(ros::ok()){ 516 | if(cloudKeyPoses3D->points.size()!=0){ 517 | sensor_msgs::PointCloud2 cloudMsgTemp; 518 | pcl::toROSMsg(*cloudKeyPoses3D, cloudMsgTemp); 519 | cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 520 | cloudMsgTemp.header.frame_id = "/camera_init"; 521 | pubKeyPoses.publish(cloudMsgTemp); 522 | 523 | } 524 | if(optimizePose3D->points.size()!=0){ 525 | sensor_msgs::PointCloud2 cloudOptimizeTemp; 526 | pcl::toROSMsg(*optimizePose3D, cloudOptimizeTemp); 527 | cloudOptimizeTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 528 | cloudOptimizeTemp.header.frame_id = "/camera_init"; 529 | pubOptimizePoses.publish(cloudOptimizeTemp); 530 | } 531 | if(loamPose3D->points.size()!=0){ 532 | sensor_msgs::PointCloud2 cloudloamTemp; 533 | pcl::toROSMsg(*loamPose3D, cloudloamTemp); 534 | cloudloamTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry); 535 | cloudloamTemp.header.frame_id = "/camera_init"; 536 | pubLoamPoses.publish(cloudloamTemp); 537 | } 538 | ros::spinOnce(); 539 | rate.sleep(); 540 | } 541 | return 0 ; 542 | } -------------------------------------------------------------------------------- /lidar_gnss_mapping/src/rtk_registration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "iostream" 23 | using namespace std; 24 | const double PI = 3.1415926; 25 | 26 | const float scanPeriod = 0.1; 27 | 28 | const int systemDelay = 20; 29 | int systemInitCount = 0; 30 | bool systemInited = false; 31 | 32 | pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud()); 33 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 34 | pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); 35 | pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); 36 | pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); 37 | pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); 38 | pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud()); 39 | pcl::PointCloud::Ptr surfPointsLessFlatScanDS(new pcl::PointCloud()); 40 | pcl::PointCloud::Ptr imuTrans(new pcl::PointCloud(4, 1)); 41 | pcl::PointCloud::Ptr laserCloudScans[16]; 42 | 43 | float cloudCurvature[40000]; 44 | int cloudSortInd[40000]; 45 | int cloudNeighborPicked[40000]; 46 | int cloudLabel[40000]; 47 | 48 | int scanStartInd[16]; 49 | int scanEndInd[16]; 50 | 51 | int imuPointerFront = 0; 52 | int imuPointerLast = -1; 53 | const int imuQueLength = 200; 54 | 55 | float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; 56 | float imuRollCur = 0, imuPitchCur = 0, imuYawCur = 0; 57 | 58 | float imuVeloXStart = 0, imuVeloYStart = 0, imuVeloZStart = 0; 59 | float imuShiftXStart = 0, imuShiftYStart = 0, imuShiftZStart = 0; 60 | 61 | float imuVeloXCur = 0, imuVeloYCur = 0, imuVeloZCur = 0; 62 | float imuShiftXCur = 0, imuShiftYCur = 0, imuShiftZCur = 0; 63 | 64 | float imuShiftFromStartXCur = 0, imuShiftFromStartYCur = 0, imuShiftFromStartZCur = 0; 65 | float imuVeloFromStartXCur = 0, imuVeloFromStartYCur = 0, imuVeloFromStartZCur = 0; 66 | double imuTime[imuQueLength] = {0}; 67 | float imuRoll[imuQueLength] = {0}; 68 | float imuPitch[imuQueLength] = {0}; 69 | float imuYaw[imuQueLength] = {0}; 70 | 71 | float imuAccX[imuQueLength] = {0}; 72 | float imuAccY[imuQueLength] = {0}; 73 | float imuAccZ[imuQueLength] = {0}; 74 | 75 | float imuVeloX[imuQueLength] = {0}; 76 | float imuVeloY[imuQueLength] = {0}; 77 | float imuVeloZ[imuQueLength] = {0}; 78 | 79 | float imuShiftX[imuQueLength] = {0}; 80 | float imuShiftY[imuQueLength] = {0}; 81 | float imuShiftZ[imuQueLength] = {0}; 82 | 83 | ros::Publisher* pubLaserCloudPointer; 84 | ros::Publisher* pubCornerPointsSharpPointer; 85 | ros::Publisher* pubCornerPointsLessSharpPointer; 86 | ros::Publisher* pubSurfPointsFlatPointer; 87 | ros::Publisher* pubSurfPointsLessFlatPointer; 88 | ros::Publisher* pubImuTransPointer; 89 | 90 | void ShiftToStartIMU(float pointTime) 91 | { 92 | imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime; 93 | imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime; 94 | imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime; 95 | 96 | float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur; 97 | float y1 = imuShiftFromStartYCur; 98 | float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur; 99 | 100 | float x2 = x1; 101 | float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1; 102 | float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1; 103 | 104 | imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2; 105 | imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2; 106 | imuShiftFromStartZCur = z2; 107 | } 108 | 109 | void VeloToStartIMU() 110 | { 111 | imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart; 112 | imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart; 113 | imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart; 114 | 115 | float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur; 116 | float y1 = imuVeloFromStartYCur; 117 | float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur; 118 | 119 | float x2 = x1; 120 | float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1; 121 | float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1; 122 | 123 | imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2; 124 | imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2; 125 | imuVeloFromStartZCur = z2; 126 | } 127 | 128 | void TransformToStartIMU(pcl::PointXYZI *p) 129 | { 130 | float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y; 131 | float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y; 132 | float z1 = p->z; 133 | 134 | float x2 = x1; 135 | float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1; 136 | float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1; 137 | 138 | float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2; 139 | float y3 = y2; 140 | float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2; 141 | 142 | float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3; 143 | float y4 = y3; 144 | float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3; 145 | 146 | float x5 = x4; 147 | float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4; 148 | float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4; 149 | 150 | p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur; 151 | p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur; 152 | p->z = z5 + imuShiftFromStartZCur; 153 | } 154 | 155 | void AccumulateIMUShift() 156 | { 157 | float roll = imuRoll[imuPointerLast]; 158 | float pitch = imuPitch[imuPointerLast]; 159 | float yaw = imuYaw[imuPointerLast]; 160 | float accX = imuAccX[imuPointerLast]; 161 | float accY = imuAccY[imuPointerLast]; 162 | float accZ = imuAccZ[imuPointerLast]; 163 | 164 | float x1 = cos(roll) * accX - sin(roll) * accY; 165 | float y1 = sin(roll) * accX + cos(roll) * accY; 166 | float z1 = accZ; 167 | 168 | float x2 = x1; 169 | float y2 = cos(pitch) * y1 - sin(pitch) * z1; 170 | float z2 = sin(pitch) * y1 + cos(pitch) * z1; 171 | 172 | accX = cos(yaw) * x2 + sin(yaw) * z2; 173 | accY = y2; 174 | accZ = -sin(yaw) * x2 + cos(yaw) * z2; 175 | 176 | int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength; 177 | double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack]; 178 | if (timeDiff < 0.1) 179 | { 180 | 181 | imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff 182 | + accX * timeDiff * timeDiff / 2; 183 | imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff 184 | + accY * timeDiff * timeDiff / 2; 185 | imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff 186 | + accZ * timeDiff * timeDiff / 2; 187 | 188 | imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff; 189 | imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff; 190 | imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff; 191 | 192 | } 193 | } 194 | 195 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn2) 196 | { 197 | if (!systemInited) 198 | { 199 | systemInitCount++; 200 | if (systemInitCount >= systemDelay) 201 | { 202 | systemInited = true; 203 | } 204 | return; 205 | } 206 | 207 | double timeScanCur = laserCloudIn2->header.stamp.toSec(); 208 | 209 | pcl::fromROSMsg(*laserCloudIn2, *laserCloudIn); 210 | std::vector indices; 211 | pcl::removeNaNFromPointCloud(*laserCloudIn,*laserCloudIn,indices); 212 | indices.clear(); 213 | int cloudSize = laserCloudIn->points.size(); 214 | 215 | float startOri = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x); 216 | float endOri = -atan2(laserCloudIn->points[cloudSize - 1].y, 217 | laserCloudIn->points[cloudSize - 1].x) + 2 * PI; 218 | 219 | if (endOri - startOri > 3 * PI) 220 | { 221 | endOri -= 2 * PI; 222 | } 223 | else if (endOri - startOri < PI) 224 | { 225 | endOri += 2 * PI; 226 | } 227 | bool halfPassed = false; 228 | pcl::PointXYZI point; 229 | for (int i = 0; i < cloudSize; i++) 230 | { 231 | point.x = laserCloudIn->points[i].y; 232 | point.y = laserCloudIn->points[i].z; 233 | point.z = laserCloudIn->points[i].x; 234 | 235 | float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / PI; 236 | int scanID = int(0.5 * angle + 0.5) + 7; 237 | /*if (angle < 0) 238 | { 239 | scanID--; 240 | }*/ 241 | 242 | if (scanID>15||scanID<0) 243 | { 244 | 245 | continue; 246 | } 247 | 248 | float ori = -atan2(point.x, point.z); 249 | if (!halfPassed) 250 | { 251 | if (ori < startOri - PI / 2) { 252 | ori += 2 * PI; 253 | } 254 | else if (ori > startOri + PI * 3 / 2) 255 | { 256 | ori -= 2 * PI; 257 | } 258 | 259 | if (ori - startOri > PI) 260 | { 261 | halfPassed = true; 262 | } 263 | } 264 | else 265 | { 266 | ori += 2 * PI; 267 | 268 | if (ori < endOri - PI * 3 / 2) 269 | { 270 | ori += 2 * PI; 271 | } else if (ori > endOri + PI / 2) 272 | { 273 | ori -= 2 * PI; 274 | } 275 | } 276 | float relTime = (ori - startOri) / (endOri - startOri); 277 | point.intensity = scanID + 0.1 * relTime; 278 | 279 | if (imuPointerLast >= 0) 280 | { 281 | float pointTime = relTime * scanPeriod; 282 | while (imuPointerFront != imuPointerLast) 283 | { 284 | if (timeScanCur + pointTime < imuTime[imuPointerFront]) 285 | { 286 | break; 287 | } 288 | imuPointerFront = (imuPointerFront + 1) % imuQueLength; 289 | } 290 | 291 | if (timeScanCur + pointTime > imuTime[imuPointerFront]) 292 | { 293 | imuRollCur = imuRoll[imuPointerFront]; 294 | imuPitchCur = imuPitch[imuPointerFront]; 295 | imuYawCur = imuYaw[imuPointerFront]; 296 | 297 | imuVeloXCur = imuVeloX[imuPointerFront]; 298 | imuVeloYCur = imuVeloY[imuPointerFront]; 299 | imuVeloZCur = imuVeloZ[imuPointerFront]; 300 | 301 | imuShiftXCur = imuShiftX[imuPointerFront]; 302 | imuShiftYCur = imuShiftY[imuPointerFront]; 303 | imuShiftZCur = imuShiftZ[imuPointerFront]; 304 | } 305 | else 306 | { 307 | int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength; 308 | float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) 309 | / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 310 | float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) 311 | / (imuTime[imuPointerFront] - imuTime[imuPointerBack]); 312 | 313 | imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack; 314 | imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack; 315 | if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > PI) 316 | { 317 | imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * PI) * ratioBack; 318 | } 319 | else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -PI) 320 | { 321 | imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * PI) * ratioBack; 322 | } 323 | else 324 | { 325 | imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack; 326 | } 327 | 328 | imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack; 329 | imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack; 330 | imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack; 331 | 332 | imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack; 333 | imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack; 334 | imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack; 335 | } 336 | 337 | if (i == 0) 338 | { 339 | imuRollStart = imuRollCur; 340 | imuPitchStart = imuPitchCur; 341 | imuYawStart = imuYawCur; 342 | 343 | imuVeloXStart = imuVeloXCur; 344 | imuVeloYStart = imuVeloYCur; 345 | imuVeloZStart = imuVeloZCur; 346 | 347 | imuShiftXStart = imuShiftXCur; 348 | imuShiftYStart = imuShiftYCur; 349 | imuShiftZStart = imuShiftZCur; 350 | } 351 | else 352 | { 353 | ShiftToStartIMU(pointTime); 354 | VeloToStartIMU(); 355 | TransformToStartIMU(&point); 356 | } 357 | } 358 | 359 | laserCloudScans[scanID]->push_back(point); 360 | } 361 | 362 | for (int i = 0; i < 16; i++) 363 | { 364 | *laserCloud += *laserCloudScans[i]; 365 | } 366 | int scanCount = -1; 367 | for (int i = 5; i < cloudSize - 5; i++) { 368 | float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x 369 | + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x 370 | + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x 371 | + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x 372 | + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x 373 | + laserCloud->points[i + 5].x; 374 | float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y 375 | + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y 376 | + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y 377 | + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y 378 | + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y 379 | + laserCloud->points[i + 5].y; 380 | float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z 381 | + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z 382 | + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z 383 | + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z 384 | + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z 385 | + laserCloud->points[i + 5].z; 386 | 387 | cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; 388 | cloudSortInd[i] = i; 389 | cloudNeighborPicked[i] = 0;// 390 | cloudLabel[i] = 0; 391 | 392 | if (int(laserCloud->points[i].intensity) != scanCount) { 393 | scanCount = int(laserCloud->points[i].intensity); 394 | 395 | if (scanCount > 0) { 396 | scanStartInd[scanCount] = i + 5; 397 | scanEndInd[scanCount - 1] = i - 5; 398 | } 399 | } 400 | } 401 | 402 | 403 | scanStartInd[0] = 5; 404 | scanEndInd[15] = cloudSize - 5; 405 | 406 | for (int i = 5; i < cloudSize - 6; i++) 407 | { 408 | float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x; 409 | float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y; 410 | float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z; 411 | float diff = diffX * diffX + diffY * diffY + diffZ * diffZ; 412 | 413 | if (diff > 0.1) 414 | { 415 | 416 | float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 417 | laserCloud->points[i].y * laserCloud->points[i].y + 418 | laserCloud->points[i].z * laserCloud->points[i].z); 419 | 420 | float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 421 | laserCloud->points[i + 1].y * laserCloud->points[i + 1].y + 422 | laserCloud->points[i + 1].z * laserCloud->points[i + 1].z); 423 | 424 | if (depth1 > depth2) 425 | { 426 | diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1; 427 | diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1; 428 | diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1; 429 | 430 | if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) 431 | { 432 | cloudNeighborPicked[i - 5] = 1; 433 | cloudNeighborPicked[i - 4] = 1; 434 | cloudNeighborPicked[i - 3] = 1; 435 | cloudNeighborPicked[i - 2] = 1; 436 | cloudNeighborPicked[i - 1] = 1; 437 | cloudNeighborPicked[i] = 1; 438 | } 439 | } 440 | else 441 | { 442 | diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x; 443 | diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y; 444 | diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z; 445 | 446 | if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) 447 | { 448 | cloudNeighborPicked[i + 1] = 1; 449 | cloudNeighborPicked[i + 2] = 1; 450 | cloudNeighborPicked[i + 3] = 1; 451 | cloudNeighborPicked[i + 4] = 1; 452 | cloudNeighborPicked[i + 5] = 1; 453 | cloudNeighborPicked[i + 6] = 1; 454 | } 455 | } 456 | } 457 | 458 | float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x; 459 | float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y; 460 | float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z; 461 | float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2; 462 | 463 | float dis = laserCloud->points[i].x * laserCloud->points[i].x 464 | + laserCloud->points[i].y * laserCloud->points[i].y 465 | + laserCloud->points[i].z * laserCloud->points[i].z; 466 | 467 | if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) 468 | { 469 | cloudNeighborPicked[i] = 1; 470 | } 471 | } 472 | 473 | for (int i = 0; i < 16; i++) 474 | { 475 | surfPointsLessFlatScan->clear(); 476 | for (int j = 0; j < 6; j++) { 477 | int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6; 478 | int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1; 479 | for (int k = sp + 1; k <= ep; k++) 480 | { 481 | for (int l = k; l >= sp + 1; l--) 482 | { 483 | if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) 484 | { 485 | int temp = cloudSortInd[l - 1]; 486 | cloudSortInd[l - 1] = cloudSortInd[l]; 487 | cloudSortInd[l] = temp; 488 | } 489 | } 490 | } 491 | int largestPickedNum = 0; 492 | for (int k = ep; k >= sp; k--) 493 | { 494 | int ind = cloudSortInd[k]; 495 | if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1) 496 | { 497 | 498 | largestPickedNum++; 499 | if (largestPickedNum <= 2) 500 | { 501 | cloudLabel[ind] = 2; 502 | cornerPointsSharp->push_back(laserCloud->points[ind]); 503 | cornerPointsLessSharp->push_back(laserCloud->points[ind]); 504 | } 505 | else if (largestPickedNum <= 20) 506 | { 507 | cloudLabel[ind] = 1; 508 | cornerPointsLessSharp->push_back(laserCloud->points[ind]); 509 | } 510 | else 511 | { 512 | break; 513 | } 514 | 515 | cloudNeighborPicked[ind] = 1; 516 | for (int l = 1; l <= 5; l++) 517 | { 518 | float diffX = laserCloud->points[ind + l].x 519 | - laserCloud->points[ind + l - 1].x; 520 | float diffY = laserCloud->points[ind + l].y 521 | - laserCloud->points[ind + l - 1].y; 522 | float diffZ = laserCloud->points[ind + l].z 523 | - laserCloud->points[ind + l - 1].z; 524 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 525 | { 526 | break; 527 | } 528 | 529 | cloudNeighborPicked[ind + l] = 1; 530 | } 531 | for (int l = -1; l >= -5; l--) 532 | { 533 | float diffX = laserCloud->points[ind + l].x 534 | - laserCloud->points[ind + l + 1].x; 535 | float diffY = laserCloud->points[ind + l].y 536 | - laserCloud->points[ind + l + 1].y; 537 | float diffZ = laserCloud->points[ind + l].z 538 | - laserCloud->points[ind + l + 1].z; 539 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 540 | { 541 | break; 542 | } 543 | 544 | cloudNeighborPicked[ind + l] = 1; 545 | } 546 | } 547 | } 548 | int smallestPickedNum = 0; 549 | for (int k = sp; k <= ep; k++) 550 | { 551 | int ind = cloudSortInd[k]; 552 | if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1) 553 | { 554 | 555 | cloudLabel[ind] = -1; 556 | surfPointsFlat->push_back(laserCloud->points[ind]); 557 | 558 | smallestPickedNum++; 559 | if (smallestPickedNum >= 4) 560 | { 561 | break; 562 | } 563 | 564 | cloudNeighborPicked[ind] = 1; 565 | for (int l = 1; l <= 5; l++) 566 | { 567 | float diffX = laserCloud->points[ind + l].x 568 | - laserCloud->points[ind + l - 1].x; 569 | float diffY = laserCloud->points[ind + l].y 570 | - laserCloud->points[ind + l - 1].y; 571 | float diffZ = laserCloud->points[ind + l].z 572 | - laserCloud->points[ind + l - 1].z; 573 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 574 | { 575 | break; 576 | } 577 | 578 | cloudNeighborPicked[ind + l] = 1; 579 | } 580 | for (int l = -1; l >= -5; l--) 581 | { 582 | float diffX = laserCloud->points[ind + l].x 583 | - laserCloud->points[ind + l + 1].x; 584 | float diffY = laserCloud->points[ind + l].y 585 | - laserCloud->points[ind + l + 1].y; 586 | float diffZ = laserCloud->points[ind + l].z 587 | - laserCloud->points[ind + l + 1].z; 588 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) 589 | { 590 | break; 591 | } 592 | 593 | cloudNeighborPicked[ind + l] = 1; 594 | } 595 | } 596 | } 597 | 598 | for (int k = sp; k <= ep; k++) 599 | { 600 | if (cloudLabel[k] <= 0) 601 | { 602 | surfPointsLessFlatScan->push_back(laserCloud->points[k]); 603 | } 604 | } 605 | } 606 | 607 | surfPointsLessFlatScanDS->clear(); 608 | pcl::VoxelGrid downSizeFilter; 609 | downSizeFilter.setInputCloud(surfPointsLessFlatScan); 610 | downSizeFilter.setLeafSize(0.2, 0.2, 0.2); 611 | downSizeFilter.filter(*surfPointsLessFlatScanDS); 612 | 613 | *surfPointsLessFlat += *surfPointsLessFlatScanDS; 614 | } 615 | 616 | sensor_msgs::PointCloud2 laserCloud2; 617 | pcl::toROSMsg(*laserCloud, laserCloud2); 618 | laserCloud2.header.stamp = laserCloudIn2->header.stamp; 619 | laserCloud2.header.frame_id = "/camera"; 620 | pubLaserCloudPointer->publish(laserCloud2); 621 | 622 | sensor_msgs::PointCloud2 cornerPointsSharp2; 623 | pcl::toROSMsg(*cornerPointsSharp, cornerPointsSharp2); 624 | cornerPointsSharp2.header.stamp = laserCloudIn2->header.stamp; 625 | cornerPointsSharp2.header.frame_id = "/camera"; 626 | pubCornerPointsSharpPointer->publish(cornerPointsSharp2); 627 | 628 | sensor_msgs::PointCloud2 cornerPointsLessSharp2; 629 | pcl::toROSMsg(*cornerPointsLessSharp, cornerPointsLessSharp2); 630 | cornerPointsLessSharp2.header.stamp = laserCloudIn2->header.stamp; 631 | cornerPointsLessSharp2.header.frame_id = "/camera"; 632 | pubCornerPointsLessSharpPointer->publish(cornerPointsLessSharp2); 633 | 634 | sensor_msgs::PointCloud2 surfPointsFlat2; 635 | pcl::toROSMsg(*surfPointsFlat, surfPointsFlat2); 636 | surfPointsFlat2.header.stamp = laserCloudIn2->header.stamp; 637 | surfPointsFlat2.header.frame_id = "/camera"; 638 | pubSurfPointsFlatPointer->publish(surfPointsFlat2); 639 | 640 | sensor_msgs::PointCloud2 surfPointsLessFlat2; 641 | pcl::toROSMsg(*surfPointsLessFlat, surfPointsLessFlat2); 642 | surfPointsLessFlat2.header.stamp = laserCloudIn2->header.stamp; 643 | surfPointsLessFlat2.header.frame_id = "/camera"; 644 | pubSurfPointsLessFlatPointer->publish(surfPointsLessFlat2); 645 | 646 | imuTrans->points[0].x = imuPitchStart; 647 | imuTrans->points[0].y = imuYawStart; 648 | imuTrans->points[0].z = imuRollStart; 649 | 650 | imuTrans->points[1].x = imuPitchCur; 651 | imuTrans->points[1].y = imuYawCur; 652 | imuTrans->points[1].z = imuRollCur; 653 | 654 | imuTrans->points[2].x = imuShiftFromStartXCur; 655 | imuTrans->points[2].y = imuShiftFromStartYCur; 656 | imuTrans->points[2].z = imuShiftFromStartZCur; 657 | 658 | imuTrans->points[3].x = imuVeloFromStartXCur; 659 | imuTrans->points[3].y = imuVeloFromStartYCur; 660 | imuTrans->points[3].z = imuVeloFromStartZCur; 661 | 662 | sensor_msgs::PointCloud2 imuTrans2; 663 | pcl::toROSMsg(*imuTrans, imuTrans2); 664 | imuTrans2.header.stamp = laserCloudIn2->header.stamp; 665 | imuTrans2.header.frame_id = "/camera"; 666 | pubImuTransPointer->publish(imuTrans2); 667 | 668 | laserCloudIn->clear(); 669 | laserCloud->clear(); 670 | cornerPointsSharp->clear(); 671 | cornerPointsLessSharp->clear(); 672 | surfPointsFlat->clear(); 673 | surfPointsLessFlat->clear(); 674 | 675 | for (int i = 0; i < 16; i++) 676 | { 677 | laserCloudScans[i]->points.clear(); 678 | } 679 | } 680 | 681 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn) 682 | { 683 | double roll, pitch, yaw; 684 | tf::Quaternion orientation; 685 | tf::quaternionMsgToTF(imuIn->orientation, orientation); 686 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); 687 | 688 | float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81; 689 | float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81; 690 | float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81; 691 | 692 | imuPointerLast = (imuPointerLast + 1) % imuQueLength; 693 | 694 | imuTime[imuPointerLast] = imuIn->header.stamp.toSec(); 695 | imuRoll[imuPointerLast] = roll; 696 | imuPitch[imuPointerLast] = pitch; 697 | imuYaw[imuPointerLast] = yaw; 698 | imuAccX[imuPointerLast] = accX; 699 | imuAccY[imuPointerLast] = accY; 700 | imuAccZ[imuPointerLast] = accZ; 701 | AccumulateIMUShift(); 702 | } 703 | 704 | int main(int argc, char** argv) 705 | { 706 | ros::init(argc, argv, "scanRegistration"); 707 | ros::NodeHandle nh; 708 | 709 | for (int i = 0; i < 16; i++) { 710 | laserCloudScans[i].reset(new pcl::PointCloud()); 711 | } 712 | 713 | ros::Subscriber subLaserCloud = nh.subscribe 714 | ("/velodyne_points", 2, laserCloudHandler); 715 | 716 | /*ros::Subscriber subLaserCloud = nh.subscribe 717 | ("/rslidar_points", 10, laserCloudHandler); 718 | */ 719 | ros::Subscriber subImu = nh.subscribe ("/imu/data", 50, imuHandler); 720 | 721 | ros::Publisher pubLaserCloud = nh.advertise 722 | ("/velodyne_cloud_2", 2); 723 | 724 | ros::Publisher pubCornerPointsSharp = nh.advertise 725 | ("/laser_cloud_sharp", 2); 726 | 727 | ros::Publisher pubCornerPointsLessSharp = nh.advertise 728 | ("/laser_cloud_less_sharp", 2); 729 | 730 | ros::Publisher pubSurfPointsFlat = nh.advertise 731 | ("/laser_cloud_flat", 2); 732 | 733 | ros::Publisher pubSurfPointsLessFlat = nh.advertise 734 | ("/laser_cloud_less_flat", 2); 735 | 736 | ros::Publisher pubImuTrans = nh.advertise ("/imu_trans", 5); 737 | 738 | pubLaserCloudPointer = &pubLaserCloud; 739 | pubCornerPointsSharpPointer = &pubCornerPointsSharp; 740 | pubCornerPointsLessSharpPointer = &pubCornerPointsLessSharp; 741 | pubSurfPointsFlatPointer = &pubSurfPointsFlat; 742 | pubSurfPointsLessFlatPointer = &pubSurfPointsLessFlat; 743 | pubImuTransPointer = &pubImuTrans; 744 | 745 | ros::spin(); 746 | 747 | return 0; 748 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /lidar_gnss_mapping/src/rtk_odom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | const double PI = 3.1415926; 24 | 25 | const float scanPeriod = 0.1; 26 | 27 | const int skipFrameNum = 1; 28 | bool systemInited = false; 29 | 30 | double timeCornerPointsSharp = 0; 31 | double timeCornerPointsLessSharp = 0; 32 | double timeSurfPointsFlat = 0; 33 | double timeSurfPointsLessFlat = 0; 34 | double timeLaserCloudFullRes = 0; 35 | double timeImuTrans = 0; 36 | //消息接收标志位 37 | bool newCornerPointsSharp = false; 38 | bool newCornerPointsLessSharp = false; 39 | bool newSurfPointsFlat = false; 40 | bool newSurfPointsLessFlat = false; 41 | bool newLaserCloudFullRes = false; 42 | bool newImuTrans = false; 43 | 44 | pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud()); 45 | pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud()); 46 | pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud()); 47 | pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud()); 48 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud()); 49 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud()); 50 | pcl::PointCloud::Ptr laserCloudOri(new pcl::PointCloud()); 51 | pcl::PointCloud::Ptr coeffSel(new pcl::PointCloud()); 52 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud()); 53 | pcl::PointCloud::Ptr imuTrans(new pcl::PointCloud()); 54 | pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN()); 55 | pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN()); 56 | 57 | int laserCloudCornerLastNum; 58 | int laserCloudSurfLastNum; 59 | 60 | int pointSelCornerInd[40000]; 61 | float pointSearchCornerInd1[40000]; 62 | float pointSearchCornerInd2[40000]; 63 | 64 | int pointSelSurfInd[40000]; 65 | float pointSearchSurfInd1[40000]; 66 | float pointSearchSurfInd2[40000]; 67 | float pointSearchSurfInd3[40000]; 68 | 69 | float transform[6] = {0}; 70 | float transformSum[6] = {0}; 71 | 72 | float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0; 73 | float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0; 74 | float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0; 75 | float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0; 76 | int last_publish_state = 0; 77 | 78 | void ResetState(const nav_msgs::Odometry::ConstPtr& laserOdometry){ 79 | //std::cout<<"--------------Reset Odom State----------------"<pose.pose.orientation; 83 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 84 | 85 | transformSum[0] = -pitch; 86 | transformSum[1] = -yaw; 87 | transformSum[2] = roll; 88 | 89 | transformSum[3] = laserOdometry->pose.pose.position.x; 90 | transformSum[4] = laserOdometry->pose.pose.position.y; 91 | transformSum[5] = laserOdometry->pose.pose.position.z; 92 | 93 | // newLaserOdometry = true; 94 | } 95 | 96 | void TransformToStart(pcl::PointXYZI *pi, pcl::PointXYZI *po) 97 | { 98 | float s = 10 * (pi->intensity - int(pi->intensity)); 99 | 100 | float rx = s * transform[0]; 101 | float ry = s * transform[1]; 102 | float rz = s * transform[2]; 103 | float tx = s * transform[3]; 104 | float ty = s * transform[4]; 105 | float tz = s * transform[5]; 106 | 107 | float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty); 108 | float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty); 109 | float z1 = (pi->z - tz); 110 | 111 | float x2 = x1; 112 | float y2 = cos(rx) * y1 + sin(rx) * z1; 113 | float z2 = -sin(rx) * y1 + cos(rx) * z1; 114 | 115 | po->x = cos(ry) * x2 - sin(ry) * z2; 116 | po->y = y2; 117 | po->z = sin(ry) * x2 + cos(ry) * z2; 118 | po->intensity = pi->intensity; 119 | } 120 | 121 | void TransformToEnd(pcl::PointXYZI *pi, pcl::PointXYZI *po) 122 | { 123 | float s = 10 * (pi->intensity - int(pi->intensity)); 124 | 125 | float rx = s * transform[0]; 126 | float ry = s * transform[1]; 127 | float rz = s * transform[2]; 128 | float tx = s * transform[3]; 129 | float ty = s * transform[4]; 130 | float tz = s * transform[5]; 131 | 132 | float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty); 133 | float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty); 134 | float z1 = (pi->z - tz); 135 | 136 | float x2 = x1; 137 | float y2 = cos(rx) * y1 + sin(rx) * z1; 138 | float z2 = -sin(rx) * y1 + cos(rx) * z1; 139 | 140 | float x3 = cos(ry) * x2 - sin(ry) * z2; 141 | float y3 = y2; 142 | float z3 = sin(ry) * x2 + cos(ry) * z2; 143 | 144 | rx = transform[0]; 145 | ry = transform[1]; 146 | rz = transform[2]; 147 | tx = transform[3]; 148 | ty = transform[4]; 149 | tz = transform[5]; 150 | 151 | float x4 = cos(ry) * x3 + sin(ry) * z3; 152 | float y4 = y3; 153 | float z4 = -sin(ry) * x3 + cos(ry) * z3; 154 | 155 | float x5 = x4; 156 | float y5 = cos(rx) * y4 - sin(rx) * z4; 157 | float z5 = sin(rx) * y4 + cos(rx) * z4; 158 | 159 | float x6 = cos(rz) * x5 - sin(rz) * y5 + tx; 160 | float y6 = sin(rz) * x5 + cos(rz) * y5 + ty; 161 | float z6 = z5 + tz; 162 | 163 | float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) 164 | - sin(imuRollStart) * (y6 - imuShiftFromStartY); 165 | float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) 166 | + cos(imuRollStart) * (y6 - imuShiftFromStartY); 167 | float z7 = z6 - imuShiftFromStartZ; 168 | 169 | float x8 = x7; 170 | float y8 = cos(imuPitchStart) * y7 - sin(imuPitchStart) * z7; 171 | float z8 = sin(imuPitchStart) * y7 + cos(imuPitchStart) * z7; 172 | 173 | float x9 = cos(imuYawStart) * x8 + sin(imuYawStart) * z8; 174 | float y9 = y8; 175 | float z9 = -sin(imuYawStart) * x8 + cos(imuYawStart) * z8; 176 | 177 | float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9; 178 | float y10 = y9; 179 | float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9; 180 | 181 | float x11 = x10; 182 | float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10; 183 | float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10; 184 | 185 | po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11; 186 | po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11; 187 | po->z = z11; 188 | po->intensity = int(pi->intensity); 189 | } 190 | void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, 191 | float alx, float aly, float alz, float &acx, float &acy, float &acz) 192 | { 193 | float sbcx = sin(bcx); 194 | float cbcx = cos(bcx); 195 | float sbcy = sin(bcy); 196 | float cbcy = cos(bcy); 197 | float sbcz = sin(bcz); 198 | float cbcz = cos(bcz); 199 | 200 | float sblx = sin(blx); 201 | float cblx = cos(blx); 202 | float sbly = sin(bly); 203 | float cbly = cos(bly); 204 | float sblz = sin(blz); 205 | float cblz = cos(blz); 206 | 207 | float salx = sin(alx); 208 | float calx = cos(alx); 209 | float saly = sin(aly); 210 | float caly = cos(aly); 211 | float salz = sin(alz); 212 | float calz = cos(alz); 213 | 214 | float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 215 | - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 216 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 217 | - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 218 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz); 219 | acx = -asin(srx); 220 | 221 | float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 222 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 223 | - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 224 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 225 | + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 226 | float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 227 | - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 228 | - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 229 | - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 230 | + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly); 231 | acy = atan2(srycrx / cos(acx), crycrx / cos(acx)); 232 | 233 | float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 234 | - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 235 | - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 236 | + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 237 | - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 238 | + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 239 | + calx*cblx*salz*sblz); 240 | float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 241 | - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 242 | + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 243 | + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 244 | + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 245 | - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 246 | - calx*calz*cblx*sblz); 247 | acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx)); 248 | } 249 | void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, 250 | float &ox, float &oy, float &oz) 251 | { 252 | float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx); 253 | ox = -asin(srx); 254 | 255 | float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 256 | + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy); 257 | float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 258 | - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx)); 259 | oy = atan2(srycrx / cos(ox), crycrx / cos(ox)); 260 | 261 | float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 262 | + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz); 263 | float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 264 | - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx)); 265 | oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox)); 266 | } 267 | 268 | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2) 269 | { 270 | timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec(); 271 | 272 | cornerPointsSharp->clear(); 273 | pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp); 274 | 275 | newCornerPointsSharp = true; 276 | } 277 | 278 | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2) 279 | { 280 | timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec(); 281 | 282 | cornerPointsLessSharp->clear(); 283 | pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp); 284 | 285 | newCornerPointsLessSharp = true; 286 | } 287 | 288 | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2) 289 | { 290 | timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec(); 291 | 292 | surfPointsFlat->clear(); 293 | pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat); 294 | 295 | newSurfPointsFlat = true; 296 | } 297 | 298 | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2) 299 | { 300 | timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec(); 301 | 302 | surfPointsLessFlat->clear(); 303 | pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat); 304 | 305 | newSurfPointsLessFlat = true; 306 | } 307 | 308 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2) 309 | { 310 | timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec(); 311 | 312 | laserCloudFullRes->clear(); 313 | pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes); 314 | 315 | newLaserCloudFullRes = true; 316 | } 317 | 318 | void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2) 319 | { 320 | timeImuTrans = imuTrans2->header.stamp.toSec(); 321 | 322 | imuTrans->clear(); 323 | pcl::fromROSMsg(*imuTrans2, *imuTrans); 324 | 325 | imuPitchStart = imuTrans->points[0].x; 326 | imuYawStart = imuTrans->points[0].y; 327 | imuRollStart = imuTrans->points[0].z; 328 | 329 | imuPitchLast = imuTrans->points[1].x; 330 | imuYawLast = imuTrans->points[1].y; 331 | imuRollLast = imuTrans->points[1].z; 332 | 333 | imuShiftFromStartX = imuTrans->points[2].x; 334 | imuShiftFromStartY = imuTrans->points[2].y; 335 | imuShiftFromStartZ = imuTrans->points[2].z; 336 | 337 | imuVeloFromStartX = imuTrans->points[3].x; 338 | imuVeloFromStartY = imuTrans->points[3].y; 339 | imuVeloFromStartZ = imuTrans->points[3].z; 340 | 341 | newImuTrans = true; 342 | } 343 | 344 | int main(int argc, char** argv) 345 | { 346 | ros::init(argc, argv, "laserOdometry"); 347 | ros::NodeHandle nh; 348 | 349 | ros::Subscriber subCornerPointsSharp = nh.subscribe 350 | ("/laser_cloud_sharp", 2, laserCloudSharpHandler); 351 | 352 | ros::Subscriber subCornerPointsLessSharp = nh.subscribe 353 | ("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler); 354 | 355 | ros::Subscriber subSurfPointsFlat = nh.subscribe 356 | ("/laser_cloud_flat", 2, laserCloudFlatHandler); 357 | 358 | ros::Subscriber subSurfPointsLessFlat = nh.subscribe 359 | ("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler); 360 | 361 | ros::Subscriber subLaserCloudFullRes = nh.subscribe 362 | ("/velodyne_cloud_2", 2, laserCloudFullResHandler); 363 | 364 | ros::Subscriber subImuTrans = nh.subscribe 365 | ("/imu_trans", 5, imuTransHandler); 366 | 367 | ros::Subscriber subCleanOdom = nh.subscribe 368 | ("/clean_odom",10,ResetState); 369 | 370 | ros::Publisher pubLaserCloudCornerLast = nh.advertise 371 | ("/laser_cloud_corner_last", 2); 372 | 373 | ros::Publisher pubLaserCloudSurfLast = nh.advertise 374 | ("/laser_cloud_surf_last", 2); 375 | 376 | ros::Publisher pubLaserCloudFullRes = nh.advertise 377 | ("/velodyne_cloud_3", 2); 378 | 379 | ros::Publisher pubLaserOdometry = nh.advertise ("/laser_odom_to_init", 5); 380 | nav_msgs::Odometry laserOdometry; 381 | laserOdometry.header.frame_id = "/camera_init"; 382 | laserOdometry.child_frame_id = "/laser_odom"; 383 | 384 | tf::TransformBroadcaster tfBroadcaster; 385 | tf::StampedTransform laserOdometryTrans; 386 | laserOdometryTrans.frame_id_ = "/camera_init"; 387 | laserOdometryTrans.child_frame_id_ = "/laser_odom"; 388 | 389 | std::vector pointSearchInd; 390 | std::vector pointSearchSqDis; 391 | 392 | pcl::PointXYZI pointOri, pointSel, tripod1, tripod2, tripod3, pointProj, coeff; 393 | 394 | bool isDegenerate = false; 395 | cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0)); 396 | 397 | int frameCount = skipFrameNum; 398 | ros::Rate rate(100); 399 | bool status = ros::ok(); 400 | while (status) { 401 | ros::spinOnce(); 402 | 403 | if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && 404 | newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans && 405 | fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 && 406 | fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 && 407 | fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 && 408 | fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 && 409 | fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) { 410 | newCornerPointsSharp = false; 411 | newCornerPointsLessSharp = false; 412 | newSurfPointsFlat = false; 413 | newSurfPointsLessFlat = false; 414 | newLaserCloudFullRes = false; 415 | newImuTrans = false; 416 | if (!systemInited) { 417 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 418 | cornerPointsLessSharp = laserCloudCornerLast; 419 | laserCloudCornerLast = laserCloudTemp; 420 | 421 | laserCloudTemp = surfPointsLessFlat; 422 | surfPointsLessFlat = laserCloudSurfLast; 423 | laserCloudSurfLast = laserCloudTemp; 424 | 425 | kdtreeCornerLast->setInputCloud(laserCloudCornerLast); 426 | kdtreeSurfLast->setInputCloud(laserCloudSurfLast); 427 | 428 | sensor_msgs::PointCloud2 laserCloudCornerLast2; 429 | pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); 430 | laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 431 | laserCloudCornerLast2.header.frame_id = "/camera"; 432 | pubLaserCloudCornerLast.publish(laserCloudCornerLast2); 433 | 434 | sensor_msgs::PointCloud2 laserCloudSurfLast2; 435 | pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); 436 | laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 437 | laserCloudSurfLast2.header.frame_id = "/camera"; 438 | pubLaserCloudSurfLast.publish(laserCloudSurfLast2); 439 | 440 | transformSum[0] += imuPitchStart; 441 | //transformSum[1] += imuYawStart; 442 | transformSum[2] += imuRollStart; 443 | 444 | systemInited = true; 445 | continue; 446 | } 447 | 448 | transform[3] -= imuVeloFromStartX * scanPeriod; 449 | transform[4] -= imuVeloFromStartY * scanPeriod; 450 | transform[5] -= imuVeloFromStartZ * scanPeriod; 451 | 452 | if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { 453 | int cornerPointsSharpNum = cornerPointsSharp->points.size(); 454 | int surfPointsFlatNum = surfPointsFlat->points.size(); 455 | 456 | for (int iterCount = 0; iterCount < 25; iterCount++) { 457 | laserCloudOri->clear(); 458 | coeffSel->clear(); 459 | for (int i = 0; i < cornerPointsSharpNum; i++) { 460 | TransformToStart(&cornerPointsSharp->points[i], &pointSel); 461 | 462 | if (iterCount % 5 == 0) { 463 | kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 464 | 465 | int closestPointInd = -1, minPointInd2 = -1; 466 | if (pointSearchSqDis[0] < 25) { 467 | closestPointInd = pointSearchInd[0]; 468 | int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity); 469 | 470 | float pointSqDis, minPointSqDis2 = 25; 471 | for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) { 472 | if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) { 473 | break; 474 | } 475 | 476 | pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 477 | (laserCloudCornerLast->points[j].x - pointSel.x) + 478 | (laserCloudCornerLast->points[j].y - pointSel.y) * 479 | (laserCloudCornerLast->points[j].y - pointSel.y) + 480 | (laserCloudCornerLast->points[j].z - pointSel.z) * 481 | (laserCloudCornerLast->points[j].z - pointSel.z); 482 | 483 | if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) { 484 | if (pointSqDis < minPointSqDis2) { 485 | minPointSqDis2 = pointSqDis; 486 | minPointInd2 = j; 487 | } 488 | } 489 | } 490 | for (int j = closestPointInd - 1; j >= 0; j--) { 491 | if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) { 492 | break; 493 | } 494 | 495 | pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 496 | (laserCloudCornerLast->points[j].x - pointSel.x) + 497 | (laserCloudCornerLast->points[j].y - pointSel.y) * 498 | (laserCloudCornerLast->points[j].y - pointSel.y) + 499 | (laserCloudCornerLast->points[j].z - pointSel.z) * 500 | (laserCloudCornerLast->points[j].z - pointSel.z); 501 | 502 | if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) { 503 | if (pointSqDis < minPointSqDis2) { 504 | minPointSqDis2 = pointSqDis; 505 | minPointInd2 = j; 506 | } 507 | } 508 | } 509 | } 510 | 511 | pointSearchCornerInd1[i] = closestPointInd; 512 | pointSearchCornerInd2[i] = minPointInd2; 513 | } 514 | 515 | if (pointSearchCornerInd2[i] >= 0) { 516 | tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]]; 517 | tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]]; 518 | 519 | float x0 = pointSel.x; 520 | float y0 = pointSel.y; 521 | float z0 = pointSel.z; 522 | float x1 = tripod1.x; 523 | float y1 = tripod1.y; 524 | float z1 = tripod1.z; 525 | float x2 = tripod2.x; 526 | float y2 = tripod2.y; 527 | float z2 = tripod2.z; 528 | 529 | float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 530 | * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 531 | + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 532 | * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 533 | + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) 534 | * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); 535 | 536 | float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); 537 | 538 | float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 539 | + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; 540 | 541 | float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 542 | - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 543 | 544 | float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 545 | + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; 546 | 547 | float ld2 = a012 / l12; 548 | 549 | pointProj = pointSel; 550 | pointProj.x -= la * ld2; 551 | pointProj.y -= lb * ld2; 552 | pointProj.z -= lc * ld2; 553 | 554 | float s = 1; 555 | if (iterCount >= 5) { 556 | s = 1 - 1.8 * fabs(ld2); 557 | } 558 | 559 | coeff.x = s * la; 560 | coeff.y = s * lb; 561 | coeff.z = s * lc; 562 | coeff.intensity = s * ld2; 563 | 564 | if (s > 0.1 && ld2 != 0) { 565 | laserCloudOri->push_back(cornerPointsSharp->points[i]); 566 | coeffSel->push_back(coeff); 567 | } 568 | } 569 | } 570 | 571 | for (int i = 0; i < surfPointsFlatNum; i++) { 572 | TransformToStart(&surfPointsFlat->points[i], &pointSel); 573 | 574 | if (iterCount % 5 == 0) { 575 | kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); 576 | 577 | int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; 578 | if (pointSearchSqDis[0] < 25) { 579 | closestPointInd = pointSearchInd[0]; 580 | int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity); 581 | 582 | float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25; 583 | for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) { 584 | if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) { 585 | break; 586 | } 587 | 588 | pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 589 | (laserCloudSurfLast->points[j].x - pointSel.x) + 590 | (laserCloudSurfLast->points[j].y - pointSel.y) * 591 | (laserCloudSurfLast->points[j].y - pointSel.y) + 592 | (laserCloudSurfLast->points[j].z - pointSel.z) * 593 | (laserCloudSurfLast->points[j].z - pointSel.z); 594 | 595 | if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) { 596 | if (pointSqDis < minPointSqDis2) { 597 | minPointSqDis2 = pointSqDis; 598 | minPointInd2 = j; 599 | } 600 | } else { 601 | if (pointSqDis < minPointSqDis3) { 602 | minPointSqDis3 = pointSqDis; 603 | minPointInd3 = j; 604 | } 605 | } 606 | } 607 | for (int j = closestPointInd - 1; j >= 0; j--) { 608 | if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) { 609 | break; 610 | } 611 | 612 | pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * 613 | (laserCloudSurfLast->points[j].x - pointSel.x) + 614 | (laserCloudSurfLast->points[j].y - pointSel.y) * 615 | (laserCloudSurfLast->points[j].y - pointSel.y) + 616 | (laserCloudSurfLast->points[j].z - pointSel.z) * 617 | (laserCloudSurfLast->points[j].z - pointSel.z); 618 | 619 | if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) { 620 | if (pointSqDis < minPointSqDis2) { 621 | minPointSqDis2 = pointSqDis; 622 | minPointInd2 = j; 623 | } 624 | } else { 625 | if (pointSqDis < minPointSqDis3) { 626 | minPointSqDis3 = pointSqDis; 627 | minPointInd3 = j; 628 | } 629 | } 630 | } 631 | } 632 | 633 | pointSearchSurfInd1[i] = closestPointInd; 634 | pointSearchSurfInd2[i] = minPointInd2; 635 | pointSearchSurfInd3[i] = minPointInd3; 636 | } 637 | 638 | if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) { 639 | tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; 640 | tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; 641 | tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; 642 | 643 | float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) 644 | - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z); 645 | float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) 646 | - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x); 647 | float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) 648 | - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y); 649 | float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z); 650 | 651 | float ps = sqrt(pa * pa + pb * pb + pc * pc); 652 | pa /= ps; 653 | pb /= ps; 654 | pc /= ps; 655 | pd /= ps; 656 | 657 | float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; 658 | 659 | pointProj = pointSel; 660 | pointProj.x -= pa * pd2; 661 | pointProj.y -= pb * pd2; 662 | pointProj.z -= pc * pd2; 663 | 664 | float s = 1; 665 | if (iterCount >= 5) { 666 | s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x 667 | + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); 668 | } 669 | 670 | coeff.x = s * pa; 671 | coeff.y = s * pb; 672 | coeff.z = s * pc; 673 | coeff.intensity = s * pd2; 674 | 675 | if (s > 0.1 && pd2 != 0) { 676 | laserCloudOri->push_back(surfPointsFlat->points[i]); 677 | coeffSel->push_back(coeff); 678 | } 679 | } 680 | } 681 | int pointSelNum = laserCloudOri->points.size(); 682 | if (pointSelNum < 10) { 683 | continue; 684 | } 685 | 686 | cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0)); 687 | cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0)); 688 | cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); 689 | cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0)); 690 | cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); 691 | cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); 692 | for (int i = 0; i < pointSelNum; i++) { 693 | pointOri = laserCloudOri->points[i]; 694 | coeff = coeffSel->points[i]; 695 | 696 | float s = 1; 697 | 698 | float srx = sin(s * transform[0]); 699 | float crx = cos(s * transform[0]); 700 | float sry = sin(s * transform[1]); 701 | float cry = cos(s * transform[1]); 702 | float srz = sin(s * transform[2]); 703 | float crz = cos(s * transform[2]); 704 | float tx = s * transform[3]; 705 | float ty = s * transform[4]; 706 | float tz = s * transform[5]; 707 | 708 | float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z 709 | + s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x 710 | + (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z 711 | + s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y 712 | + (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z 713 | + s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z; 714 | 715 | float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x 716 | + (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z 717 | + tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx) 718 | + s*tz*crx*cry) * coeff.x 719 | + ((s*cry*crz - s*srx*sry*srz)*pointOri.x 720 | + (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z 721 | + s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry) 722 | - tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z; 723 | 724 | float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y 725 | + tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x 726 | + (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y 727 | + s*ty*crx*srz + s*tx*crx*crz) * coeff.y 728 | + ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y 729 | + tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z; 730 | 731 | float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y 732 | - s*(crz*sry + cry*srx*srz) * coeff.z; 733 | 734 | float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y 735 | - s*(sry*srz - cry*crz*srx) * coeff.z; 736 | 737 | float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z; 738 | 739 | float d2 = coeff.intensity; 740 | 741 | matA.at(i, 0) = arx; 742 | matA.at(i, 1) = ary; 743 | matA.at(i, 2) = arz; 744 | matA.at(i, 3) = atx; 745 | matA.at(i, 4) = aty; 746 | matA.at(i, 5) = atz; 747 | matB.at(i, 0) = -0.05 * d2; 748 | } 749 | cv::transpose(matA, matAt); 750 | 751 | matAtA = matAt * matA + 0.1*cv::Mat::eye(6,6,CV_32F); 752 | matAtB = matAt * matB; 753 | cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); 754 | 755 | if (iterCount == 0) { 756 | cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0)); 757 | cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0)); 758 | cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0)); 759 | 760 | cv::eigen(matAtA, matE, matV); 761 | matV.copyTo(matV2); 762 | 763 | isDegenerate = false; 764 | float eignThre[6] = {10, 10, 10, 10, 10, 10}; 765 | for (int i = 5; i >= 0; i--) { 766 | if (matE.at(0, i) < eignThre[i]) { 767 | for (int j = 0; j < 6; j++) { 768 | matV2.at(i, j) = 0; 769 | } 770 | isDegenerate = true; 771 | } else { 772 | break; 773 | } 774 | } 775 | matP = matV.inv() * matV2; 776 | } 777 | 778 | if (isDegenerate) { 779 | cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0)); 780 | matX.copyTo(matX2); 781 | matX = matP * matX2; 782 | 783 | //ROS_INFO ("laser odometry degenerate"); 784 | } 785 | transform[0] += matX.at(0, 0); 786 | transform[1] += matX.at(1, 0); 787 | transform[2] += matX.at(2, 0); 788 | transform[3] += matX.at(3, 0); 789 | transform[4] += matX.at(4, 0); 790 | transform[5] += matX.at(5, 0); 791 | 792 | float deltaR = sqrt(matX.at(0, 0) * 180 / PI * matX.at(0, 0) * 180 / PI 793 | + matX.at(1, 0) * 180 / PI * matX.at(1, 0) * 180 / PI 794 | + matX.at(2, 0) * 180 / PI * matX.at(2, 0) * 180 / PI); 795 | float deltaT = sqrt(matX.at(3, 0) * 100 * matX.at(3, 0) * 100 796 | + matX.at(4, 0) * 100 * matX.at(4, 0) * 100 797 | + matX.at(5, 0) * 100 * matX.at(5, 0) * 100); 798 | 799 | if (deltaR < 0.1 && deltaT < 0.1) { 800 | break; 801 | } 802 | } 803 | } 804 | 805 | float rx, ry, rz, tx, ty, tz; 806 | AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 807 | -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz); 808 | float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) 809 | - sin(rz) * (transform[4] - imuShiftFromStartY); 810 | float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) 811 | + cos(rz) * (transform[4] - imuShiftFromStartY); 812 | float z1 = transform[5] * 1.05 - imuShiftFromStartZ; 813 | 814 | float x2 = x1; 815 | float y2 = cos(rx) * y1 - sin(rx) * z1; 816 | float z2 = sin(rx) * y1 + cos(rx) * z1; 817 | tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2); 818 | ty = transformSum[4] - y2; 819 | tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2); 820 | 821 | PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 822 | imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz); 823 | 824 | transformSum[0] = rx; 825 | transformSum[1] = ry; 826 | transformSum[2] = rz; 827 | transformSum[3] = tx; 828 | transformSum[4] = ty; 829 | transformSum[5] = tz; 830 | 831 | geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry); 832 | 833 | laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 834 | laserOdometry.pose.pose.orientation.x = -geoQuat.y; 835 | laserOdometry.pose.pose.orientation.y = -geoQuat.z; 836 | laserOdometry.pose.pose.orientation.z = geoQuat.x; 837 | laserOdometry.pose.pose.orientation.w = geoQuat.w; 838 | laserOdometry.pose.pose.position.x = tx; 839 | laserOdometry.pose.pose.position.y = ty; 840 | laserOdometry.pose.pose.position.z = tz; 841 | pubLaserOdometry.publish(laserOdometry); 842 | 843 | laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat); 844 | laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); 845 | laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz)); 846 | tfBroadcaster.sendTransform(laserOdometryTrans); 847 | 848 | int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); 849 | for (int i = 0; i < cornerPointsLessSharpNum; i++) { 850 | TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); 851 | } 852 | 853 | int surfPointsLessFlatNum = surfPointsLessFlat->points.size(); 854 | for (int i = 0; i < surfPointsLessFlatNum; i++) { 855 | TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]); 856 | } 857 | 858 | frameCount++; 859 | 860 | if (frameCount >= skipFrameNum + 1) { 861 | int laserCloudFullResNum = laserCloudFullRes->points.size(); 862 | for (int i = 0; i < laserCloudFullResNum; i++) { 863 | TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]); 864 | } 865 | } 866 | 867 | pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp; 868 | cornerPointsLessSharp = laserCloudCornerLast; 869 | laserCloudCornerLast = laserCloudTemp; 870 | 871 | laserCloudTemp = surfPointsLessFlat; 872 | surfPointsLessFlat = laserCloudSurfLast; 873 | laserCloudSurfLast = laserCloudTemp; 874 | 875 | laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 876 | laserCloudSurfLastNum = laserCloudSurfLast->points.size(); 877 | if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) { 878 | kdtreeCornerLast->setInputCloud(laserCloudCornerLast); 879 | kdtreeSurfLast->setInputCloud(laserCloudSurfLast); 880 | } 881 | 882 | if (frameCount >= skipFrameNum + 1) { 883 | frameCount = 0; 884 | 885 | sensor_msgs::PointCloud2 laserCloudCornerLast2; 886 | pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); 887 | laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 888 | laserCloudCornerLast2.header.frame_id = "/camera"; 889 | pubLaserCloudCornerLast.publish(laserCloudCornerLast2); 890 | 891 | sensor_msgs::PointCloud2 laserCloudSurfLast2; 892 | pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2); 893 | laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 894 | laserCloudSurfLast2.header.frame_id = "/camera"; 895 | pubLaserCloudSurfLast.publish(laserCloudSurfLast2); 896 | 897 | sensor_msgs::PointCloud2 laserCloudFullRes3; 898 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3); 899 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); 900 | laserCloudFullRes3.header.frame_id = "/camera"; 901 | pubLaserCloudFullRes.publish(laserCloudFullRes3); 902 | } 903 | } 904 | 905 | status = ros::ok(); 906 | rate.sleep(); 907 | } 908 | 909 | return 0; 910 | } --------------------------------------------------------------------------------