Cleaning intrinsicBase

alicevision · Dec 27, 2024 · e3b0ba5 · e3b0ba5
1 parent c1e15b3
commit e3b0ba5
Show file tree

Hide file tree

Showing 3 changed files with 94 additions and 77 deletions.
diff --git a/src/aliceVision/calibration/distortionEstimationGeometry.cpp b/src/aliceVision/calibration/distortionEstimationGeometry.cpp
@@ -7,6 +7,7 @@
 #include "distortionEstimationGeometry.hpp"
 
 #include <aliceVision/system/Logger.hpp>
+#include <aliceVision/numeric/algebra.hpp>
 #include <aliceVision/sfm/bundle/manifolds/so3.hpp>
 #include <ceres/ceres.h>
 #include <cmath>

diff --git a/src/aliceVision/camera/IntrinsicBase.hpp b/src/aliceVision/camera/IntrinsicBase.hpp
@@ -434,6 +434,12 @@ class IntrinsicBase
      */
     virtual double getVerticalFov() const = 0;
 
+    /**
+     * @brief Initialize state with default values
+     * The estimator state is used in the bundle adjustment to decide if we update it.
+     * It is set to constant if the intrinsic is locked
+     * It is set to refined if unlocked
+    */
     virtual void initializeState()
     {
         if (_locked)
@@ -445,9 +451,17 @@ class IntrinsicBase
             _state = EEstimatorParameterState::REFINED;
         }
     }
-
+
+    /**
+     * @brief accessor to estimator state
+     * @return a state
+    */
     EEstimatorParameterState getState() const { return _state; }
 
+    /**
+     * @brief mutator for the estimator state
+     * @param state the new state of the intrinsic
+    */
     void setState(EEstimatorParameterState state) { _state = state; }
 
   protected:
@@ -456,6 +470,7 @@ class IntrinsicBase
     /// intrinsic lock
     bool _locked = false;
     EEstimatorParameterState _state = EEstimatorParameterState::REFINED;
+
     unsigned int _w = 0;
     unsigned int _h = 0;
     double _sensorWidth = 36.0;
@@ -531,81 +546,5 @@ inline double angleBetweenRays(const geometry::Pose3& pose1, const geometry::Pos
 
 }  // namespace camera
 
-template<size_t M, size_t N>
-Eigen::Matrix<double, M * N, M * N> getJacobian_At_wrt_A()
-{
-    Eigen::Matrix<double, M * N, M * N> ret;
-
-    /** vec(M1*M2*M3) = kron(M3.t, M1) * vec(M2) */
-    /** vec(IAtB) = kron(B.t, I) * vec(A) */
-    /** dvec(IAtB)/dA = kron(B.t, I) * dvec(At)/dA */
-
-    ret.fill(0);
-
-    size_t pos_at = 0;
-    for (size_t i = 0; i < M; i++)
-    {
-        for (size_t j = 0; j < N; j++)
-        {
-            size_t pos_a = N * j + i;
-            ret(pos_at, pos_a) = 1;
-
-            pos_at++;
-        }
-    }
-
-    return ret;
-}
-
-template<size_t M, size_t N, size_t K>
-Eigen::Matrix<double, M * K, M * N> getJacobian_AB_wrt_A(const Eigen::Matrix<double, M, N>& A, const Eigen::Matrix<double, N, K>& B)
-{
-    Eigen::Matrix<double, M * K, M * N> ret;
-
-    /** vec(M1*M2*M3) = kron(M3.t, M1) * vec(M2) */
-    /** vec(IAB) = kron(B.t, I) * vec(A) */
-    /** dvec(IAB)/dA = kron(B.t, I) * dvec(A)/dA */
-    /** dvec(IAB)/dA = kron(B.t, I) */
-
-    ret.fill(0);
-
-    Eigen::Matrix<double, K, N> Bt = B.transpose();
-
-    for (size_t row = 0; row < K; row++)
-    {
-        for (size_t col = 0; col < N; col++)
-        {
-            ret.template block<M, M>(row * M, col * M) = Bt(row, col) * Eigen::Matrix<double, M, M>::Identity();
-        }
-    }
-
-    return ret;
-}
-
-template<size_t M, size_t N, size_t K>
-Eigen::Matrix<double, M * K, M * N> getJacobian_AtB_wrt_A(const Eigen::Matrix<double, M, N>& A, const Eigen::Matrix<double, M, K>& B)
-{
-    return getJacobian_AB_wrt_A<M, N, K>(A.transpose(), B) * getJacobian_At_wrt_A<M, N>();
-}
-
-template<size_t M, size_t N, size_t K>
-Eigen::Matrix<double, M * K, N * K> getJacobian_AB_wrt_B(const Eigen::Matrix<double, M, N>& A, const Eigen::Matrix<double, N, K>& B)
-{
-    Eigen::Matrix<double, M * K, N * K> ret;
-
-    /** vec(M1*M2*M3) = kron(M3.t, M1) * vec(M2) */
-    /** vec(ABI) = kron(I, A) * vec(B) */
-    /** dvec(ABI)/dB = kron(I, A) * dvec(B)/dB */
-    /** dvec(ABI)/dB = kron(I, A) */
-
-    ret.fill(0);
-
-    for (size_t index = 0; index < K; index++)
-    {
-        ret.template block<M, N>(M * index, N * index) = A;
-    }
-
-    return ret;
-}
 
 }  // namespace aliceVision
