The Imu used in the example is Lunqu Technology n100 mini. The coordinate system of the data output by imu is based on the ROS coordinate system.
Eigen::Quaterniond q_ahrs(ahrs_frame_.frame.data.data_pack.Qw,
ahrs_frame_.frame.data.data_pack.Qx,
ahrs_frame_.frame.data.data_pack.Qy,
ahrs_frame_.frame.data.data_pack.Qz);
Eigen::Quaterniond q_r =
Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_rr =
Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q_out = q_r * q_ahrs * q_rr
Why do we need to right multiply q_rr?
Answer: You can remove *q_rr. After recompiling, you can see through TF conversion that the coordinates are actually upside down and need to be rotated 180° around the X axis to be the coordinate system used in our ROS.
The coordinate transformation of TF can be completed by just multiplying q_r to the left. However, the coordinate system we usually use in ros is the front left and upper left, so we have to correct the pose of the coordinate system by multiplying q_rr to the right.
Note: This IMU is a nine-axis IMU, integrated with a magnetometer, that is, the original yaw angle refers to north is 0 degrees, and the range is 0-360°, increasing clockwise. After converting to the coordinates under the imu single product ROS standard, the heading should be specially processed as the northeast sky coordinate system.
Input single frame Imu example
seq: 90498
stamp:
secs: 1698127594
nsecs: 155961838
frame_id: "gyro_link"
orientation:
x: 0.0049992394633591695
y: 0.002799155190586991
z: -0.1353550255298614
w: -0.9907805919647217
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.0013670891057699919
y: 0.0030183307826519012
z: -0.002960443962365389
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration:
x: 0.04501450061798096
y: -0.09672745317220688
z: 9.795211791992188
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
imu_integration.h
#ifndef SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
#define SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
#include "eigen_types.h"
#include "imu.h"
#include "nav_state.h"
namespace sad {<!-- -->
/**
* This program demonstration relies solely on the points of the IMU
*/
class IMUIntegration {<!-- -->
public:
IMUIntegration(const Vec3d & amp; gravity, const Vec3d & amp; init_bg, const Vec3d & amp; init_ba)
: gravity_(gravity), bg_(init_bg), ba_(init_ba) {<!-- -->}
// Increase imu reading
void AddIMU(const IMU & amp; imu) {<!-- -->
double dt = imu.timestamp_ - timestamp_;
if (dt > 0 & amp; & amp; dt < 0.1)
{<!-- -->
// Assume that the IMU time interval is within 0 to 0.1
p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;
v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
}
//update time
timestamp_ = imu.timestamp_;
}
/// Compose NavState
NavStated GetNavState() const {<!-- --> return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }
SO3 GetR() const {<!-- --> return R_; }
Vec3d GetV() const {<!-- --> return v_; }
Vec3d GetP() const {<!-- --> return p_; }
private:
// Cumulative amount
SO3 R_;
Vec3d v_ = Vec3d::Zero();
Vec3d p_ = Vec3d::Zero();
double timestamp_ = 0.0;
// Zero offset, set externally
Vec3d bg_ = Vec3d::Zero();
Vec3d ba_ = Vec3d::Zero();
Vec3d gravity_ = Vec3d(0, 0, -9.8); // Gravity
};
} // namespace sad
#endif // SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
static_imu_init.h
#ifndef SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
#define SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
#include "eigen_types.h"
#include "imu.h"
#include "odom.h"
#include <deque>
namespace sad {<!-- -->
/**
* IMU horizontal static state initializer
* Usage: Call AddIMU, AddOdom to add data, and use InitSuccess to get whether the initialization is successful.
* After success, use each Get function to obtain internal parameters
*
* The initializer attempts to initialize the system each time AddIMU is called. When there is odom, initialization requires that the odom wheel speed reading is close to zero; when there is no odom, it is assumed that the vehicle is initially stationary.
* The initializer collects IMU readings over a period of time, estimates the initial zero bias and noise parameters according to section 3.5.4 of the book, and provides them to ESKF or other filters
*/
class StaticIMUInit {<!-- -->
public:
struct Options
{<!-- -->
Options() {<!-- -->}
double init_time_seconds_ = 10.0; // inactivity time
int init_imu_queue_max_size_ = 2000; // Initialize the maximum length of the IMU queue
int static_odom_pulse_ = 5; // Wheel speedometer output noise when stationary
double max_static_gyro_var = 0.5; // Static gyro measurement variance
double max_static_acce_var = 0.05; // Static aggregation measurement variance
double gravity_norm_ = 9.81; // Gravity size
bool use_speed_for_static_checking_ = false; // Whether to use odom to determine if the vehicle is stationary (some data sets do not have the odom option)
};
/// Constructor
StaticIMUInit(Options options = Options()) : options_(options) {<!-- -->}
/// Add IMU data
bool AddIMU(const IMU & amp; imu);
/// Add wheel speed data
bool AddOdom(const Odom & amp; odom);
/// Determine whether initialization is successful
bool InitSuccess() const {<!-- --> return init_success_; }
/// Get each Cov, bias, gravity
Vec3d GetCovGyro() const {<!-- --> return cov_gyro_; }
Vec3d GetCovAcce() const {<!-- --> return cov_acce_; }
Vec3d GetInitBg() const {<!-- --> return init_bg_; }
Vec3d GetInitBa() const {<!-- --> return init_ba_; }
Vec3d GetGravity() const {<!-- --> return gravity_; }
private:
/// Try to initialize the system
bool TryInit();
Options options_; // option information
bool init_success_ = false; // Whether initialization is successful
Vec3d cov_gyro_ = Vec3d::Zero(); // Gyro measurement noise covariance (evaluated during initialization)
Vec3d cov_acce_ = Vec3d::Zero(); // Added measurement noise covariance (evaluated at initialization)
Vec3d init_bg_ = Vec3d::Zero(); // gyro initial zero bias
Vec3d init_ba_ = Vec3d::Zero(); // Add initial zero offset
Vec3d gravity_ = Vec3d::Zero(); // Gravity
bool is_static_ = false; // Indicates whether the vehicle is stationary
std::deque<IMU> init_imu_deque_; // Data for initialization
double current_time_ = 0.0; // current time
double init_start_time_ = 0.0; // Initial time of inactivity
};
} // namespace sad
#endif // SLAM_IN_AUTO_DRIVING_STATIC_IMU_INIT_H
static_imu_init.cc
#include "math_utils.h"
namespace sad {
bool StaticIMUInit::AddIMU(const IMU & amp; imu)
{
if (init_success_)
{
return true;
}
if (options_.use_speed_for_static_checking_ & amp; & amp; !is_static_)
{
LOG(WARNING) << "Wait for the vehicle to be stationary";
init_imu_deque_.clear();
return false;
}
if (init_imu_deque_.empty())
{
//Record initial rest time
init_start_time_ = imu.timestamp_;
}
//Enter into the initialization queue
init_imu_deque_.push_back(imu);
double init_time = imu.timestamp_ - init_start_time_; // Initialization elapsed time
if (init_time > options_.init_time_seconds_)
{
//Try initialization logic
TryInit();
}
// Maintain initial queue length
while (init_imu_deque_.size() > options_.init_imu_queue_max_size_)
{
init_imu_deque_.pop_front();
}
current_time_ = imu.timestamp_;
return false;
}
bool StaticIMUInit::AddOdom(const Odom & amp; odom) {
// Determine whether the vehicle is stationary
if (init_success_) {
return true;
}
if (odom.left_pulse_ < options_.static_odom_pulse_ & amp; & amp; odom.right_pulse_ < options_.static_odom_pulse_) {
is_static_ = true;
} else {
is_static_ = false;
}
current_time_ = odom.timestamp_;
return true;
}
bool StaticIMUInit::TryInit()
{
if (init_imu_deque_.size() < 10) {
return false;
}
// Calculate mean and variance
Vec3d mean_gyro, mean_acce;
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU & amp; imu) { return imu.gyro_; });
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU & amp; imu) { return imu.acce_; });
// Take the ace mean as the direction, and take the length of 9.8 as the gravity
LOG(INFO) << "mean acce: " << mean_acce.transpose();
gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;
// Recalculate the covariance of the summation
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
[this](const IMU & amp; imu) { return imu.acce_ + gravity_; });
// Check IMU noise
if (cov_gyro_.norm() > options_.max_static_gyro_var) {
LOG(ERROR) << "The gyroscope measurement is too noisy" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;
return false;
}
if (cov_acce_.norm() > options_.max_static_acce_var) {
LOG(ERROR) << "The total measurement noise is too noisy" << cov_acce_.norm() << " > " << options_.max_static_acce_var;
return false;
}
// Estimate measurement noise and bias
init_bg_ = mean_gyro;
init_ba_ = mean_acce;
LOG(INFO) << "IMU initialization successful, initialization time = " << current_time_ - init_start_time_ << ", bg = " << init_bg_.transpose()
<< ", ba = " << init_ba_.transpose() << ", gyro sq = " << cov_gyro_.transpose()
<< ", acce sq = " << cov_acce_.transpose() << ", grav = " << gravity_.transpose()
<< ", norm: " << gravity_.norm();
LOG(INFO) << "mean gyro: " << mean_gyro.transpose() << " acce: " << mean_acce.transpose();
init_success_ = true;
return true;
}
} // namespace sad
main.cpp
#include<ros/ros.h>
#include<imu_integration.h>
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include<nav_msgs/Odometry.h>
#include<static_imu_init.h>
// In this experiment, we assume that the zero bias is known
Vec3d gravity(-0.050622,0.102474,-9.809334); // Gravity direction
Vec3d init_bg(-0.005136,-0.000981,-0.003482);
Vec3d init_ba(-0.000118,0.000239,-0.022851);
sad::IMUIntegration imu_integ1(gravity, init_bg, init_ba);
ros::Publisher pub_;
sad::StaticIMUInit imu_init; // Use default configuration
sad::IMU imu_format(const sensor_msgs::Imu & amp;imu_msg)
{<!-- -->
sad::IMU imu;
imu.timestamp_=imu_msg.header.stamp.toSec();
imu.acce_.x()=imu_msg.linear_acceleration.x;
imu.acce_.y()=imu_msg.linear_acceleration.y;
imu.acce_.z()=imu_msg.linear_acceleration.z;
imu.gyro_.x()=imu_msg.angular_velocity.x;
imu.gyro_.y()=imu_msg.angular_velocity.y;
imu.gyro_.z()=imu_msg.angular_velocity.z;
return imu;
}
void IMUCallback(const sensor_msgs::Imu & amp;imu_msg)
{<!-- -->
sad::IMU imu_out=imu_format(imu_msg);
/// IMU processing function
if (!imu_init.InitSuccess())
{<!-- -->
imu_init.AddIMU(imu_out);
return;
}
//imu_init.GetInitBg(), imu_init.GetInitBa(), imu_init.GetGravity()
static bool imu_inited = false;
if(!imu_inited)
{<!-- -->
ROS_INFO("bg:%f,%f,%f",imu_init.GetInitBg().x(),imu_init.GetInitBg().y(),imu_init.GetInitBg().z());
ROS_INFO("ba:%f,%f,%f",imu_init.GetInitBa().x(),imu_init.GetInitBa().y(),imu_init.GetInitBa().z());
ROS_INFO("g:%f,%f,%f",imu_init.GetGravity().x(),imu_init.GetGravity().y(),imu_init.GetGravity().z());
imu_inited=true;
}
imu_integ1.AddIMU(imu_out);
Eigen::Matrix3d R=imu_integ1.GetR().matrix();
nav_msgs::Odometry odom;
odom.header.frame_id="odom";
odom.header.stamp=imu_msg.header.stamp;
odom.child_frame_id="base_link";
odom.pose.pose.position.x=imu_integ1.GetP().x();
odom.pose.pose.position.y=imu_integ1.GetP().y();
odom.pose.pose.position.z=imu_integ1.GetP().z();
Eigen::Quaterniond q;
q=R.block<3,3>(0,0);
odom.pose.pose.orientation.x=q.x();
odom.pose.pose.orientation.y=q.y();
odom.pose.pose.orientation.z=q.z();
odom.pose.pose.orientation.w=q.w();
odom.twist.twist.linear.x=imu_integ1.GetV().x();
odom.twist.twist.linear.y=imu_integ1.GetV().y();
odom.twist.twist.linear.z=imu_integ1.GetV().z();
pub_.publish(odom);
}
int main(int argc, char** argv)
{<!-- -->
ros::init(argc, argv, "imu_integration");
ros::NodeHandle n;
ros::Subscriber sub = n.subscribe("/imu", 100, IMUCallback);
pub_=n.advertise<nav_msgs::Odometry>("/imu_integration_odom",10);
ros::spin();
return 0;
}