使用lidar-imu外参标定工具lidar_align

使用lidar-imu外参标定工具lidar_align，需要一个合适的激光可检测明显特征点的检测环境。

使用imu_utils 测量结果：零偏  随机噪音；使用kalibra进行视觉和imu的联合标定

方法：使用lidar_align

1.sudo apt-get install libnlopt-dev

2.下载并安装lidar_align

    mkdir  -p  lidar_align/src

    cd lidar_align/src

    git clone https://github.com/ethz-asl/lidar_align.git

将lidar_align/src/lidar_align/NLOPTConfig.cmake文件移动到lidar_align/src/下(与lidar_align同级)

catkin_make

3.若提示error: conflicting declaration ‘typedef struct LZ4_stream_t LZ4_stream_t’ typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t

alt+ctrl+t新开终端处理定义冲突问题（不提示error则跳过此步） ，之后回到上个终端重新编译

sudo mv /usr/include/flann/ext/lz4.h /usr/include/flann/ext/lz4.h.bak

sudo mv /usr/include/flann/ext/lz4hc.h /usr/include/flann/ext/lz4.h.bak

sudo ln -s /usr/include/lz4.h /usr/include/flann/ext/lz4.h

sudo ln -s /usr/include/lz4hc.h /usr/include/flann/ext/lz4hc.h

catkin_make

编译成功！

4.改写imu接口（lidar_align原来不是用于标定lidar与 imu的这里需要改写imu接口）

打开lidar_align/src/lidar_align/src/loader.cpp在开头添加个头文件，注释掉odom部分，添加一部分

    //在loader.cpp文件的开头添加头文件

    #include

    //找到odom部分注释掉，如下

    /*  types.push_back(std::string("geometry_msgs/TransformStamped"));

      rosbag::View view(bag, rosbag::TypeQuery(types));

      size_t tform_num = 0;

      for (const rosbag::MessageInstance& m : view) {

        std::cout << " Loading transform: \e[1m" << tform_num++

                  << "\e[0m from ros bag" << '\r' << std::flush;

        geometry_msgs::TransformStamped transform_msg =

            *(m.instantiate());

        Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +

                          transform_msg.header.stamp.nsec / 1000ll;

        Transform T(Transform::Translation(transform_msg.transform.translation.x,

                                           transform_msg.transform.translation.y,

                                           transform_msg.transform.translation.z),

                    Transform::Rotation(transform_msg.transform.rotation.w,

                                        transform_msg.transform.rotation.x,

                                        transform_msg.transform.rotation.y,

                                        transform_msg.transform.rotation.z));

        odom->addTransformData(stamp, T);

      }

    */

     

    //在注释的位置粘贴如下代码

     

     types.push_back(std::string("sensor_msgs/Imu"));
     rosbag::View view(bag, rosbag::TypeQuery(types));
     size_t imu_num = 0;
     double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
     ros::Time time;
     double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
     for (const rosbag::MessageInstance& m : view){
       std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;
       sensor_msgs::Imu imu=*(m.instantiate());
       Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
       if(imu_num==1){
      time=imu.header.stamp;
      Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));
      odom->addTransformData(stamp, T);
      }
      else{
      timeDiff=(imu.header.stamp-time).toSec();
      time=imu.header.stamp;
      velX=velX+imu.linear_acceleration.x*timeDiff;
      velY=velX+imu.linear_acceleration.y*timeDiff;
      velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;

      lastShiftX=shiftX;
      lastShiftY=shiftY;
      lastShiftZ=shiftZ;
      shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;
      shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;
      shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
      Transform T(Transform::Translation(shiftX,shiftY,shiftZ),
     Transform::Rotation(imu.orientation.w,
      imu.orientation.x,
      imu.orientation.y,
      imu.orientation.z));
      odom->addTransformData(stamp, T);
      }
     }

5.catkin_make  编译完成

6.录制lidar和imu数据包用于标定（这里先测试能否正常标定随便录个两分钟的bag，后面实战的时候需要让小车在开阔有参照物的场景录两分钟来回转向和直行运动）

给lidar和imu上电运行，在home/下新建文件夹命名testbag，进入testbag右键新开终端

    // rosbag record -大写欧（小写o你的包名会自动加时间日期） 包名 要录的话题1 要录的话题2

    rosbag record -O lidarimu.bag /imu/data /rslidar_points

修改为我自己的

    rosbag record -O lidarimu.bag /imu/data  /rslidar_points

    //两分钟左右ctrl+c结束录制，包会自动保存在打开终端的testbag文件夹

使用数据：~/data_imu/2023-06-08-15-05-52-calibra-rerecord.bag

         ~/data_imu/lidarimu.bag   

rviz查看，貌似激光数据在imu_link下还比较稳定，可以作为标定数据；应该使用比较平稳的地面

7、修改launch文件lidar_align.launch（把录的包路径和名称添加进去）

  

修改为自己的bag

  

注意：imu的topic？？？

8.开始标定（在lidar_align工作空间下，标定过程大概二十多分钟）

需要修改topic

    source devel/setup.bash

    roslaunch lidar_align lidar_align.launch

问题：需要使用点云数据，重新录制！

[1686209913.596894625]: Loading Pointcloud Data...

[ERROR] [1686209913.667998801]: No points were loaded, verify that the bag contains populated messages of type sensor_msgs/PointCloud2

暂时修改程序：

把bluesea2输出点云topic : rslidar_points；

把xb输出的imu的topic /xbimu修改为/imu/data   使用remap也可以

标定结果保存在 lidar_align/src/lidar_align/results下以包名命名的一个txt文件一个ply文件

9.结果分析  imu到laser的位姿变换（pose传感器到 Lidar Frame）

比如：

Active Translation Vector (x,y,z) from the Pose Sensor Frame to  the Lidar Frame:

[-0.0197469, 0.0453332, 8.05059e-06]

可以直接使用向量变换或者斯元数的位姿变换；所以优先使用标定数据，urdf中的作为初始化就可以了

标定结果

    标定结果惨不忍睹，可能因为采集的数据太差。标定得来的数据比手动测量的看上去误差还大。