Optimize vector warp interpolation

cpp/core/interp/linear.h

@@ -197,33 +197,49 @@ class LinearInterp<ImageType, LinearInterpProcessingType::Value>
 
   //! Read interpolated values from volumes along axis >= 3
   /*! See file interp/base.h for details. */
-  Eigen::Matrix<value_type, Eigen::Dynamic, 1> row(size_t axis) {
+  template <int N = Eigen::Dynamic> FORCE_INLINE Eigen::Matrix<value_type, N, 1> row(size_t axis) {
+    using Vec = Eigen::Matrix<value_type, N, 1>;
+
+    if constexpr (N != Eigen::Dynamic) {
+      assert(ImageType::size(axis) == N);
+    }
+
     if (Base<ImageType>::out_of_bounds) {
-      Eigen::Matrix<value_type, Eigen::Dynamic, 1> out_of_bounds_row(ImageType::size(axis));
-      out_of_bounds_row.setOnes();
-      out_of_bounds_row *= Base<ImageType>::out_of_bounds_value;
-      return out_of_bounds_row;
+      Vec out;
+      if constexpr (N == Eigen::Dynamic) {
+        out.resize(ImageType::size(axis));
+      }
+      out.setConstant(Base<ImageType>::out_of_bounds_value);
+      return out;
     }
 
     const Eigen::Array<ssize_t, 3, 1> c(P.array().floor().template cast<ssize_t>());
 
-    Eigen::Matrix<value_type, Eigen::Dynamic, 8> coeff_matrix(ImageType::size(3), 8);
+    Vec out;
+    if constexpr (N == Eigen::Dynamic) {
+      out = Vec::Zero(ImageType::size(axis));
+    } else {
+      out = Vec::Zero();
+    }
 
-    size_t i(0);
+    size_t i = 0;
     for (ssize_t z = 0; z < 2; ++z) {
       ImageType::index(2) = clamp(c[2] + z, ImageType::size(2));
       for (ssize_t y = 0; y < 2; ++y) {
         ImageType::index(1) = clamp(c[1] + y, ImageType::size(1));
         for (ssize_t x = 0; x < 2; ++x) {
           ImageType::index(0) = clamp(c[0] + x, ImageType::size(0));
-          coeff_matrix.col(i++) = ImageType::row(axis);
+          out.noalias() += Vec(ImageType::row(axis)) * factors[i++];
         }
       }
     }
 
-    return coeff_matrix * factors;
+    return out;
   }
 
+  //! Convenience wrapper for row<3>(axis) to avoid dynamic allocation when only 3 values are required
+  FORCE_INLINE Eigen::Matrix<value_type, 3, 1> vec3(size_t axis) { return row<3>(axis); }
+
 protected:
   Eigen::Matrix<coef_type, 8, 1> factors;
 };

cpp/core/registration/warp/compose.h

@@ -72,7 +72,7 @@ class ComposeDispKernel {
     if (!disp2_interp) {
       disp_output.row(3) = disp_input1.row(3);
     } else {
-      const Eigen::Vector3d displacement(Eigen::Vector3d(disp2_interp.row(3)).array() * step);
+      const Eigen::Vector3d displacement(Eigen::Vector3d(disp2_interp.vec3(3)).array() * step);
       const Eigen::Vector3d new_position = displacement + original_position;
       disp_output.row(3) = new_position - voxel_position;
     }
@@ -105,12 +105,12 @@ template <class DeformationField1Type, class DeformationField2Type> class Compos
     if (!deform1_interp) {
       deform.row(3) = out_of_bounds;
     } else {
-      const Eigen::Vector3d position2 = deform1_interp.row(3);
+      const Eigen::Vector3d position2 = deform1_interp.vec3(3);
       deform2_interp.scanner(position2);
       if (!deform2_interp) {
         deform.row(3) = out_of_bounds;
       } else {
-        const Eigen::Vector3d position3 = deform2_interp.row(3);
+        const Eigen::Vector3d position3 = deform2_interp.vec3(3);
         deform.row(3) = linear2 * position3;
       }
     }

cpp/core/registration/warp/invert.h

@@ -54,7 +54,7 @@ class DisplacementThreadKernel {
 private:
   default_type update(Eigen::Vector3d &current, const Eigen::Vector3d &truth) {
     displacement.scanner(current);
-    Eigen::Vector3d discrepancy = truth - (current + Eigen::Vector3d(displacement.row(3)));
+    const Eigen::Vector3d discrepancy = truth - (current + Eigen::Vector3d(displacement.vec3(3)));
     current += discrepancy;
     return discrepancy.dot(discrepancy);
   }
@@ -93,7 +93,7 @@ class DeformationThreadKernel {
 private:
   default_type update(Eigen::Vector3d &current, const Eigen::Vector3d &truth) {
     deform.scanner(current);
-    Eigen::Vector3d discrepancy = truth - Eigen::Vector3d(deform.row(3));
+    const Eigen::Vector3d discrepancy = truth - Eigen::Vector3d(deform.vec3(3));
     current += discrepancy;
     return discrepancy.dot(discrepancy);
   }