method getEntity now template function

jmeidow · jmeidow · commit 0485ece866b6 · 2026-03-13T09:17:03.000+01:00
diff --git a/src/adjustment.cpp b/src/adjustment.cpp
@@ -37,7 +37,6 @@
 #include <cmath>
 #include <memory>
 #include <sstream>
-#include <utility>
 
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
@@ -70,17 +69,6 @@ using TextColor::green;
 using TextColor::red;
 
 
-std::pair<Vector3d,Matrix3d >
-AdjustmentFramework::getEntity( const Index s) const
-{
-    const Index offset = 3*s;
-    const Matrix3d RR = Rot_ab<double,3>( l_.segment(offset,3),
-                                          l0_.segment(offset,3) );
-    return { l0_.segment(offset,3),
-             RR*Cov_ll_.block(offset,offset,3,3)*RR.adjoint() };
-}
-
-
 //! update of parameters (observations) via retraction
 void AdjustmentFramework::update( const VectorXd &x)
 {
diff --git a/src/adjustment.h b/src/adjustment.h
@@ -28,6 +28,8 @@
 
 #include "qcontainerfwd.h"
 
+#include "geometry/minrot.h"
+
 
 namespace Constraint {class ConstraintBase;} // namespace Constraint
 namespace Graph {class IncidenceMatrix;} // namespace Graph
@@ -64,8 +66,19 @@ class AdjustmentFramework
                             Eigen::Array<Attribute,Eigen::Dynamic,1> & status,
                             Eigen::Array<bool,Eigen::Dynamic,1> & enforced);
 
-    //! Get s-th entity, i.e., segment, represented by vector of length len
-    [[nodiscard]] std::pair<Eigen::Vector3d, Eigen::Matrix3d> getEntity( Eigen::Index s) const;
+    //! Get s-th entity represented by vector of length N
+    template <int N>
+    std::pair<Eigen::Vector<double,N>,Eigen::Matrix<double,N,N> >
+    getEntity( const Eigen::Index s) const
+    {
+        const Eigen::Index offset = N*s;
+        const Eigen::Matrix<double,N,N> RR = Geometry::Rot_ab(
+            l_.segment(offset,N).eval(),
+            l0_.segment(offset,N).eval() );
+
+        return { l0_.segment(offset,N),
+                RR*Cov_ll_.block(offset,offset,N,N)*RR.adjoint() };
+    }
 
 private:
     [[nodiscard]] static int nIterMax() { return nIterMax_; }
diff --git a/src/state.cpp b/src/state.cpp
@@ -823,7 +823,7 @@ void impl::update_segments( const ArrayXi & maps_,
 {
     for ( Index s=0; s<maps_.size(); s++ ) {
         //  straight line:  s-th 3-vector
-        const std::pair<Vector3d,Matrix3d> p = a.getEntity(s);
+        const std::pair<Vector3d,Matrix3d> p = a.getEntity<3>(s);
         const uStraightLine ul( p.first, p.second );
 
         const uPoint ux = m_segm.at( maps_(s) )->ux();

Original file line number	Diff line number	Diff line change
`@@ -823,7 +823,7 @@ void impl::update_segments( const ArrayXi & maps_,`
`823`	`823`	`{`
`824`	`824`	`for ( Index s=0; s<maps_.size(); s++ ) {`
`825`	`825`	`// straight line: s-th 3-vector`
`826`		`- const std::pair<Vector3d,Matrix3d> p = a.getEntity(s);`
	`826`	`+ const std::pair<Vector3d,Matrix3d> p = a.getEntity<3>(s);`
`827`	`827`	`const uStraightLine ul( p.first, p.second );`
`828`	`828`
`829`	`829`	`const uPoint ux = m_segm.at( maps_(s) )->ux();`