修改PointLIO项目

添加key_frame_info.msg消息

新建.msg文件，内容填写为：

# Cloud Info
Header header # cloud messages
sensor_msgs/PointCloud2 key_frame_cloud_ori
sensor_msgs/PointCloud2 key_frame_cloud_transed
sensor_msgs/PointCloud2 key_frame_poses

其中key_frame_cloud_ori为原始点云（需要包含行号ring），key_frame_cloud_transed 为利用SLAM转换到统一坐标系后的点云．key_frame_poses为SLAM输出的位姿．

把构建好的key_frame_info.msg文件放在msg文件夹下，随后，在CMakeLists.txt文件中添加

add_message_files(FILESkey_frame_info.msg
)

并修改generate_messages，在generate_messages中添加sensor_msgs

generate_messages(DEPENDENCIESgeometry_msgssensor_msgs
)

编译，会生成key_frame_info.h文件．

之后在common_lib.h文件中添加key_frame_info.h文件的引用，即可添加使用自定义的key_frame_info消息类型

#include <point_lio/key_frame_info.h>

添加自定义数据结构PointXYZIRPYT，并重命名为PointTypePose（用于存储X,Y,Z,Roll,Pitch,Yaw）

找到preprocess.h头文件，在velodyne_ros，hesai_ros，ouster_ros等一系列数据结构后面添加下述代码：

/** A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)*/
struct PointXYZIRPYT
{PCL_ADD_POINT4DPCL_ADD_INTENSITY;                  // preferred way of adding a XYZ+paddingfloat roll;float pitch;float yaw;double time;EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignmentPOINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,(float, x, x) (float, y, y)(float, z, z) (float, intensity, intensity)(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)(double, time, time))typedef PointXYZIRPYT  PointTypePose;

添加额外的变量

注意：本文以ouster lidar为例，所以在变量命名过程中大多贴近Ouster

1. ouster_buffer (存储ouster原始点云类型指针)，ouster_undistort (存储ouster单帧原始点云数据，从ouster_buffer提取数据)，key_frame_poses_data (存储PointLIO计算的关键帧位姿，点类型为前述定义的PointTypePose)

在li_initialization.h头文件中添加声明：

// set the ouster data buffer
extern  std::deque<pcl::PointCloud<ouster_ros::Point>::Ptr> ouster_buffer;
// set the ouster point cloud and the key frame pose
extern pcl::PointCloud<ouster_ros::Point>::Ptr ouster_undistort;
// set the key frame pose
extern pcl::PointCloud<PointTypePose>::Ptr key_frame_poses_data;

在li_initialization.cpp头文件中添加定义：

// set the ouster data buffer
std::deque<pcl::PointCloud<ouster_ros::Point>::Ptr>  ouster_buffer;
// set the ouster point cloud and the key frame pose
pcl::PointCloud<ouster_ros::Point>::Ptr ouster_undistort(new pcl::PointCloud<ouster_ros::Point>());
// set the key frame pose
pcl::PointCloud<PointTypePose>::Ptr key_frame_poses_data(new pcl::PointCloud<PointTypePose>());

上述两部分均放在了lidar_buffer, time_buffer, imu_deque等变量之后．

２. pubKeyFrameInfo变量，以发布关键帧相关信息
在laserMapping.cpp文件中添加，定义如下：

