前段时间参与一个项目需要进行激光雷达和imu的外参标定，发现目前网上开源的标定包只有浙江大学开源的方案-lidar_IMU_calib、lidar-align等少数几种，其中lidar-align听说精度不高，所以最后选择浙大的开源方案lidar_IMU_calib，在github上查询后发现它目前只支持VLP-16线激光雷达，而我参与的项目用的是RS-32线激光雷达，但根据作者叙述应该很容易扩展，在经过一番挣扎终于成功了，下面是实现的流程。

---

一、修改方法

最开始感觉无从下手，还从官网上下载了论文翻译并看了一遍，毕竟以前没有接触过标定算法，后来才发现压根没必要，扩展激光雷达并不需要了解该标定算法的原理。

学长之前告诉我可以在issues里面找找其他人的做法，在翻完所有回答后发现了这位大佬的建议：

 即修改从https://github.com/ethz-asl/lidar_align.git下载的代码中/include/utils下的dataset_reader.h文件和vlp_common.h文件，修改关于激光雷达类型的相关函数和变量、常量等。

二、修改vlp_common.h文件

打开文件后先看了一遍代码，即定义了一个VelodyneCorrection类，里面包含各种函数和结构体等，因为修改的地方比较多，所以直接一个个上图说明：

这一部分定义了激光雷达类型和构造函数，我参与的项目用的是RS-32 激光雷达，你可以把类型和初始化参数直接修改成你的激光雷达类型，名字随便取，但你要保证后面代码都用这个名字。

接下来是unpack_scan函数，作者进行了函数重载，有两个unpack_scan函数，第一个是将velodyne_msgs::VelodyneScan类型点云转化为TPointCloud（就是xyzit）类型点云，这里velodyne_msgs::VelodyneScan类型点云数据包格式在用户手册可以找到，例如下面是程序原来的vlp-16的数据包格式：

 下面是我用的RS-32的数据包格式：

 它们有一定的相似性但又有所不同，具体含义可以看用户手册，或者搜“vlp-16 激光雷达数据格式”有一些博客帮助理解。下面修改的代码基本上都要参考用户手册，16线激光雷达有fring这个参数，而32线激光雷达不需要，可以直接改为1。注意，要使用第一个unpack_scan函数，需要把输入点云的数据包类型改为你的激光雷达数据包类型，例如vlp-16是velodyne_msgs::VelodyneScan，我是把这个函数修改完后才发现这个问题的，但是因为时间紧迫，我没有精力再找rs-32这个类型叫啥，而且也不知道这种类型的包怎么录制，问学长说他也没有录过这个类型的包，所以最后我还是使用的第二个unpack_scan函数。

