Ubuntu20.04安装和编译运行lidar_align来联合标定lidar与imu的外参

news2024/11/24 13:56:20

硬件：树霉派4b

1、下载并安装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同级)

NLOPTConfig.cmake 文件移动前的位置：

NLOPTConfig.cmake 文件移动后的位置：

然后运行：

cd ..
catkin_make

（a）运行后，出现了错误： 没有找到pcl_ros包，因此需要安装这个包。可以查看我的另一篇文章ubuntu20.04中运行lego_slam算法-CSDN博客

(b)安装pcl_ros包后，删除编译生成的build、devel文件夹，重新 catkin_make 编译。这次编译可能会出现下面的错误：

In file included from /usr/include/pcl-1.10/pcl/pcl_macros.h:77,
                 from /usr/include/pcl-1.10/pcl/PCLHeader.h:10,
                 from /usr/include/pcl-1.10/pcl/point_cloud.h:47,
                 from /usr/include/pcl-1.10/pcl/common/transforms.h:42,
                 from /home/ubuntu/ccy/lidar_align/src/lidar_align/include/lidar_align/sensors.h:6,
                 from /home/ubuntu/ccy/lidar_align/src/lidar_align/src/sensors.cpp:1:
/usr/include/pcl-1.10/pcl/pcl_config.h:7:4: error: #error PCL requires C++14 or above
    7 |   #error PCL requires C++14 or above
      |    ^~~~~
In file included from /usr/include/pcl-1.10/pcl/console/print.h:44,
                 from /usr/include/pcl-1.10/pcl/conversions.h:53,
                 from /usr/include/pcl-1.10/pcl/common/impl/centroid.hpp:45,
                 from /usr/include/pcl-1.10/pcl/common/centroid.h:1098,
                 from /usr/include/pcl-1.10/pcl/common/transforms.h:44,
                 from /home/ubuntu/ccy/lidar_align/src/lidar_align/include/lidar_align/sensors.h:6,
                 from /home/ubuntu/ccy/lidar_align/src/lidar_align/src/sensors.cpp:1:
/usr/include/pcl-1.10/pcl/pcl_config.h:7:4: error: #error PCL requires C++14 or above
    7 |   #error PCL requires C++14 or above

从错误信息来看，PCL（Point Cloud Library）库要求使用C++14或更高版本的C++标准。编译环境似乎没有设置为使用C++14或更高版本。解决方法：

（1）打开你的lidar_align项目的CMakeLists.txt文件。

（2）在文件顶部添加下面两行CMake命令来设置C++标准。

set(CMAKE_CXX_STANDARD 14)  
set(CMAKE_CXX_STANDARD_REQUIRED True)

（3）保存CMakeLists.txt文件。

（4）删除build目录，然后重新运行catkin_make构建项目。

(c)编译之后，会出现下面的error:

解决方案：安装nlopt

git clone http://github.com/stevengj/nlopt
cd nlopt/
mkdir build
cd build
cmake ..
make
sudo make install

那么在 /usr/local/lib/cmake 目录下出现 nlopt 文件。

然后，在 lidar_align 工程目录下，并在CMakeLists.txt里加上以下内容：

list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_SOURCE_DIR})
set (CMAKE_PREFIX_PATH "/usr/local/lib/cmake/nlopt")

以上操作结束后，回到 lidar_align工程中，删除build、devel后，重新 catkin_make

会出现警告，暂时先不管

2、lidar、imu分别上电，分别接入树霉派4B

2.1 天眸lidar的运行参考：获取天眸激光雷达点云图-CSDN博客

2.2 imu的运行：在 start_imu.sh 路径下运行以下命令：

./start_imu.sh

启动imu和lidar后，查看ros话题：

分别输出 /pa_gs02/imu/data 和 /timoo_points 话题数据：

rostopic echo /timoo_points
rostopic echo /pa_gs02/imu/data

2.3 录制数据包

录制的话题包括： /pa_gs02/imu/data 、 /timoo_points

新建 testbag 文件夹，在该文件夹下打开terminal,运行

//rosbag record -大写欧（小写o你的包名会自动加时间日期） 包名 要录的话题1 要录的话题2
 
rosbag record -O lidarimu.bag  /pa_gs02/imu/data  /timoo_points
 
//两分钟左右ctrl+c结束录制，包会自动保存在打开终端的testbag文件夹

录制完成后，

修改bag路径：

打开 /home/ubuntu/ccy/lidar_align/src/lidar_align/launch/lidar_align.launch ，修改 <arg name="bag_file" default="/PATH/TO/YOUR.bag"/> ，将 default="/PATH/TO/YOUR.bag"/> 替换为lidarimu.bag包的路径 /home/ubuntu/ccy/testbag/lidarimu.bag 。

修改 loader.cpp文件

首先在该文件顶部添加 #include <sensor_msgs/Imu.h，然后这个工具包原先是用来标定lidar和odom(里程计)，所以需要将里程计接口替换为imu接口：

替换前：

将上图中标记部分的代码注释掉，在注释之后位置添加：

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);
    }
  }

替换后：

替换后，删除lidar_align工程下的 build、devel文件夹，再使用 catkin_make编译（过程中出现warning可以先不用管）

3 、标定

进入lidar_align工程，打开terminal，运行以下命令：

source devel/setup.bash 
roslaunch lidar_align lidar_align.launch

开始标定迭代过程，上图中的Iteration表示迭代次数。

标定结束：

标定好之后会在终端显示出标定结果，也会在 result 文件夹下面生成对应的标定文件。

一个从姿态传感器imu坐标系到激光雷达坐标系的转换关系，包括平移向量、旋转矩阵、四元数和时间偏移。这些信息对于在ROS中正确对齐激光雷达数据至关重要。

平移向量：
-0.0107452, 0.0120904, 0.00375917

这表示从姿态传感器坐标系到激光雷达坐标系在x、y、z轴上的平移量。
旋转矩阵：

这是一个3x3的矩阵，描述了从姿态传感器坐标系到激光雷达坐标系的旋转关系。
四元数：
[0.0131817, 0.00623436, 2.43504e-05, -0.999894]

四元数提供了一种紧凑且不易出现奇异值的方式来描述三维旋转。
时间偏移：
-0.05237

这表示在将激光雷达的时间戳用于姿态估计或对齐时，需要从这个时间戳中减去0.05237秒。