最近在做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++編程能力果然還是得靠手撕源碼的能力！