From 33f466010e68ba8f7244b441dd15dc7c92823874 Mon Sep 17 00:00:00 2001
From: "mattia.tavernini" <mattia.tavernini@gmail.com>
Date: Tue, 19 May 2026 16:04:45 +0200
Subject: [PATCH] Add support for logging 3D reference frames from homogeneous
 matrices

Enable visualization of 3D coordinate systems by parsing 4x4 homogeneous matrices from specified JSON keypaths. Extracts origin and axis vectors, logging them as colored arrows to Rerun. Supports flat [16] and nested [[4],[4],[4],[4]] array formats.
---
 src/rerunner.cpp | 79 ++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 79 insertions(+)

diff --git a/src/rerunner.cpp b/src/rerunner.cpp
index d39ee0f..16df729 100644
--- a/src/rerunner.cpp
+++ b/src/rerunner.cpp
@@ -55,6 +55,7 @@ class RerunnerPlugin : public Sink<json> {
   std::vector<std::string> _acf_keypaths;
   std::vector<std::string> _fft_keypaths;
   std::vector<std::string> _trace_keypaths;
+  std::vector<std::string> _hmat_keypaths;
   MovingWindowStats _stats;
   size_t _window_size = 1000; // Store 10x ACF width for better statistics
   filesystem::path _blueprint;
@@ -213,6 +214,64 @@ class RerunnerPlugin : public Sink<json> {
       }
     }
 
+    // Reference frames from 4x4 homogeneous matrices
+    for (const auto &keypath : _hmat_keypaths) {
+      json::json_pointer ptr(dot_to_pointer(keypath));
+      const auto &mat = data[ptr];
+      cout << mat.dump(2);
+      // Expect flat array [16] row-major, or array of 4 rows of 4
+      // Normalize to flat row-major double[16]
+      std::array<double, 16> m{};
+      try {
+        if (mat.is_array() && mat.size() == 16) {
+          // flat [m00, m01, ..., m33]
+          for (int i = 0; i < 16; i++) m[i] = mat[i].get<double>();
+        } else if (mat.is_array() && mat.size() == 4 && mat[0].is_array()) {
+          // [[row0], [row1], [row2], [row3]]
+          for (int r = 0; r < 4; r++)
+            for (int c = 0; c < 4; c++)
+              m[r*4+c] = mat[r][c].get<double>();
+        } else {
+          _error = "hmat keypath " + keypath + ": expected 4x4 or flat[16] array";
+          return return_type::error;
+        }
+      } catch (const json::exception &e) {
+        _error = "hmat keypath " + keypath + ": " + e.what();
+        return return_type::error;
+      }
+
+      // Extract columns: rotation axes and translation
+      // Row-major: m[r*4+c]
+      //   col0 = X axis  (m[0],m[4],m[8])
+      //   col1 = Y axis  (m[1],m[5],m[9])
+      //   col2 = Z axis  (m[2],m[6],m[10])
+      //   col3 = origin  (m[3],m[7],m[11])
+      float ax_len = _params.value("h_scale", 1.0f);
+
+      rerun::Position3D origin{
+        (float)m[3] ,
+        (float)m[7] ,
+        (float)m[11]
+      };
+
+      // Three arrows: one per axis
+      _rec->log("frame/" + keypath,
+        rerun::Arrows3D::from_vectors({
+          rerun::Vector3D{(float)m[0]*ax_len, (float)m[4]*ax_len, (float)m[8]*ax_len},   // X
+          rerun::Vector3D{(float)m[1]*ax_len, (float)m[5]*ax_len, (float)m[9]*ax_len},   // Y
+          rerun::Vector3D{(float)m[2]*ax_len, (float)m[6]*ax_len, (float)m[10]*ax_len},  // Z
+        })
+        .with_origins({origin, origin, origin})
+        .with_colors({
+          rerun::Color(255,  40,  40),  // X = red
+          rerun::Color( 40, 220,  40),  // Y = green
+          rerun::Color( 40,  40, 255),  // Z = blue
+        })
+        .with_labels({"X", "Y", "Z"})
+      );
+      logged = true;
+    }
+
     // Process ACF keypaths
     for (const auto &keypath : _acf_keypaths) {
       if (_stats.is_full(keypath)) {
@@ -360,6 +419,26 @@ class RerunnerPlugin : public Sink<json> {
       }
     }
 
+    // In set_params, add a new param:
+    //_params["hmat_keypaths"] = json::array();  // list of keypaths to 4x4 matrices
+
+    // Load in set_params:
+    _hmat_keypaths.clear();
+    if (_params.contains("hmat_keypaths") && _params["hmat_keypaths"].is_array()) {
+      std::cout << "\ntrovato hmat " << _params["hmat_keypaths"].size() << "\n";
+      for (const auto &path : _params["hmat_keypaths"])
+      {
+         std::cout << "\nvaluto\n";
+        if (path.is_string()){
+          cout << "\naggiunto\n";
+          _hmat_keypaths.push_back(path.get<std::string>());
+          cout << "\naggiunto\n";
+        }else{
+          cout << "\nnon aggiunto\n";
+        }
+      }
+    }
+
     // Skeleton
     if (!_params["skeleton"].get<string>().empty()) {
       _skeleton = make_unique<Skeleton>(_rec.get());