lego-loam加入imu数据建图,使用自己的数据集建图

配置lego-loam教程:

https://blog.csdn.net/qq_35102059/article/details/122671432?spm=1001.2014.3001.5501

激光雷达与imu的外参标定教程:

https://blog.csdn.net/qq_35102059/article/details/122577644?spm=1001.2014.3001.5501

lego-loam默认可以没有imu信息,但有imu建图效果会更好

配置跑通lego-loam后,进行外参标定,得到标定结果.

开始修改程序参数:

utility.h文件:

确认utility.h文件中imu的话题名称是否正确:

extern const string imuTopic = "/imu/data";

featureAssociation.cpp文件:

1. 私有成员加入3个转换变量:

class FeatureAssociation{

private:
    ...
    ...
    ...
    Eigen::Matrix3d extRot;
    Eigen::Matrix3d extRPY;
    Eigen::Quaterniond extQRPY;
public:
    ...

2.nh("~")函数中加入标定好的外参旋转矩阵:

nh("~")
        {
			// extRot << 1, 0, 0,
			// 					0, -1, 0,
			// 					0, 0, -1;
			// extRPY << 1, 0, 0,
			// 					0, -1, 0,
			// 					0, 0, -1;
            extRot <<     -0.2424,   -0.937885,   -0.248224,
                                -0.44903,   -0.118344,    0.885645, 
                                -0.860008,     0.32614,   -0.392452;
            extRPY <<     -0.2424,   -0.937885,   -0.248224,
                                -0.44903,   -0.118344,    0.885645, 
                                -0.860008,     0.32614,   -0.392452;
			extQRPY = Eigen::Quaterniond(extRPY);

        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
        subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
        subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);

        pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
        pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
        pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
        pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);

        pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
        pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
        pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
        pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
        
        initializationValue();
}

3.紧跟着下面写一个外参转换函数:

sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
    sensor_msgs::Imu imu_out = imu_in;
    // rotate acceleration
    Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
    acc = extRot * acc;
    imu_out.linear_acceleration.x = acc.x();
    imu_out.linear_acceleration.y = acc.y();
    imu_out.linear_acceleration.z = acc.z();
    // rotate gyroscope
    Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
    gyr = extRot * gyr;
    imu_out.angular_velocity.x = gyr.x();
    imu_out.angular_velocity.y = gyr.y();
    imu_out.angular_velocity.z = gyr.z();
    // rotate roll pitch yaw
    Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
    Eigen::Quaterniond q_final = q_from * extQRPY;
    imu_out.orientation.x = q_final.x();
    imu_out.orientation.y = q_final.y();
    imu_out.orientation.z = q_final.z();
    imu_out.orientation.w = q_final.w();

    if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
    {
        ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
        ros::shutdown();
    }

    return imu_out;
}

4.在imu的回调函数第一行加入调用转换函数,完成转换:

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
        sensor_msgs::Imu thisImu = imuConverter(*imuIn);  //就加这1行

    double roll, pitch, yaw;
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(thisImu.orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

注:

1.此外参为imu与激光雷达的标定结果中的旋转矩阵.

2.lego-loam中只考虑了imu到激光雷达的旋转,没有考虑平移(个人理解是,与小车的运动模型有关),故应该尽量将imu安装在激光雷达的正下方.

保存重新catkin_make编译.即可开始运行程序建图: