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

wishchin

已于 2023-06-17 10:39:46 修改

阅读量1.3k

点赞数 3

分类专栏： Cartogropher1.0和2.0笔记三维重建/SLAM 文章标签：数学建模 java 开发语言

于 2023-06-16 12:00:42 首次发布

本文链接：https://blog.csdn.net/wishchin/article/details/131244285

版权

三维重建/SLAM 同时被 2 个专栏收录

103 篇文章 29 订阅

订阅专栏

Cartogropher1.0和2.0笔记

17 篇文章 1 订阅

订阅专栏

使用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 <sensor_msgs/Imu.h>

//找到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<geometry_msgs::TransformStamped>());

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<sensor_msgs::Imu>());
       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（把录的包路径和名称添加进去）