 ros::Publisher pubKeyFrameInfo  = nh.advertise<point_lio::key_frame_info> ("/point_lio/key_frame_info", 100000);

即发布"/point_lio/key_frame_info"消息，存储关键帧位置的原始点云，转换到world坐标系的点云，关键帧位姿等信息．

添加自定义函数

1. 在laserMapping.cpp文件中添加发布关键帧信息函数publish_key_frame_info：

/******* Publish Key Frame Info *******/
void publish_key_frame_info(const ros::Publisher pubKeyFrameInfo)
{// set the key frame infopoint_lio::key_frame_info keyframeInfo;keyframeInfo.header.stamp = ros::Time().fromSec(lidar_end_time);keyframeInfo.header.frame_id = "camera_init";pcl::PointCloud<ouster_ros::Point>::Ptr cloud(new pcl::PointCloud<ouster_ros::Point>);// transformed the ouster dataint size = ouster_undistort->points.size();pcl::PointCloud<ouster_ros::Point>::Ptr ousterCloudWorld(new pcl::PointCloud<ouster_ros::Point>(size,1));for (int i = 0; i < size; i++){pointBodyToWorld(&ouster_undistort->points[i], &ousterCloudWorld->points[i]);(&ouster_undistort->points[i], &ousterCloudWorld->points[i]);}// get the original Ouster datasensor_msgs::PointCloud2 ousterCloudOriMsg;pcl::toROSMsg(*ouster_undistort, ousterCloudOriMsg);ousterCloudOriMsg.header.stamp = ros::Time().fromSec(lidar_end_time);ousterCloudOriMsg.header.frame_id = "camera_init";// get the transformed Ouster datasensor_msgs::PointCloud2 ousterCloudWorldMsg;pcl::toROSMsg(*ousterCloudWorld, ousterCloudWorldMsg);ousterCloudWorldMsg.header.stamp = ros::Time().fromSec(lidar_end_time);ousterCloudWorldMsg.header.frame_id = "camera_init";// get current transtformationstd::vector<double> curr_pose = getKeyTransformation();// set the key frame Info msg data (include original data and tranformed to world)keyframeInfo.key_frame_cloud_ori = ousterCloudOriMsg;keyframeInfo.key_frame_cloud_transed = ousterCloudWorldMsg;/*** slected the keyframe at 1Hz ***/static int jjj = 0;if (jjj % 10 == 0) {// stack the pose at 1 HzPointTypePose pj;pj.x = curr_pose[0]; pj.y = curr_pose[1]; pj.z = curr_pose[2];pj.roll = curr_pose[3]; pj.pitch = curr_pose[4]; pj.yaw = curr_pose[5];key_frame_poses_data->push_back(pj);// convert the point cloud into sensor messagesensor_msgs::PointCloud2 keyFramePosesMsg;keyFramePosesMsg.header.stamp = ros::Time().fromSec(lidar_end_time);keyFramePosesMsg.header.frame_id = "camera_init";pcl::toROSMsg(*key_frame_poses_data, keyFramePosesMsg);// set the data of key frame infokeyframeInfo.key_frame_poses = keyFramePosesMsg;pubKeyFrameInfo.publish(keyframeInfo);}jjj++;
}

2. 在Estimator.h头文件中添加转换点坐标系函数，专门针对Ouster类型进行处理：

// transform the ouster point from body to world
void pointBodyToWorld(ouster_ros::Point const * const pi, ouster_ros::Point * const po);

随后在对应的Estimator.cpp头文件中添加函数定义

// transform the ouster point from body to world
void pointBodyToWorld(ouster_ros::Point const * const pi, ouster_ros::Point * const po)
{if (pi == nullptr || po == nullptr) {std::cerr << "Error: Null pointer passed to RGBpointBodyToWorld!" << std::endl;return;}V3D p_body(pi->x, pi->y, pi->z);V3D p_global;if (extrinsic_est_en){	if (!use_imu_as_input){p_global = kf_output.x_.rot * (kf_output.x_.offset_R_L_I * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;}else{p_global = kf_input.x_.rot * (kf_input.x_.offset_R_L_I * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;}}else{if (!use_imu_as_input){p_global = kf_output.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; // .normalized()}else{p_global = kf_input.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; // .normalized()}}po->x = p_global(0);po->y = p_global(1);po->z = p_global(2);// copy pipo->intensity = pi->intensity;po->t = pi->t;po->reflectivity = pi->reflectivity;po->ring = pi->ring;po->ambient = pi->ambient;po->range = pi->range;}

3. 在Estimator.h头文件中添加获取当前变换的函数，能够获取PointLIO系统当前处理的坐标系变换，输出为X,Y,Z,Roll,Pitch,Yaw，并存储在一个vector中：

声明如下：

// get current key transformation data
std::vector<double> getKeyTransformation();

在Estimator.cpp文件中的对应定义如下：

// get current key transformation data
std::vector<double> getKeyTransformation()
{SO3 ROT;vect3 TRANS;if (extrinsic_est_en){	if (!use_imu_as_input){ROT = kf_output.x_.rot * kf_output.x_.offset_R_L_I;TRANS = kf_output.x_.rot * kf_output.x_.offset_T_L_I + kf_output.x_.pos;// p_global = kf_output.x_.rot * (kf_output.x_.offset_R_L_I * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos;}else{ROT = kf_input.x_.rot * kf_input.x_.offset_R_L_I;TRANS = kf_input.x_.rot * kf_input.x_.offset_T_L_I + kf_input.x_.pos;// p_global = kf_input.x_.rot * (kf_input.x_.offset_R_L_I * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos;}}else{if (!use_imu_as_input){ROT = kf_output.x_.rot * Lidar_R_wrt_IMU;TRANS = kf_output.x_.rot * Lidar_T_wrt_IMU + kf_output.x_.pos;// p_global = kf_output.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; // .normalized()}else{ROT = kf_input.x_.rot * Lidar_R_wrt_IMU;TRANS = kf_input.x_.rot * Lidar_T_wrt_IMU + kf_input.x_.pos;// p_global = kf_input.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; // .normalized()}}// create an Affine variableEigen::Affine3d affine = Eigen::Affine3d::Identity();affine.linear() = ROT;affine.translation() = TRANS;// convert to the x y z roll pitch yaw variablesstd::vector<double> transformed_para;double x, y, z, roll, pitch, yaw;pcl::getTranslationAndEulerAngles(affine, x, y, z, roll, pitch, yaw);// return datatransformed_para.push_back(x); transformed_para.push_back(y); transformed_para.push_back(z);transformed_para.push_back(roll); transformed_para.push_back(pitch); transformed_para.push_back(yaw);return transformed_para;
}

添加处理流程

首先，要能够用ouster_buffer 在standard_pcl_cbk回调函数中接收到ouster数据消息，修改如下：

// 修改前if (ptr->points.size() > 0){lidar_buffer.emplace_back(ptr);time_buffer.emplace_back(msg->header.stamp.toSec());}
// 修改后if (ptr->points.size() > 0){lidar_buffer.emplace_back(ptr);time_buffer.emplace_back(msg->header.stamp.toSec());// get the ouster datapcl::PointCloud<ouster_ros::Point>::Ptr ouster_data(new pcl::PointCloud<ouster_ros::Point>());pcl::fromROSMsg(*msg, *ouster_data);ouster_buffer.push_back(ouster_data);}

随后，在sync_packages函数中，从ouster_buffer提取数据，到ouster_undistort变量：

// 修改前if (lidar_buffer.empty() || imu_deque.empty()){return false;}
// 修改后if (lidar_buffer.empty() || imu_deque.empty() || ouster_buffer.empty()){return false;}

随后，提取数据：

    /*** push a lidar scan ***/if(!lidar_pushed){lose_lid = false;meas.lidar = lidar_buffer.front();meas.lidar_beg_time = time_buffer.front();//在这个位置提取数据:ouster_undistort = ouster_buffer.front();

在函数的最后，把ouster_buffer变量的头部数据pop掉：

    lidar_buffer.pop_front();time_buffer.pop_front();// 添加代码ouster_buffer.pop_front();lidar_pushed = false;imu_pushed = false;return true;

通过上述过程，我们能够在pointlio系统运行过程中不断获取ouster_undistort原始点云，回到laserMapping.cpp主函数中，我们在发布消息的位置添加我们自定义的关键帧信息发布函数publish_key_frame_info．

添加如下：

            /******* Publish points *******/if (path_en)                         publish_path(pubPath);if (scan_pub_en || pcd_save_en)      publish_frame_world(pubLaserCloudFullRes);if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body);// 添加自定义关键帧发布函数/******* Publish keyframe information *******/publish_key_frame_info(pubKeyFrameInfo);

• 在publish_key_frame_info函数中，我们定义了一个关键帧消息point_lio::key_frame_info
  keyframeInfo;

• 随后，使用自定义的pointBodyToWorld函数，把原始ouster_undistort数据，转换到了world坐标系，放到ousterCloudWorld变量中．

• 分别对ouster_undistort变量和ousterCloudWorld变量，转换为ROS消息，放在keyframeInfo结构中．

• 使用getKeyTransformation函数获取当前位姿，转换为PointTypePose类型，接着放在key_frame_poses_data变量中，同样转换为ROS消息，放在keyframeInfo结构中，以1Hz的频率发布．

我们的在线处理模块，参数文件中接收对应的"/point_lio/key_frame_info"话题，即可收到关键帧数据，进行处理，在线进行单木属性提取的工作．