diff --git a/src/aliceVision/numeric/algebra.hpp b/src/aliceVision/numeric/algebra.hpp
@@ -80,4 +80,81 @@ inline double Nullspace2(const TMat& A, TVec1& x1, TVec2& x2)
     return vec(vec.rows() - 1);
 }
 
+template<size_t M, size_t N>
+Eigen::Matrix<double, M * N, M * N> getJacobian_At_wrt_A()
+{
+    Eigen::Matrix<double, M * N, M * N> ret;
+
+    /** vec(M1*M2*M3) = kron(M3.t, M1) * vec(M2) */
+    /** vec(IAtB) = kron(B.t, I) * vec(A) */
+    /** dvec(IAtB)/dA = kron(B.t, I) * dvec(At)/dA */
+
+    ret.fill(0);
+
+    size_t pos_at = 0;
+    for (size_t i = 0; i < M; i++)
+    {
+        for (size_t j = 0; j < N; j++)
+        {
+            size_t pos_a = N * j + i;
+            ret(pos_at, pos_a) = 1;
+
+            pos_at++;
+        }
+    }
+
+    return ret;
+}
+
+template<size_t M, size_t N, size_t K>
+Eigen::Matrix<double, M * K, M * N> getJacobian_AB_wrt_A(const Eigen::Matrix<double, M, N>& A, const Eigen::Matrix<double, N, K>& B)
+{
+    Eigen::Matrix<double, M * K, M * N> ret;
+
+    /** vec(M1*M2*M3) = kron(M3.t, M1) * vec(M2) */
+    /** vec(IAB) = kron(B.t, I) * vec(A) */
+    /** dvec(IAB)/dA = kron(B.t, I) * dvec(A)/dA */
+    /** dvec(IAB)/dA = kron(B.t, I) */
+
+    ret.fill(0);
+
+    Eigen::Matrix<double, K, N> Bt = B.transpose();
+
+    for (size_t row = 0; row < K; row++)
+    {
+        for (size_t col = 0; col < N; col++)
+        {
+            ret.template block<M, M>(row * M, col * M) = Bt(row, col) * Eigen::Matrix<double, M, M>::Identity();
+        }
+    }
+
+    return ret;
+}
+
+template<size_t M, size_t N, size_t K>
+Eigen::Matrix<double, M * K, M * N> getJacobian_AtB_wrt_A(const Eigen::Matrix<double, M, N>& A, const Eigen::Matrix<double, M, K>& B)
+{
+    return getJacobian_AB_wrt_A<M, N, K>(A.transpose(), B) * getJacobian_At_wrt_A<M, N>();
+}
+
+template<size_t M, size_t N, size_t K>
+Eigen::Matrix<double, M * K, N * K> getJacobian_AB_wrt_B(const Eigen::Matrix<double, M, N>& A, const Eigen::Matrix<double, N, K>& B)
+{
+    Eigen::Matrix<double, M * K, N * K> ret;
+
+    /** vec(M1*M2*M3) = kron(M3.t, M1) * vec(M2) */
+    /** vec(ABI) = kron(I, A) * vec(B) */
+    /** dvec(ABI)/dB = kron(I, A) * dvec(B)/dB */
+    /** dvec(ABI)/dB = kron(I, A) */
+
+    ret.fill(0);
+
+    for (size_t index = 0; index < K; index++)
+    {
+        ret.template block<M, N>(M * index, N * index) = A;
+    }
+
+    return ret;
+}
+
 }  // namespace aliceVision