Capturing Intel RealSense IMU data for calibration

[Mahesha999]

I am trying to calibrate IMU from Intel Realsense. Kalibr requires input in rosbag. We can create rosbag from csv as specified here using bagcreater tool. However bagcreater requires input csv in following format:

timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z
1385030208736607488,0.5,-0.2,-0.1,8.1,-1.9,-3.3
 ...
1386030208736607488,0.5,-0.1,-0.1,8.1,-1.9,-3.3

I was thinking how can I get csv in this format. First task was to log IMU data. I used Intel realsense-viewer to record IMU data. I run it for few seconds to quickly get the rosbag it created for analysis. The rosbag contained many topics (corresponding to each data stream), out of which following two seems to of interest:

 /device_0/sensor_2/Accel_0/imu/data                    1269 msgs    : sensor_msgs/Imu  
 /device_0/sensor_2/Gyro_0/imu/data                     2528 msgs    : sensor_msgs/Imu             

I was planning to extract these topic using bagextractor and then merge them and the create a new bag out of them using bagcreater in the format required by kalibr. But, as you can see they contain different number of messages. Gyro logged 2528 messages whereas accel logged 1269 messages. How can I merge them correctly? Or is there different tool that I should be using to capture Intel RealSense IMU data in the rosbag format required by Kalibr? Or should I be using the realsense API to log both gyro and accel for each timestamp?

PS: First few rows of CSVs exported from these two topics looks as follows:

/device_0/sensor_2/Accel_0/imu/data

timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z
1703886880345048064,-0.6841690540313721,2.3858649730682373,-0.07504915446043015,0.0,0.0,0.0
1703886880350096640,-0.39618974924087524,2.5638885498046875,-0.2687807083129883,0.0,0.0,0.0
1703886880355147008,-0.05235987901687622,2.717477560043335,-0.45902159810066223,0.0,0.0,0.0

/device_0/sensor_2/Gyro_0/imu/data

timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z
1703886880343395072,0.0,0.0,0.0,-3.5696206092834473,6.531228542327881,7.00194787979126
1703886880353453824,0.0,0.0,0.0,-3.7657535076141357,6.1095428466796875,6.158576011657715
1703886880363513088,0.0,0.0,0.0,-3.255807638168335,6.050703048706055,5.776116847991943

Notice how gyro data contains all zeroes for accel columns and vice versa.

[goldbattle]

You should be able to specify the driver to combine the two topics via linear interpolation. Otherwise, you will need to handle the async nature of the two and recover a single csv with both columns filled out.

I would recommend just recording the bag directly instead of trying to use the conversion utilities since you should be able to launch realsense over ros itself.

On Sat, Dec 30, 2023 at 2:54 AM Mahesha999 @.***> wrote:

I am trying to calibrate IMU from Intel Realsense. Kalibr requires input in rosbag. We can create rosbag from csv as specified here https://github.com/ethz-asl/kalibr/wiki/bag-format using bagcreater tool. However bagcreater requires input csv in following format:

timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z 1385030208736607488,0.5,-0.2,-0.1,8.1,-1.9,-3.3 ... 1386030208736607488,0.5,-0.1,-0.1,8.1,-1.9,-3.3

I was thinking how can I get csv in this format. First task was to log IMU data. I used Intel realsense-viewer to record IMU data. I run it for few seconds to quickly get the rosbag it created for analysis. The rosbag contained many topics (corresponding to each data stream), out of which following two seems to of interest:

/device_0/sensor_2/Accel_0/imu/data 1269 msgs : sensor_msgs/Imu /device_0/sensor_2/Gyro_0/imu/data 2528 msgs : sensor_msgs/Imu

I was planning to extract these topic using bagextractor https://github.com/ethz-asl/kalibr/wiki/bag-format#bagextractor and then merge them and the create a new bag out of them using bagcreater https://github.com/ethz-asl/kalibr/wiki/bag-format#bagcreater in the format required by kalibr. But, as you can see they contain different number of messages. Gyro logged 2528 messages whereas accel logged 1269 messages. How can I merge them correctly? Or is there different tool that I should be using to capture Intel RealSense IMU data in the rosbag format required by Kalibr? Or should I be using the [realsense API] ( https://github.com/IntelRealSense/librealsense/blob/master/doc/d435i.md#api) to log both gyro and accel for each timestamp?

PS: First few rows of CSVs exported from these two topics looks as follows:

/device_0/sensor_2/Accel_0/imu/data

timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z 1703886880345048064,-0.6841690540313721,2.3858649730682373,-0.07504915446043015,0.0,0.0,0.0 1703886880350096640,-0.39618974924087524,2.5638885498046875,-0.2687807083129883,0.0,0.0,0.0 1703886880355147008,-0.05235987901687622,2.717477560043335,-0.45902159810066223,0.0,0.0,0.0

/device_0/sensor_2/Gyro_0/imu/data

timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z 1703886880343395072,0.0,0.0,0.0,-3.5696206092834473,6.531228542327881,7.00194787979126 1703886880353453824,0.0,0.0,0.0,-3.7657535076141357,6.1095428466796875,6.158576011657715 1703886880363513088,0.0,0.0,0.0,-3.255807638168335,6.050703048706055,5.776116847991943

Notice how gyro data contains all zeroes for accel columns and vice versa.

— Reply to this email directly, view it on GitHub https://github.com/ethz-asl/kalibr/issues/661, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYXKQGYPHVRJXN2KIX3YL7XF3AVCNFSM6AAAAABBHOC4S6VHI2DSMVQWIX3LMV43ASLTON2WKOZSGA3DANZRGIYTOMQ . You are receiving this because you are subscribed to this thread.Message ID: @.***>

[Mahesha999]

I ended up writing basic CPP script to dump the data in desired CSV format:

#include <librealsense2/rs.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>

int main() {
    rs2::pipeline pipeline;
    rs2::config config;
    config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F, 63);
    config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F, 200);

    pipeline.start(config);

    // Open a CSV file to write IMU data
    std::ofstream file("IMU_Data.csv");
    if (!file.is_open()) {
        std::cerr << "Failed to open file for writing!" << std::endl;
        return EXIT_FAILURE;
    }

    // Write header to the CSV file
    file << "timestamp,omega_x,omega_y,omega_z,alpha_x,alpha_y,alpha_z\n";

    long long prev_timestamp = 0;

    try {
        while (true) {
            // Wait for a coherent pair of frames: accelerometer and gyroscope
            rs2::frameset frames = pipeline.wait_for_frames();
            rs2::motion_frame accel_frame = frames.first_or_default(RS2_STREAM_ACCEL);
            rs2::motion_frame gyro_frame = frames.first_or_default(RS2_STREAM_GYRO);

            if (accel_frame && gyro_frame) {
                rs2_vector accel_data = accel_frame.get_motion_data();
                rs2_vector gyro_data = gyro_frame.get_motion_data();

                // Write IMU data to CSV file
                file << std::fixed << std::setprecision(0) << accel_frame.get_timestamp() * 1e6f << ",";
                file << std::fixed << std::setprecision(16) 
                    << gyro_data.x << "," << gyro_data.y << "," << gyro_data.z << "," 
                    << accel_data.x << "," << accel_data.y << "," << accel_data.z << std::endl;  
            }
        }
    } catch (const rs2::error &e) {
        std::cerr << "RealSense error: " << e.what() << std::endl;
    } catch (const std::exception &e) {
        std::cerr << "Exception: " << e.what() << std::endl;
    } catch (...) {
        std::cerr << "Unknown exception occurred!" << std::endl;
    }

    pipeline.stop();
    return EXIT_SUCCESS;
}

I then obtained rosbag:

# rosrun kalibr kalibr_bagcreater --folder ./data/imu_data --output-bag ./data/imu-rosbag.bag

Cooked the bag with cookbag utility:

# rosrun allan_variance_ros cookbag.py --input /data/imu_data/imu-rosbag.bag --output /data/imu_data/imu-bag.bag 

moved imu-rosbag.bag out of the folder and then tried running allan variance:

# rosrun allan_variance_ros allan_variance /data/imu_data /data/allan_variance/imu_configuration.yaml

Then run analysis:

# rosrun allan_variance_ros analysis.py --data /data/imu_data/allan_variance.csv --config /data/allan_variance/imu_configuration.yaml
/catkin_ws/src/allan_variance_ros/scripts/analysis.py:23: RuntimeWarning: divide by zero encountered in log
  logy = np.log(y)
Traceback (most recent call last):
  File "/catkin_ws/src/allan_variance_ros/scripts/analysis.py", line 101, in <module>
    accel_wn_intercept_x, xfit_wn = get_intercept(period[0:white_noise_break_point], acceleration[0:white_noise_break_point,0], -0.5, 1.0)
  File "/catkin_ws/src/allan_variance_ros/scripts/analysis.py", line 24, in get_intercept
    coeffs, _ = curve_fit(line_func, logx, logy, bounds=([m, -np.inf], [m + 0.001, np.inf]))
  File "/usr/lib/python3/dist-packages/scipy/optimize/minpack.py", line 708, in curve_fit
    ydata = np.asarray_chkfinite(ydata, float)
  File "/usr/lib/python3/dist-packages/numpy/lib/function_base.py", line 495, in asarray_chkfinite
    raise ValueError(
ValueError: array must not contain infs or NaNs

It seems that there are some NaNs in allan_variance.csv. Now, am guessing what I missed?

[goldbattle]

It should be this unite_imu_method method in the driver: https://github.com/IntelRealSense/realsense-ros?tab=readme-ov-file#parameters https://github.com/rpng/ar_table_dataset/blob/master/d455_drivers/launch/d455_color.launch#L49

[Mahesha999]

Seems that unite_imu_method is correct direction. Need to check. Meanwhile it worked when I captured data for 20+ hours.