Add imu data simulation to support evaluation of the calibration result

CMakeLists.txt

@@ -43,3 +43,7 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EX
 add_executable(allan_variance src/allan_variance.cpp)
 
 target_link_libraries(allan_variance ${PROJECT_NAME} )
+
+add_executable(imu_simulator src/ImuSimulator.cpp)
+
+target_link_libraries(imu_simulator ${PROJECT_NAME} )

config/simulation/imu_simulator.yaml

@@ -0,0 +1,14 @@
+#Accelerometer
+accelerometer_noise_density: 0.0025019929573561175 
+accelerometer_random_walk: 6.972435158192731e-05
+accelerometer_bias_init: 0.007
+
+#Gyroscope
+gyroscope_noise_density: 0.0001888339269965301 
+gyroscope_random_walk: 2.5565313322052523e-06
+gyroscope_bias_init: 0.006
+
+rostopic: '/sensors/imu'
+update_rate: 400.0
+
+sequence_time: 11000

src/ImuSimulator.cpp

@@ -0,0 +1,137 @@
+/**
+ * @file ImuSimulator.cpp
+ * @brief Tool to simulate imu data, ref:
+ * https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model.
+ * @author Rick Liu
+ */
+
+// std, eigen and boost
+#include <boost/filesystem.hpp>
+#include <boost/random.hpp>
+#include <boost/random/normal_distribution.hpp>
+#include <ctime>
+#include <fstream>
+#include <set>
+
+// ROS
+#include <ros/node_handle.h>
+#include <rosbag/bag.h>
+#include <sensor_msgs/Imu.h>
+
+#include "allan_variance_ros/yaml_parsers.hpp"
+
+using Vec3d = Eigen::Vector3d;
+
+Vec3d RandomNormalDistributionVector(double sigma) {
+ static boost::mt19937 rng;
+ static boost::normal_distribution<> nd(0, 1);
+ return {sigma * nd(rng), sigma * nd(rng), sigma * nd(rng)};
+}
+
+template <typename S, typename T> void FillROSVector3d(const S &from, T &to) {
+ to.x = from.x();
+ to.y = from.y();
+ to.z = from.z();
+}
+
+class ImuSimulator {
+public:
+ ImuSimulator(std::string config_file, std::string output_path)
+ : bag_output_(output_path, rosbag::bagmode::Write) {
+ auto yaml_config = loadYamlFile(config_file);
+
+ get(yaml_config, "accelerometer_noise_density",
+ accelerometer_noise_density_);
+ get(yaml_config, "accelerometer_random_walk", accelerometer_random_walk_);
+ get(yaml_config, "accelerometer_bias_init", accelerometer_bias_init_);
+
+ get(yaml_config, "gyroscope_noise_density", gyroscope_noise_density_);
+ get(yaml_config, "gyroscope_random_walk", gyroscope_random_walk_);
+ get(yaml_config, "gyroscope_bias_init", gyroscope_bias_init_);
+
+ get(yaml_config, "rostopic", rostopic_);
+ get(yaml_config, "update_rate", update_rate_);
+ get(yaml_config, "sequence_time", sequence_time_);
+ }
+
+ virtual ~ImuSimulator() { bag_output_.close(); }
+
+ void run() {
+ ROS_INFO_STREAM("Generating imu data ...");
+
+ double dt = 1 / update_rate_;
+
+ // clang-format off
+ ros::Time start_time = ros::Time::now();
+ Vec3d accelerometer_bias = Vec3d::Constant(accelerometer_bias_init_);
+ Vec3d gyroscope_bias = Vec3d::Constant(gyroscope_bias_init_);
+ Vec3d accelerometer_real = Vec3d::Zero();
+ Vec3d gyroscope_real = Vec3d::Zero();
+
+
+ for (int64_t i = 0; i < sequence_time_ * update_rate_; ++i) {
+ // Reference: https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
+ accelerometer_bias += RandomNormalDistributionVector(accelerometer_random_walk_) * sqrt(dt);
+ gyroscope_bias += RandomNormalDistributionVector(gyroscope_random_walk_) * sqrt(dt);
+
+ Vec3d acc_measure = accelerometer_real + accelerometer_bias + RandomNormalDistributionVector(accelerometer_noise_density_) / sqrt(dt);
+ Vec3d gyro_measure = gyroscope_real + gyroscope_bias + RandomNormalDistributionVector(gyroscope_noise_density_) / sqrt(dt);
+
+ sensor_msgs::Imu msg;
+ msg.header.stamp = start_time + ros::Duration(1, 0) * (i / update_rate_);
+ msg.header.seq = i;
+ FillROSVector3d(acc_measure, msg.linear_acceleration);
+ FillROSVector3d(gyro_measure, msg.angular_velocity);
+
+ bag_output_.write(rostopic_, msg.header.stamp, msg);
+ }
+ // clang-format on
+
+ ROS_INFO_STREAM("Finished generating data. ");
+ }
+
+private:
+ // ROS
+ rosbag::Bag bag_output_;
+
+private:
+ double accelerometer_noise_density_;
+ double accelerometer_random_walk_;
+ double accelerometer_bias_init_;
+
+ double gyroscope_noise_density_;
+ double gyroscope_random_walk_;
+ double gyroscope_bias_init_;
+
+ std::string rostopic_;
+ double update_rate_;
+
+ double sequence_time_;
+};
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "allan_variance_ros");
+ ros::NodeHandle nh;
+ std::string rosbag_filename;
+ std::string config_file;
+
+ if (argc >= 2) {
+ rosbag_filename = argv[1];
+ config_file = argv[2];
+ ROS_INFO_STREAM("Bag filename = " << rosbag_filename);
+ ROS_INFO_STREAM("Config File = " << config_file);
+ } else {
+ ROS_WARN("Usage: ./imu_simulator /path/to/output/bag_filename /path/to/simulation/config_filename");
+ return 1;
+ }
+
+ auto start = std::clock();
+
+ ImuSimulator simulator(config_file, rosbag_filename);
+ ROS_INFO_STREAM("Imu simulator constructed");
+ simulator.run();
+
+ double durationTime = (std::clock() - start) / (double)CLOCKS_PER_SEC;
+ ROS_INFO("Total computation time: %f s", durationTime);
+ return 0;
+}