最近在做Lidar和imu的联合标定,即通过算法给出安装完成后Lidar和imu间的相对位姿(平移和旋转)。使用github上的lidar_align标定工具。在参考博文https://blog.csdn.net/miracle629/article/details/87854450后,发现自己录制的数据包中没有lidar_align需要用到的geometry_msgs/TransformStamped数据类型。博文作者给出的方法:1、建议直接使用rosbag方式读入数据,对于不发布TransformStamped类型数据的程序,只要稍微改动数据接口的几处函数(全部在loader.cpp文件中)便可以正常使用。博文下的评论中还提供了另外两种思路:2、自己写一个ros节点,订阅IMU的topic,把message里面的内容,重新按照geometry_msgs/TransformStamped 的格式赋值一遍,重新发布一次topic。3、不转格式,可以存一个csv文件,把6dof姿态存下来。
首先需要安装nlopt非线性优化库,报错:could not find a package configuration file provided by "Nlopt" with any of: NloptConfig.cmake。博文的评论中将 NLOPTConfig.cmake 移到 catkin_ws/src 文件下的方法可行。搜到另外一种方法也可行:即让系统按照Module模式进行查找,把nlopt/build下的NloptConfig.cmake文件重命名为FindNlopt.cmake,在CMakeLists.txt中添加set(CMAKE_MODULE_PATH /home/hjh/nlopt/build)。
尝试方法3,把imu话题保存到imu.csv文件中。运行命令$rostopic echo -b map.bag -p /imu/data >imu.csv ,这里的map.bag是我自己录的rosbag。再改写launch文件中的数据包路径。运行失败。查看csv文件后发现imu没提供x,y,z三个轴上的位置变换。
尝试方法2,订阅sensor_msgs/Imu,处理,发布geometry_msgs/TransformStamped。imu和TransformStamped的消息类型分别如下:
1 #include "ros/ros.h"
2 #include "sensor_msgs/Imu.h"
3 #include "geometry_msgs/TransformStamped.h"
4 #include "std_msgs/Time.h"
5 #include "std_msgs/Float64.h"
6 double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
7 class SubscribeAndPublish
8 {
9 public:
10 SubscribeAndPublish()
11 {
12 pub=n_.advertise<geometry_msgs::TransformStamped>("/geometry_msgs/TransformStamped",1);
13 sub_=n.subscribe("/imu/data",1,&SubscribeAndPublish::callback,this);
14 }
15 void callback(const sensor_msgs::Imu::ConstPtr& input)
16 {
17 geometry_msgs::TransformStamped output;
18 if(firstT){
19 time=input->header.stamp;
20 timeDiff=0;
21 output.transform.rotation.x=input->orientation.x;
22 output.transform.rotation.y=input->orientation.y;
23 output.transform.rotation.z=input->orientation.z;
24 output.transform.rotation.w=input->orientation.w;
25 firstT=false;
26 }
27 else{
28 output.header=input->header;
29
30 output.transform.rotation.x=input->orientation.x;
31 output.transform.rotation.y=input->orientation.y;
32 output.transform.rotation.z=input->orientation.z;
33 output.transform.rotation.w=input->orientation.w;
34
35 timeDiff=(input->header.stamp-time).toSec();
36 time=input->header.stamp;
37
38 velX=velX+input->linear_acceleration.x*timeDiff;
39 velY=velX+input->linear_acceleration.y*timeDiff;
40 velZ=velZ+(input->linear_acceleration.z-9.801)*timeDiff;
41
42 lastShiftX=shiftX;
43 lastShiftY=shiftY;
44 lastShiftZ=shiftZ;
45 shiftX=lastShiftX+velX*timeDiff+input->linear_acceleration.x*timeDiff*timeDiff/2;
46 shiftY=lastShiftY+velY*timeDiff+input->linear_acceleration.y*timeDiff*timeDiff/2;
47 shiftZ=lastShiftZ+velZ*timeDiff+(input->linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
48
49 output.transform.translation.x=lastShiftX+shiftX;
50 output.transform.translation.y=lastShiftY+shiftY;
51 output.transform.translation.z=lastShiftZ+shiftZ;
52 pub_.publish(output);
53 }
54 }
55 private:
56 ros::NodeHandle n_;
57 ros::Publisher pub_;
58 ros::Subscriber sub_;
59 ros::Time time;
60 double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
61 bool firstT=true;
62 };
63 int main(int argc,char **argv)
64 {
65 ros::init(argc,argv,"subscribe_and_publish");
66 SubscribeAndPublish SAP;
67 ros::spin();
68 return 0;
69 }
编译通过后再次运行失败。报错:No odom messages found. Error loading transforms from ROS bag.
尝试方法1:看源码。发现数据都应该从 bag中导入,前两种方法方向错了。修改loader.cpp文件
1 std::vector<std::string> types;
2 types.push_back(std::string("sensor_msgs/Imu"));
3 rosbag::View view(bag, rosbag::TypeQuery(types));
4 size_t imu_num = 0;
5 double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
6 ros::Time time;
7 double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
8 for (const rosbag::MessageInstance& m : view){
9 std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;
10
11 sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());
12
13 Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
14 if(imu_num==1){
15 time=imu.header.stamp;
16 Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));
17 odom->addTransformData(stamp, T);
18 }
19 else{
20 timeDiff=(imu.header.stamp-time).toSec();
21 time=imu.header.stamp;
22 velX=velX+imu.linear_acceleration.x*timeDiff;
23 velY=velX+imu.linear_acceleration.y*timeDiff;
24 velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;
25
26 lastShiftX=shiftX;
27 lastShiftY=shiftY;
28 lastShiftZ=shiftZ;
29 shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;
30 shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;
31 shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
32
33 Transform T(Transform::Translation(shiftX,shiftY,shiftZ),
34 Transform::Rotation(imu.orientation.w,
35 imu.orientation.x,
36 imu.orientation.y,
37 imu.orientation.z));
38 odom->addTransformData(stamp, T);
39 }
40 }
编译运行成功,输出变换矩阵。以后处理rosbag时可以参考loader.cpp的方法,作者的代码功力挺强。
如果要提高标定的精度需要做的改进:录制变化幅度较大的rosbag。
做这个做了将近一个礼拜,记录些心得体会。1、传感器方面,对于imu加深了理解。2、rosbag和ros中消息的订阅与发布。3、提高C++编程能力果然还是得靠手撕源码的能力!