Default IMU Extrinsics

cmake/Depthai/DepthaiDeviceSideConfig.cmake

@@ -2,7 +2,7 @@
 set(DEPTHAI_DEVICE_SIDE_MATURITY "snapshot")
 
 # "full commit hash of device side binary"
-set(DEPTHAI_DEVICE_SIDE_COMMIT "e106a257e5c41a1167df769e53d6e2512a5a26d9")
+set(DEPTHAI_DEVICE_SIDE_COMMIT "5c28512d649be4a8b6b359858d385ff5c228c61e")
 
 # "version if applicable"
 set(DEPTHAI_DEVICE_SIDE_VERSION "")

examples/CMakeLists.txt

@@ -289,6 +289,8 @@ dai_add_example(warp_mesh Warp/warp_mesh.cpp ON OFF)
 # IMU
 dai_add_example(imu_gyroscope_accelerometer IMU/imu_gyroscope_accelerometer.cpp ON OFF)
 dai_add_example(imu_rotation_vector IMU/imu_rotation_vector.cpp ON OFF)
+dai_add_example(imu_extrinsics IMU/imu_extrinsics.cpp OFF OFF)
+dai_add_example(imu_firmware_update IMU/imu_firmware_update.cpp OFF OFF)
 
 # mixed
 dai_add_example(mono_depth_mobilenetssd mixed/mono_depth_mobilenetssd.cpp ON OFF)
@@ -432,4 +434,4 @@ target_compile_definitions(cast_diff PRIVATE BLOB_PATH="${diff_model}")
 dai_add_example(spatial_tiny_yolo_tof_v3 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF)
 dai_add_example(spatial_tiny_yolo_tof_v4 SpatialDetection/spatial_tiny_yolo_tof.cpp OFF OFF)
 target_compile_definitions(spatial_tiny_yolo_tof_v3 PRIVATE BLOB_PATH="${tiny_yolo_v3_blob}")
-target_compile_definitions(spatial_tiny_yolo_tof_v4 PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}")
+target_compile_definitions(spatial_tiny_yolo_tof_v4 PRIVATE BLOB_PATH="${tiny_yolo_v4_blob}")

examples/IMU/imu_extrinsics.cpp

@@ -0,0 +1,92 @@
+#include <iostream>
+
+#include "utility.hpp"
+
+// Includes common necessary includes for development using depthai library
+#include "depthai/depthai.hpp"
+
+
+std::array<float, 3> transformIMUVector(
+    const std::array<float, 3>& vec,
+    const std::vector<std::vector<float>>& extrinsics)
+{
+    // Safety checks
+    if (extrinsics.size() != 4 || extrinsics[0].size() != 4) {
+        throw std::runtime_error("extrinsics must be a 4x4 matrix!");
+    }
+
+    // Extract the top-left 3x3 rotation R (row-major)
+    // R(i,j) = extrinsics[i][j] for i,j in [0..2].
+    std::array<float, 3> out;
+    out[0] = extrinsics[0][0] * vec[0]
+            + extrinsics[0][1] * vec[1]
+            + extrinsics[0][2] * vec[2];
+    out[1] = extrinsics[1][0] * vec[0]
+            + extrinsics[1][1] * vec[1]
+            + extrinsics[1][2] * vec[2];
+    out[2] = extrinsics[2][0] * vec[0]
+            + extrinsics[2][1] * vec[1]
+            + extrinsics[2][2] * vec[2];
+
+    return out;
+}
+
+int main() {
+    using namespace std;
+
+    dai::Pipeline pipeline;
+
+    // Define sources and outputs
+    auto imu = pipeline.create<dai::node::IMU>();
+    auto xlinkOut = pipeline.create<dai::node::XLinkOut>();
+
+    xlinkOut->setStreamName("imu");
+
+    // enable ACCELEROMETER_RAW at 500 hz rate
+    imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
+    // it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
+    // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
+    imu->setBatchReportThreshold(1);
+    // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
+    // if lower or equal to batchReportThreshold then the sending is always blocking on device
+    // useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
+    imu->setMaxBatchReports(10);
+
+    // Link plugins IMU -> XLINK
+    imu->out.link(xlinkOut->input);
+
+    // Pipeline is defined, now we can connect to the device
+    dai::Device d(pipeline);
+
+    auto imuExtr = d.readCalibration().getImuToCameraExtrinsics(dai::CameraBoardSocket::CAM_B, true);
+
+    std::vector<std::vector<float>> extr ={
+        {0, -1, 0, 0},
+        {-1, 0, 0, 0},
+        {0, 0, 1, 0},
+        {0, 0, 0, 1}
+    };
+
+
+    auto imuQueue = d.getOutputQueue("imu", 50, false);
+    while(true) {
+        auto imuData = imuQueue->get<dai::IMUData>();
+
+        auto imuPackets = imuData->packets;
+        for(auto& imuPacket : imuPackets) {
+            auto& acceleroValues = imuPacket.acceleroMeter;
+
+            std::array<float, 3> vULF = {acceleroValues.x, acceleroValues.y, acceleroValues.z};
+            std::array<float, 3> vRDF = transformIMUVector(vULF, imuExtr);
+            std::cout << "###############" << std::endl;
+            std::cout << "vULF = " << vULF[0] << ", " << vULF[1] << ", " << vULF[2] << " -> vRDF = {" << vRDF[0] << ", " << vRDF[1] << ", " << vRDF[2] << "}" << std::endl;
+            std::cout << "###############" << std::endl;
+        }
+
+        int key = cv::waitKey(1);
+        if(key == 'q') {
+            return 0;
+        }
+    }
+    return 0;
+}