new template base class

jmeidow · jmeidow · commit a790e7bddcab · 2026-03-23T09:36:24.000+01:00
diff --git a/src/greasepad.pro b/src/greasepad.pro
@@ -81,6 +81,7 @@ HEADERS += \
     statistics/uniform.h \
     uncertain/quncertain.h \
     uncertain/udistance.h \
+    uncertain/uelement.h \
     uncertain/uncertain.h \
     uncertain/upoint.h \
     uncertain/usegment.h \
diff --git a/src/uncertain/uelement.h b/src/uncertain/uelement.h
@@ -0,0 +1,130 @@
+/*
+ * This file is part of the GreasePad distribution (https://github.com/FraunhoferIOSB/GreasePad).
+ * Copyright (c) 2022-2026 Jochen Meidow, Fraunhofer IOSB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+
+#ifndef UELEMENT_H
+#define UELEMENT_H
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+#include <cassert>
+#include <utility>
+
+#include "geometry/skew.h"
+#include "kernel.h"
+#include "matfun.h"
+#include "statistics/iscov.h"
+
+
+//! Uncertain geometric entities
+namespace Uncertain {
+
+using Eigen::Matrix;
+using Eigen::Vector;
+
+using Geometry::skew;
+
+using Matfun::null;
+using Matfun::sign;
+
+using Stats::isCovMat;
+
+
+//! Base class for uncertain geometric elements, represented by homogeneous N-vectors
+template <int N>
+class uElement
+{
+
+public:
+    //! Construct geometric element with N-vector and its covariance matrix
+    uElement( Vector<double,N> z, Matrix<double,N,N> Sigma_zz)
+        : m_val(std::move(z)), m_cov(std::move(Sigma_zz))
+    {
+        assert( m_val.size()==m_cov.cols() );
+        assert( isCovMat( m_cov) );
+    }
+    uElement ( const uElement &) = default;  //!< Copy constructor
+    uElement( uElement &&) = default;        //!< Move constructor
+    uElement & operator= ( const uElement &&) = delete;  //!< Move assignment
+
+    ~uElement() = default;
+
+    void normalizeSpherical();
+
+    //! Get covariance matrix
+    [[nodiscard]] Matrix<double,N,N> Cov() const {return m_cov;}
+
+    //! Get homogeneous N-vector representing the geometric element
+    [[nodiscard]] Vector<double,N> v() const {return m_val;}
+
+    //! Get i-th element of the homogeneous N-vector
+    [[nodiscard]] double v( const int i) const {return m_val(i);}
+
+    [[nodiscard]] bool isIdenticalTo( const uElement &s, double T) const;
+
+protected:
+    uElement() = default;  //!< default constructor
+    uElement & operator= ( const uElement &) = default;  //!< copy assignment
+
+private:
+    Vector<double,N> m_val; //!< homogeneous N-vector representing the element
+    Matrix<double,N,N> m_cov; //!< homogeneous NxN covariance matrix
+};
+
+
+//! Spherically normalize the entity
+template <int N>
+void uElement<N>::normalizeSpherical()
+{
+    static const Matrix<double,N,N> Id = Matrix<double,N,N>::Identity();
+    assert( m_val.norm() > 0 );
+    const Matrix<double,N,N> Jac = (Id-m_val*m_val.transpose()/m_val.squaredNorm())/m_val.norm();
+    m_cov = Jac*m_cov*Jac.transpose();
+    m_val.normalize();  // x = x /norm(x)
+}
+
+
+//! Check if uncertain element is identical with other uncertain element
+template<int N>
+bool uElement<N>::isIdenticalTo(const uElement & us,
+                                const double T) const
+{
+    uElement a(*this);
+    uElement b(us);
+
+    // fix ambiguities
+    a.normalizeSpherical();
+    b.normalizeSpherical();
+
+    int idx = 0;  // visitor
+    a.v().cwiseAbs().maxCoeff( &idx );  // [~,idx] = max( abs(a) )
+    a.m_val *= sign( a.v(idx) );
+    b.m_val *= sign( b.v(idx) );
+
+    const Matrix<double,N,N-1> Jac = null(a.v());     // (A.120)
+    const Vector<double,N-1> d = Jac.transpose()*(a.v()-b.v() );  // (10.141)
+    const Matrix<double,N-1,N-1> Sigma_dd = Jac.transpose()*(a.Cov()+b.Cov())*Jac;
+
+    return d.dot( Sigma_dd.ldlt().solve(d) ) < T;
+}
+
+} // namespace Uncertain
+
+
+#endif // UELEMENT_H
diff --git a/src/uncertain/upoint.cpp b/src/uncertain/upoint.cpp
@@ -20,7 +20,7 @@
 #include "matfun.h"
 #include "statistics/iscov.h"
 #include "udistance.h"
-#include "uncertain.h"
+#include "uncertain/uelement.h"
 #include "upoint.h"
 #include "ustraightline.h"
 
@@ -38,7 +38,9 @@
 
 using Eigen::Matrix;
 using Eigen::Matrix2d;
+using Eigen::Matrix3d;
 using Eigen::Vector2d;
+using Eigen::Vector3d;
 
 using Matfun::sign;
 using Matfun::cof3;
@@ -51,7 +53,7 @@ namespace Uncertain {
 
 //! Construction of uncertain point via 3-vector x and its covariance matrix
 uPoint::uPoint( const Vector3d & x, const Matrix3d & Sigma_xx)
-    : BasicEntity2D (x, Sigma_xx)
+    : uElement (x, Sigma_xx)
 {
     // qDebug() << Q_FUNC_INFO;
 }
diff --git a/src/uncertain/upoint.h b/src/uncertain/upoint.h
@@ -19,8 +19,10 @@
 #ifndef UPOINT_H
 #define UPOINT_H
 
+#include "Eigen/Core"
+
 #include "geometry/aabb.h"     // axis-aligned bounding box
-#include "uncertain.h"
+#include "uelement.h"
 
 
 namespace Uncertain {
@@ -32,18 +34,18 @@ using Geometry::Aabb;
 
 
 //! Uncertain point
-class uPoint : public BasicEntity2D
+class uPoint : public uElement<3>
 {
 public:
     uPoint() = delete;
-    uPoint( const Vector3d & x,
-            const Matrix3d & Sigma_xx);
+    uPoint( const Eigen::Vector3d & x,
+            const Eigen::Matrix3d & Sigma_xx);
 
     [[nodiscard]] Aabb<double,2> bbox() const;
-    [[nodiscard]] Matrix3d conicMatrix(double) const;
+    [[nodiscard]] Eigen::Matrix3d conicMatrix(double) const;
     [[nodiscard]] uPoint euclidean() const;
     [[nodiscard]] uPoint sphericalNormalized() const;
-    [[nodiscard]] uPoint transformed( const Matrix3d & TT) const;
+    [[nodiscard]] uPoint transformed( const Eigen::Matrix3d & TT) const;
     [[nodiscard]] uStraightLine cross( const uPoint &) const;
     [[nodiscard]] uDistance distanceAlgebraicTo( const uStraightLine & ul) const;
     [[nodiscard]] uDistance distanceEuclideanTo( const uStraightLine & ul) const;
diff --git a/src/uncertain/ustraightline.cpp b/src/uncertain/ustraightline.cpp
@@ -23,7 +23,7 @@
 
 #include "matfun.h"
 #include "statistics/iscov.h"
-#include "uncertain.h"
+#include "uncertain/uelement.h"
 #include "upoint.h"
 #include "ustraightline.h"
 
@@ -55,7 +55,7 @@ namespace Uncertain {
 //! Uncertain straight line, defined by a 3-vector and its covariance matrix
 uStraightLine::uStraightLine(const Vector3d & l,
                              const Matrix3d & Sigma_ll)
-    : BasicEntity2D( l, Sigma_ll )
+    : uElement( l, Sigma_ll )
 {
     /* check: null(Sigma_ll)
     Eigen::FullPivLU<MatrixXd> lu(Sigma_ll);
diff --git a/src/uncertain/ustraightline.h b/src/uncertain/ustraightline.h
@@ -21,30 +21,28 @@
 
 #include "Eigen/Core"
 
-#include "uncertain.h"
+#include "uelement.h"
 
 #include <cmath>
 
 
 namespace Uncertain {
 
-using Eigen::VectorXd;
-
 class uPoint;
 
-
 //! Uncertain straight line
-class uStraightLine : public BasicEntity2D
+class uStraightLine : public uElement<3>
 {
 public:
     uStraightLine() = default;
-    uStraightLine( const Vector3d & l, const Matrix3d & Sigma_ll);
+    uStraightLine( const Eigen::Vector3d & l, const Eigen::Matrix3d & Sigma_ll);
 
-    [[nodiscard]] static uStraightLine estim( const VectorXd & xi, const VectorXd & yi);
+    [[nodiscard]] static uStraightLine
+    estim( const Eigen::VectorXd & xi, const Eigen::VectorXd & yi);
 
     [[nodiscard]] uStraightLine euclidean() const;
     [[nodiscard]] uStraightLine sphericalNormalized() const;
-    [[nodiscard]] Matrix3d conicMatrix(double) const;
+    [[nodiscard]] Eigen::Matrix3d conicMatrix(double) const;
 
     //! Angle between this straight line and the x-axis in radians
     [[nodiscard]] double angle_rad()  const { return std::atan2( v(1),v(0) ); }
