| 42 | } |
| 43 | |
| 44 | void ImuProcess::IntegrateGyr(const std::vector<sensor_msgs::Imu::ConstPtr> &v_imu) |
| 45 | { |
| 46 | /// Reset gyr integrator |
| 47 | gyr_int_.Reset(last_lidar_->header.stamp.toSec(), last_imu_); |
| 48 | /// And then integrate all the imu measurements |
| 49 | for (const auto &imu : v_imu) |
| 50 | { |
| 51 | gyr_int_.Integrate(imu); |
| 52 | } |
| 53 | ROS_INFO("integrate rotation angle [x, y, z]: [%.2f, %.2f, %.2f]", |
| 54 | gyr_int_.GetRot().angleX() * 180.0 / M_PI, |
| 55 | gyr_int_.GetRot().angleY() * 180.0 / M_PI, |
| 56 | gyr_int_.GetRot().angleZ() * 180.0 / M_PI); |
| 57 | } |
| 58 | |
| 59 | void ImuProcess::UndistortPcl(const PointCloudXYZI::Ptr &pcl_in_out,double dt_be, const Sophus::SE3d &Tbe) |
| 60 | { |