雷达imu外参在state_point里并未进行初始化

[taffychann]

void RGBpointBodyLidarToIMU(PointType const *const pi, PointType *const po)
{
    V3D p_body_lidar(pi->x, pi->y, pi->z);
    V3D p_body_imu(state_point.offset_R_L_I.matrix() * p_body_lidar + state_point.offset_T_L_I);
    std::cout<<"offset_R_L_I: "<<state_point.offset_R_L_I<<std::endl;
    std::cout<<"offset_T_L_I: "<<state_point.offset_T_L_I.transpose()<<std::endl;
    po->x = p_body_imu(0);
    po->y = p_body_imu(1);
    po->z = p_body_imu(2);
    po->intensity = pi->intensity;
}

上面的代码里，使用了state_point中的lidar-imu外参对点云进行转换，但此时，state_point中，这两个外参还未被初始化。所以，如果使用的lidar和imu的朝向不同，初始化时的逻辑就会出错。代码能运行的前提是lidar imu外参刚好的state_point中的初始lidar imu外参接近。