| 96 | } |
| 97 | |
| 98 | void ImuProcess::Process(const MeasureGroup &meas) |
| 99 | { |
| 100 | ROS_ASSERT(!meas.imu.empty()); |
| 101 | ROS_ASSERT(meas.lidar != nullptr); |
| 102 | ROS_DEBUG("Process lidar at time: %.4f, %lu imu msgs from %.4f to %.4f", |
| 103 | meas.lidar->header.stamp.toSec(), meas.imu.size(), |
| 104 | meas.imu.front()->header.stamp.toSec(), |
| 105 | meas.imu.back()->header.stamp.toSec()); |
| 106 | |
| 107 | auto pcl_in_msg = meas.lidar; |
| 108 | |
| 109 | if (b_first_frame_) |
| 110 | { |
| 111 | /// The very first lidar frame |
| 112 | |
| 113 | /// Reset |
| 114 | Reset(); |
| 115 | |
| 116 | /// Record first lidar, and first useful imu |
| 117 | last_lidar_ = pcl_in_msg; |
| 118 | last_imu_ = meas.imu.back(); |
| 119 | |
| 120 | ROS_WARN("The very first lidar frame"); |
| 121 | |
| 122 | /// Do nothing more, return |
| 123 | b_first_frame_ = false; |
| 124 | return; |
| 125 | } |
| 126 | |
| 127 | /// Integrate all input imu message |
| 128 | IntegrateGyr(meas.imu); |
| 129 | |
| 130 | /// Compensate lidar points with IMU rotation |
| 131 | //// Initial pose from IMU (with only rotation) |
| 132 | SE3d T_l_c(gyr_int_.GetRot(), Eigen::Vector3d::Zero()); |
| 133 | dt_l_c_ = |
| 134 | pcl_in_msg->header.stamp.toSec() - last_lidar_->header.stamp.toSec(); |
| 135 | //// Get input pcl |
| 136 | pcl::fromROSMsg(*pcl_in_msg, *cur_pcl_in_); |
| 137 | |
| 138 | /// Undistort points |
| 139 | |
| 140 | Sophus::SE3d T_l_be = T_i_l.inverse() * T_l_c * T_i_l; |
| 141 | pcl::copyPointCloud(*cur_pcl_in_, *cur_pcl_un_); |
| 142 | UndistortPcl(cur_pcl_un_, dt_l_c_, T_l_be); |
| 143 | |
| 144 | { |
| 145 | static ros::Publisher pub_UndistortPcl = |
| 146 | nh.advertise<sensor_msgs::PointCloud2>("/livox_first_point", 100); |
| 147 | sensor_msgs::PointCloud2 pcl_out_msg; |
| 148 | pcl::toROSMsg(*laserCloudtmp, pcl_out_msg); |
| 149 | pcl_out_msg.header = pcl_in_msg->header; |
| 150 | pcl_out_msg.header.frame_id = "/camera_init"; |
| 151 | pub_UndistortPcl.publish(pcl_out_msg); |
| 152 | laserCloudtmp->clear(); |
| 153 | } |
| 154 | |
| 155 | { |