这里放一下第一个函数修改后的代码（传入的数据包格式不知道叫啥就没改），给想要使用这个函数的同学参考一下（当然不一定对，我也没验证过）。

    void unpack_scan(const velodyne_msgs::VelodyneScan::ConstPtr &lidarMsg,
                     TPointCloud &outPointCloud) const
    {
      outPointCloud.clear();
      outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header);
      //设置点云格式
      if (m_modelType == ModelType::RS_32)
      {
        outPointCloud.height = 32;
        outPointCloud.width = 12 * (int)lidarMsg->packets.size();
        outPointCloud.is_dense = false; //点云中的数据不都是有限的，包含inf/NaN 值
        outPointCloud.resize(outPointCloud.height * outPointCloud.width);
      }

      int block_counter = 0;

      double scan_timestamp = lidarMsg->header.stamp.toSec();

      for (size_t i = 0; i < lidarMsg->packets.size(); ++i) // lidarMsg->packets.size()：packet总数
      {
        float azimuth; //方位
        float azimuth_diff;
        float last_azimuth_diff = 0;
        float azimuth_corrected_f;
        int azimuth_corrected;
        float x, y, z;

        const raw_packet_t *raw = (const raw_packet_t *)&lidarMsg->packets[i].data[0]; //将第i个packets的data数据（1204个字节）赋值给自定义数据类型raw_packet_t的对象

        for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_counter++) //遍历12个block块
        {
          // Calculate difference between current and next block's azimuth angle.
          //计算当前block和下一block的角度差
          azimuth = (float)(raw->blocks[block].rotation);

          if (block < (BLOCKS_PER_PACKET - 1))
          {
            azimuth_diff = (float)((36000 + raw->blocks[block + 1].rotation - raw->blocks[block].rotation) % 36000);
            last_azimuth_diff = azimuth_diff;
          }
          else
          {
            azimuth_diff = last_azimuth_diff;
          }

          for (int firing = 0, k = 0; firing < FIRINGS_PER_BLOCK; firing++) //一个block有两个firing(直接改为1个)
          {
            for (int dsr = 0; dsr < SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) //每个firing有16个点（改为32）
            {

              /** Position Calculation ，2字节距离值*/
              union two_bytes tmp;
              //RS-32的距离数据前一个字节是高位，后一个字节是低位，与VLP-16相反 ?
              tmp.bytes[0] = raw->blocks[block].data[k + 1];
              tmp.bytes[1] = raw->blocks[block].data[k];

              /** correct for the laser rotation as a function of timing during the firings **/
              azimuth_corrected_f = azimuth + (azimuth_diff * (dsr * DSR_TOFFSET) / BLOCK_TDURATION); //该点的角度值
              //四舍五入并转化为整型2
              azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;

              /*condition added to avoid calculating points which are not
          in the interesting defined area (min_angle < area < max_angle)（添加条件以避免计算不在感兴趣的定义区域内的点）*/
              if ((azimuth_corrected >= m_config.min_angle && azimuth_corrected <= m_config.max_angle && m_config.min_angle < m_config.max_angle) || (m_config.min_angle > m_config.max_angle && (azimuth_corrected <= m_config.max_angle || azimuth_corrected >= m_config.min_angle)))
              {
                // convert polar coordinates to Euclidean XYZ（将极坐标转换为欧几里得 XYZ）
                float distance = tmp.uint * DISTANCE_RESOLUTION; //距离值乘以分辨率 

                float cos_vert_angle = cos_vert_angle_[dsr];
                float sin_vert_angle = sin_vert_angle_[dsr];

                float cos_rot_angle = cos_rot_table_[azimuth_corrected];
                float sin_rot_angle = sin_rot_table_[azimuth_corrected];
                //将极坐标下的角度和距离信息转化为笛卡尔坐标系下的 x,y,z 坐标
                x = distance * cos_vert_angle * sin_rot_angle;
                y = distance * cos_vert_angle * cos_rot_angle;
                z = distance * sin_vert_angle;

                /** Use standard ROS coordinate system (right-hand rule) ——使用标准 ROS 坐标系（右手法则）*/
                float x_coord = y;
                float y_coord = -x;
                float z_coord = z;
                // 反射率
                float intensity = raw->blocks[block].data[k + 2];
                double point_timestamp = scan_timestamp + getExactTime(scan_mapping_32[dsr],block_counter ); //激光点的时间戳
                //定义云点
                TPoint point;
                point.timestamp = point_timestamp;
                //距离满足要求，则将求得的值赋值给云点，否则赋值NAN
                if (pointInRange(distance))
                {
                  point.x = x_coord;
                  point.y = y_coord;
                  point.z = z_coord;
                  point.intensity = intensity;
                }
                else
                {
                  point.x = NAN;
                  point.y = NAN;
                  point.z = NAN;
                  point.intensity = 0;
                }
                //如果为RS_32的激光雷达，赋值给点云
                if (m_modelType == ModelType::RS_32)
                  outPointCloud.at(block_counter , scan_mapping_32[dsr]) = point;
              }
            }
          }
        }
      }
    }

 第二个unpack_scan函数（也就是我实际使用的函数）就比较简单了，它是将PointCloud2格式点云转换为XYZIT格式点云，这一部分当初有点困扰我的是：使用pcl::fromROSMsg()函数将PointCloud2格式点云转换为XYZI格式点云后，点云变为无序的了，所以运行程序一直报错，后来查阅资料修改如下：

    //点云格式转换函数（将PointCloud2格式点云转换为XYZIT格式点云）
    void unpack_scan(const sensor_msgs::PointCloud2::ConstPtr &lidarMsg,
                     TPointCloud &outPointCloud) const
    {
      VPointCloud temp_pc; //XYZI格式点云
      pcl::fromROSMsg(*lidarMsg, temp_pc); //将PointCloud2格式转换为XYZI格式

      outPointCloud.clear();
      outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header);// 时间戳的转换
      outPointCloud.height = 32;
      outPointCloud.width = temp_pc.size() / 32;
      outPointCloud.is_dense = true;
      outPointCloud.resize(outPointCloud.height * outPointCloud.width);

      double timebase = lidarMsg->header.stamp.toSec(); //初始时间
      for (int h = 0; h < outPointCloud.height; h++)
      {
        for (int w = 0; w < outPointCloud.width; w++)
        {
          TPoint point; //XYZIT格式云点
	  pcl::PointXYZI& rs_point =  temp_pc[w + h * temp_pc.size() / 32];

          point.x = rs_point.x;
          point.y = rs_point.y;
          point.z = rs_point.z;
          point.intensity = rs_point.intensity;
          point.timestamp = timebase + getExactTime(h, w); //计算云点时间戳
          outPointCloud.at(w, h) = point;
        }
      }
    }

