imu内参标定，从数据录制到标定结果过程记录

一、准备录制

这部分内容主要参考了博主 huawwenwnewn 的文章http://t.csdn.cn/yvkdz，非常感谢！

1、建立工作空间在imu_data.cpp 写入以下内容

mkdir -p publish_ws/src
cd publish_ws/src
catkin_create_pkg imu_publish std_msgs rospy roscpp
cd imu_publish/src
vim imu_data.cpp 

在imu_data.cpp 写入以下内容

// Step 1:  Include Library Headers:
//发布imu的数据到IMU_data
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>

int main(int argc, char** argv)
{
    // Step 2: Initialization:
       ros::init(argc, argv, "imu");
    ros::NodeHandle n;

    ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("IMU_data", 20);
    ros::Rate loop_rate(50);
    while(ros::ok())
    {

            sensor_msgs::Imu imu_data;
            imu_data.header.stamp = ros::Time::now();
            imu_data.header.frame_id = "base_link";
            //四元数位姿,所有数据设为固定值，可以自己写代码获取ＩＭＵ的数据，，然后进行传递
           imu_data.orientation.x = 0;
            imu_data.orientation.y = -1;
            imu_data.orientation.z = -5;
            imu_data.orientation.w = 6;
            //线加速度
            imu_data.linear_acceleration.x = 0.01;
            imu_data.linear_acceleration.y = 0.02;
            imu_data.linear_acceleration.z = 0.03;
            //角速度
            imu_data.angular_velocity.x = 0.05;
            imu_data.angular_velocity.y = 0.06;
            imu_data.angular_velocity.z = 0.07;



            IMU_pub.publish(imu_data);



        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

改一下CMakeLists.txt

cd imu_publish
vim CMakeLists.txt

 添加以下内容

add_executable(imu_data src/imu_data.cpp)
target_link_libraries(imu_data ${catkin_LIBRARIES})

 如图所示

 回到工作空间下编译

2、硬件连接

将imu连接到电脑上，终端运行

ls -l /dev/ttyUSB*  //确定连接成功
sudo chmod 777 /dev/ttyUSB0  //打开串口权限

二、开始录制

终端1：运行roscore

终端2：发布imu数据

注意，这里要source一下，确定ROS_PACKAGE_PATH是在当前工作空间下。如图：

终端3：

rosbag record /IMU_data

等待两小时......然后ctr+C停止录制。注意，这里的 /IMU_data 是自己的话题名称，不要写错了，如果不知道是什么，看看前面编写的 imu_data.cpp 里面的 publisher 那一行。

若要查看录制包的信息：另开终端运行 

rosbag info 包名

 若要查看话题信息，终端4：rostopic echo /IMU_data  

若想可视化，终端5，运行命令rviz，在可视化界面点击ADD，选中imu_plugin底下的imu，点击ok。左侧Fixed Frame那里，填上对应的名称（终端4里有）。imu底下的topic那里，点击小三角，会出现，选中即可。最后会出现个坐标系，如图：

 三、配置imu标定的环境

这部分主要参考了博主Yudel Koh 的文章RealSense D435i + imu 标定 Ros Melodic_rs_camera.launch_Yudel Koh的博客-CSDN博客

1、安装ceres

2、创建imu标定的工作空间

mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
catkin_init_workspace
cd ~/imu_catkin_ws
catkin_make
source ~/imu_catkin_ws/devel/setup.bash

 3、下载code_utils并编译（注意这里一定要先下载编译完code_utils，再下载编译imu_utils）

cd ~/imu_catkin_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ~/imu_catkin_ws
catkin_make

如果报错看看上面的参考文章。

4、下载imu_utils并编译

cd ~/imu_catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ~/imu_catkin_ws
catkin_make 

5、编写标定参数文件

cd ~/imu_catkin_ws/src/imu_utils/launch
touch imu_calibration.launch
gedit imu_calibration.launch

在imu_calibration.launch文件里写入以下内容：

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/IMU_data"/>
        <param name="imu_name" type="string" value= "wit_imu"/>  //这里是imu的名字，自己随便取
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>  //单位：分钟，不要长于自己的录制时间
        <param name="max_cluster" type="int" value= "100"/>
    </node>



</launch>

（添加该文件之后不需要重新编译工作空间）

环境配置准备工作至此完成

四、开始标定imu内参

录制包默认放在主目录下，需将它拷到imu_catkin_ws 空间下，并改名为 imu_calibration.bag（如果在录制时制定存放位置和包名，这一步可以其实可以省掉，但是我没有用这个命令因为命令格式没写对）

终端1：

cd imu_catkin_ws
roslaunch imu_utils imu_calibration.launch

  终端2：

cd imu_catkin_ws
rosbag play -r 200 imu_calibration.bag  //以200倍的速度播放该文件 

标定结果，：文件wit_imu_imu_param.yaml

文件wit_imu_imu_param.yaml里的内容：

 至此，完成。