made gaussians transformable

uos · Dec 2, 2024 · 5836918 · 5836918
1 parent 44af063
commit 5836918
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 0 deletions.
diff --git a/src/rmagine_core/include/rmagine/math/types/Quaternion.hpp b/src/rmagine_core/include/rmagine/math/types/Quaternion.hpp
@@ -43,6 +43,7 @@
 #include "definitions.h"
 #include <rmagine/types/shared_functions.h>
 #include <initializer_list>
+#include "Gaussian3D.hpp"
 #include "CrossStatistics.hpp"
 
 namespace rmagine
@@ -125,6 +126,12 @@ struct Quaternion_
     RMAGINE_INLINE_FUNCTION
     Vector3_<DataT> mult(const Vector3_<DataT>& p) const;
 
+    /**
+     * @brief Rotate gaussian with this quaternion
+     */
+    RMAGINE_INLINE_FUNCTION
+    Gaussian3D_<DataT> mult(const Gaussian3D_<DataT>& g) const;
+
     /**
      * @brief Rotate cross statistics with this quaternion
      */

diff --git a/src/rmagine_core/include/rmagine/math/types/Quaternion.tcc b/src/rmagine_core/include/rmagine/math/types/Quaternion.tcc
@@ -74,6 +74,18 @@ Vector3_<DataT> Quaternion_<DataT>::mult(const Vector3_<DataT>& p) const
     return {PT.x, PT.y, PT.z};
 }
 
+template<typename DataT>
+RMAGINE_INLINE_FUNCTION
+Gaussian3D_<DataT> Quaternion_<DataT>::mult(const Gaussian3D_<DataT>& g) const
+{
+    Gaussian3D_<DataT> res;
+    res.mean = mult(g.mean);
+    const Matrix_<DataT, 3, 3> M = *this;
+    res.sigma = M * g.sigma * M.T();
+    res.n_meas = g.n_meas;
+    return res;
+}
+
 template<typename DataT>
 RMAGINE_INLINE_FUNCTION
 CrossStatistics_<DataT> Quaternion_<DataT>::mult(const CrossStatistics_<DataT>& stats) const

diff --git a/src/rmagine_core/include/rmagine/math/types/Transform.hpp b/src/rmagine_core/include/rmagine/math/types/Transform.hpp
@@ -45,6 +45,7 @@
 #include "Vector3.hpp"
 #include "Quaternion.hpp"
 #include "CrossStatistics.hpp"
+#include "Gaussian3D.hpp"
 
 namespace rmagine
 {
@@ -109,6 +110,9 @@ struct Transform_
     RMAGINE_INLINE_FUNCTION
     Vector3_<DataT> mult(const Vector3_<DataT>& v) const;
 
+    RMAGINE_INLINE_FUNCTION
+    Gaussian3D_<DataT> mult(const Gaussian3D_<DataT>& g) const;
+
     RMAGINE_INLINE_FUNCTION
     CrossStatistics_<DataT> mult(const CrossStatistics_<DataT>& stats) const;
 

diff --git a/src/rmagine_core/include/rmagine/math/types/Transform.tcc b/src/rmagine_core/include/rmagine/math/types/Transform.tcc
@@ -61,6 +61,18 @@ Vector3_<DataT> Transform_<DataT>::mult(const Vector3_<DataT>& v) const
     return R * v + t;
 }
 
+template<typename DataT>
+RMAGINE_INLINE_FUNCTION
+Gaussian3D_<DataT> Transform_<DataT>::mult(const Gaussian3D_<DataT>& g) const
+{
+    Gaussian3D_<DataT> res;
+    res.mean = mult(g.mean);
+    const Matrix_<DataT, 3, 3> M = R; // TODO: can we do the two steps more efficient?
+    res.sigma = M * g.sigma * M.T();
+    res.n_meas = g.n_meas;
+    return res; 
+}
+
 template<typename DataT>
 RMAGINE_INLINE_FUNCTION
 CrossStatistics_<DataT> Transform_<DataT>::mult(const CrossStatistics_<DataT>& stats) const