之后是各种函数。例如获取点的相对时间：

    inline double getExactTime(int dsr, int firing) const
    {
      return mRS32TimeBlock[firing][dsr];
    }

参数初始化（参考激光雷达的用户手册）：

  private:
    void setParameters(ModelType modelType)
    {
      m_modelType = modelType;
      m_config.max_range = 200;
      m_config.min_range = 0.4;
      m_config.min_angle = 0;
      m_config.max_angle = 36000;
      // Set up cached values for sin and cos of all the possible headings
      for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index)
      {
        float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
        cos_rot_table_[rot_index] = cosf(rotation);
        sin_rot_table_[rot_index] = sinf(rotation);
      }

      if (modelType == RS_32)
      {
        FIRINGS_PER_BLOCK = 1;
        SCANS_PER_FIRING = 32;
        BLOCK_TDURATION = 55.52f; // [µs]
        DSR_TOFFSET = 1.44f;       // [µs]
        FIRING_TOFFSET = 55.52f;   // [µs]
        PACKET_TIME = (BLOCKS_PER_PACKET * BLOCK_TDURATION);

        //垂直角度
        float vert_correction[32] = {
            -0.179437300397537023,
            -0.112119951148115732,
            0.040718531449027709,
            0.058171823968971004,
            0.081454516190575361,
            0.12217304763960307,
            0.180344871608574075,
            0.261799387799149436,
            0.005811946409141117,
            0,
            -0.005811946409141117,
            -0.011641346110802178,
            0.029094638630745474,
            0.023265238929084413,
            0.017453292519943295,
            0.011641346110802178,
            -0.436332312998582394,
            -0.255481295906929963,
            -0.138055543832751469,
            -0.0943699526553334,
            -0.064001223670632065,
            -0.069813170079773183,
            -0.0756251164889143,
            -0.081454516190575361,
            -0.040718531449027709,
            -0.046547931150688769,
            -0.052359877559829887,
            -0.058171823968971004,
            -0.017453292519943295,
            -0.023265238929084413,
            -0.029094638630745474,
            -0.034906585039886591};
        //垂直角度对应的cos和sin值
        for (int i = 0; i < 32; i++)
        {
          cos_vert_angle_[i] = std::cos(vert_correction[i]);
          sin_vert_angle_[i] = std::sin(vert_correction[i]);
        }

        scan_mapping_32[0] = 29;
        scan_mapping_32[1] = 27;
        scan_mapping_32[2] = 5;
        scan_mapping_32[3] = 4;
        scan_mapping_32[4] = 3;
        scan_mapping_32[5] = 2;
        scan_mapping_32[6] = 1;
        scan_mapping_32[7] = 0;
        scan_mapping_32[8] = 10;
        scan_mapping_32[9] = 11;
        scan_mapping_32[10] = 12;
        scan_mapping_32[11] = 13;
        scan_mapping_32[12] = 6;
        scan_mapping_32[13] = 7;
        scan_mapping_32[14] = 8;
        scan_mapping_32[15] = 9;
        scan_mapping_32[16] = 31;
        scan_mapping_32[17] = 30;
        scan_mapping_32[18] =28 ;
        scan_mapping_32[19] = 26;
        scan_mapping_32[20] = 22;
        scan_mapping_32[21] = 23;
        scan_mapping_32[22] = 24;
        scan_mapping_32[23] = 25;
        scan_mapping_32[24] = 18;
        scan_mapping_32[25] = 19;
        scan_mapping_32[26] = 20;
        scan_mapping_32[27] = 21;
        scan_mapping_32[28] = 14;
        scan_mapping_32[29] = 15;
        scan_mapping_32[30] = 16;
        scan_mapping_32[31] = 17;

        for (unsigned int w = 0; w < 1800; w++)
        {
          for (unsigned int h = 0; h < 32; h++)
          {
            mRS32TimeBlock[w][h] = h * 1.44 * 1e-6 + w * 55.52 * 1e-6; /// VLP_16 16*1824，(RS-32 32*1800)
          }
        }
      }
    }

