Lidar-Imu联合标定工具lidar_align的数据接口改写

2020/08/08 15:12
阅读数 1.7W

  最近在做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++编程能力果然还是得靠手撕源码的能力!

 

 

 

 

 

展开阅读全文
加载中
点击引领话题📣 发布并加入讨论🔥
打赏
0 评论
0 收藏
0
分享
返回顶部
顶部