各种常量、结构体设置（同样参考用户手册）：

  private:
    static const int RAW_SCAN_SIZE = 3;
    static const int SCANS_PER_BLOCK = 32;
    static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); //一个block中32个点数据（距离+强度）
    constexpr static const float ROTATION_RESOLUTION = 0.01f;             //角度分辨率
    static const uint16_t ROTATION_MAX_UNITS = 36000u;                    //角度最大值36000，除以100，即360
    constexpr static const float DISTANCE_RESOLUTION = 0.005f;            //距离分辨率

    /** @todo make this work for both big and little-endian machines  ? */
    static const uint16_t UPPER_BANK = 0xeeff;
    static const uint16_t LOWER_BANK = 0xddff;

    static const int BLOCKS_PER_PACKET = 12; //一个packet含有12个block
    static const int PACKET_STATUS_SIZE = 2;

    int FIRINGS_PER_BLOCK;
    int SCANS_PER_FIRING;
    float BLOCK_TDURATION;
    float DSR_TOFFSET;
    float FIRING_TOFFSET;
    float PACKET_TIME;

    float sin_rot_table_[ROTATION_MAX_UNITS];
    float cos_rot_table_[ROTATION_MAX_UNITS];
    float cos_vert_angle_[32];
    float sin_vert_angle_[32];
    int scan_mapping_16[16];
    int scan_mapping_32[32];

    typedef struct raw_block
    {
      uint16_t header;               ///< UPPER_BANK or LOWER_BANK（起始标志符）
      uint16_t rotation;             ///< 0-35999, divide by 100 to get degrees（角度值）
      uint8_t data[BLOCK_DATA_SIZE]; //32个点的数据
    } raw_block_t;                   //block数据格式

    union two_bytes
    {
      uint16_t uint;
      uint8_t bytes[2];
    };

    union four_bytes
    {
      uint32_t uint32;
      float_t float32;
    };

    typedef struct raw_packet
    {
      raw_block_t blocks[BLOCKS_PER_PACKET];
      uint32_t revolution;
      uint8_t status[PACKET_STATUS_SIZE];
    } raw_packet_t; //packet数据格式

    /** configuration parameters */
    typedef struct
    {
      double max_range; ///< maximum range to publish
      double min_range;
      int min_angle; ///< minimum angle to publish
      int max_angle; ///< maximum angle to publish
    } Config;
    Config m_config;

    ModelType m_modelType;

    double mRS32TimeBlock[1800][32];

三、修改dataset_reader.h文件

这个文件要改的很少，主要把和激光雷达类型相关代码修改成自己定义的类型就可以了，修改了下面这些部分。

 

这里如果要用第一个unpack_scan函数，需要修改下面代码（还是之前说的那个数据包类型名称的相关修改）:

总结

因为用rosbag录制的包的数据类型是PointCloud2格式，所以我实际上只使用了第二个unpack_scan函数。主要代码修改完后，之后在licalib_gui.launch和calib.sh文件中修改一下imu和激光雷达话题名，录制的rosbag包时间、路径等。

之后运行代码，结果一直初始化失败。找到lidar_IMU_calib/src/core/inertial_initializer.cpp，即初始化程序发现，它与cov(2)值有关，程序设置必须值大于0.25才能初始化成功，而我标定的rosbag包它最开始的值只有零点零几，离0.25差很多，我猜测是因为录制的数据包不太好，因为这个标定程序需要录制的rosbag包有很多条件，例如：需要在平面多的房间里录制、xyz轴方向都需要移动等。因为我们项目rosbag包录制太麻烦了，所以我没有更换rosbag包，而是直接将阈值设置为0.20，然后一直初始化直到它大于0.20（花了一天时间，特别慢）初始化成功，之后按照官网给的步骤逐步运行，最后终于标定成功，结果如下：

 大约看了一下，姿态角度大致和实际相同，把参数发给学长后，学长说应该是正确的。大致步骤就是上面这些，在学长的建议下写下这篇博客，第一次写csdn有什么不足欢迎指出，希望能对各位小伙伴有所帮助。