Merge pull request #2118 from SCIInstitute/fix_matrix_2117

Switch to eigen matrix for inverse covariance matrix to fix #2117

Author: akenmorris

Libs/Optimize/Function/CorrespondenceFunction.cpp

@@ -41,7 +41,9 @@ void CorrespondenceFunction::ComputeUpdates(const ParticleSystem* c) {
 
   if (this->m_UseMeanEnergy) {
     pinvMat.set_identity();
-    m_InverseCovMatrix->clear();
+
+    m_InverseCovMatrix->setZero();
+
   } else {
     gramMat = points_minus_mean.transpose() * points_minus_mean;
 
@@ -60,7 +62,11 @@ void CorrespondenceFunction::ComputeUpdates(const ParticleSystem* c) {
     const auto lhs = projMat * invLambda;
     const auto rhs =
         invLambda * projMat.transpose();  // invLambda doesn't need to be transposed since its a diagonal matrix
-    m_InverseCovMatrix->set_size(num_dims, num_dims);
+
+    // resize the inverse covariance matrix if necessary
+    if (m_InverseCovMatrix->rows() != num_dims || m_InverseCovMatrix->cols() != num_dims) {
+      m_InverseCovMatrix->resize(num_dims, num_dims);
+    }
     Utils::multiply_into(*m_InverseCovMatrix, lhs, rhs);
   }
 
@@ -166,7 +172,14 @@ CorrespondenceFunction::VectorType CorrespondenceFunction::Evaluate(unsigned int
   if (this->m_UseMeanEnergy) {
     tmp1.set_identity();
   } else {
-    tmp1 = m_InverseCovMatrix->extract(sz_Yidx, sz_Yidx, num, num);
+    // extract 3x3 submatrix at k,k
+    Eigen::MatrixXd region = m_InverseCovMatrix->block(sz_Yidx, sz_Yidx, 3, 3);
+    // convert to vnl
+    for (unsigned int i = 0; i < 3; i++) {
+      for (unsigned int j = 0; j < 3; j++) {
+        tmp1(i, j) = region(i, j);
+      }
+    }
   }
 
   vnl_matrix_type Y_dom_idx(sz_Yidx, 1, 0.0);

Libs/Optimize/Function/CorrespondenceFunction.h

@@ -175,7 +175,7 @@ class CorrespondenceFunction : public VectorFunction {
     num_dims = 0;
     num_samples = 0;
     m_PointsUpdate = std::make_shared<vnl_matrix_type>(10, 10);
-    m_InverseCovMatrix = std::make_shared<vnl_matrix_type>(10, 10);
+    m_InverseCovMatrix = std::make_shared<Eigen::MatrixXd>(10, 10);
     m_points_mean = std::make_shared<vnl_matrix_type>(10, 10);
   }
   virtual ~CorrespondenceFunction() {}
@@ -203,7 +203,7 @@ class CorrespondenceFunction : public VectorFunction {
   std::vector<bool> m_UseXYZ;
   std::vector<bool> m_UseNormals;
   std::shared_ptr<vnl_matrix_type> m_points_mean;
-  std::shared_ptr<vnl_matrix_type> m_InverseCovMatrix;
+  std::shared_ptr<Eigen::MatrixXd> m_InverseCovMatrix;
   int num_dims, num_samples;
 };
 }  // namespace shapeworks

Libs/Optimize/Function/LegacyCorrespondenceFunction.cpp

@@ -63,7 +63,7 @@ void LegacyCorrespondenceFunction ::ComputeCovarianceMatrix() {
 
   if (this->m_UseMeanEnergy) {
     pinvMat.set_identity();
-    m_InverseCovMatrix->clear();
+    m_InverseCovMatrix->setZero();
   } else {
     gramMat = points_minus_mean.transpose() * points_minus_mean;
 
@@ -83,7 +83,9 @@ void LegacyCorrespondenceFunction ::ComputeCovarianceMatrix() {
     const auto lhs = projMat * invLambda;
     const auto rhs =
         invLambda * projMat.transpose();  // invLambda doesn't need to be transposed since its a diagonal matrix
-    m_InverseCovMatrix->set_size(num_dims, num_dims);
+    if (m_InverseCovMatrix->rows() != num_dims || m_InverseCovMatrix->cols() != num_dims) {
+      m_InverseCovMatrix->resize(num_dims, num_dims);
+    }
     Utils::multiply_into(*m_InverseCovMatrix, lhs, rhs);
   }
   m_PointsUpdate->update(points_minus_mean * pinvMat);
@@ -108,9 +110,8 @@ void LegacyCorrespondenceFunction ::ComputeCovarianceMatrix() {
 }
 
 LegacyCorrespondenceFunction::VectorType LegacyCorrespondenceFunction ::Evaluate(unsigned int idx, unsigned int d,
-                                                                                       const ParticleSystem* system,
-                                                                                       double& maxdt,
-                                                                                       double& energy) const {
+                                                                                 const ParticleSystem* system,
+                                                                                 double& maxdt, double& energy) const {
   // NOTE: This code requires that indices be contiguous, i.e. it won't work if
   // you start deleting particles.
   const unsigned int DomainsPerShape = m_ShapeMatrix->GetDomainsPerShape();
@@ -130,10 +131,18 @@ LegacyCorrespondenceFunction::VectorType LegacyCorrespondenceFunction ::Evaluate
 
   vnl_matrix_type tmp1(3, 3, 0.0);
 
-  if (this->m_UseMeanEnergy)
+  if (this->m_UseMeanEnergy) {
     tmp1.set_identity();
-  else
-    tmp1 = m_InverseCovMatrix->extract(3, 3, k, k);
+  } else {
+    // extract 3x3 submatrix at k,k
+    Eigen::MatrixXd region = m_InverseCovMatrix->block(k, k, 3, 3);
+    // convert to vnl
+    for (unsigned int i = 0; i < 3; i++) {
+      for (unsigned int j = 0; j < 3; j++) {
+        tmp1(i, j) = region(i, j);
+      }
+    }
+  }
 
   vnl_matrix_type tmp = Xi.transpose() * tmp1;
 

Libs/Optimize/Function/LegacyCorrespondenceFunction.h

@@ -144,7 +144,7 @@ class LegacyCorrespondenceFunction : public VectorFunction {
     m_Counter = 0;
     m_UseMeanEnergy = true;
     m_PointsUpdate = std::make_shared<vnl_matrix_type>(10, 10);
-    m_InverseCovMatrix = std::make_shared<vnl_matrix_type>(10, 10);
+    m_InverseCovMatrix = std::make_shared<Eigen::MatrixXd>(10, 10);
     m_points_mean = std::make_shared<vnl_matrix_type>(10, 10);
   }
   virtual ~LegacyCorrespondenceFunction() {}
@@ -164,7 +164,7 @@ class LegacyCorrespondenceFunction : public VectorFunction {
   bool m_UseMeanEnergy;
 
   std::shared_ptr<vnl_matrix_type> m_points_mean;       // 3Nx3N - used for energy computation
-  std::shared_ptr<vnl_matrix_type> m_InverseCovMatrix;  // 3NxM - used for energy computation
+  std::shared_ptr<Eigen::MatrixXd> m_InverseCovMatrix;  // 3NxM - used for energy computation
 };
 
 }  // namespace shapeworks

Libs/Utils/Utils.cpp

@@ -412,17 +412,17 @@ std::string Utils::int2str(int n, int number_of_zeros)
 // A, VXL does an extra allocation and copy. This could be many GB large. Additionally Eigen
 // is slightly faster.
 template<typename T>
-void Utils::multiply_into(vnl_matrix<T> &out, const vnl_matrix<T> &lhs, const vnl_matrix<T> &rhs) {
+void Utils::multiply_into(Eigen::MatrixXd &out, const vnl_matrix<T> &lhs, const vnl_matrix<T> &rhs) {
   typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMajorMatrix;
-  Eigen::Map<RowMajorMatrix> eig_out(out.data_block(), out.rows(), out.cols());
+  //Eigen::Map<RowMajorMatrix> eig_out(out.data_block(), out.rows(), out.cols());
   Eigen::Map<const RowMajorMatrix> eig_lhs(lhs.data_block(), lhs.rows(), lhs.cols());
   Eigen::Map<const RowMajorMatrix> eig_rhs(rhs.data_block(), rhs.rows(), rhs.cols());
-  eig_out.noalias() = eig_lhs * eig_rhs;
+  out.noalias() = eig_lhs * eig_rhs;
 }
 
 // Explicitly instantiate this function templatized over double
 template void
-Utils::multiply_into(vnl_matrix<double> &, const vnl_matrix<double> &, const vnl_matrix<double> &);
+Utils::multiply_into(Eigen::MatrixXd &, const vnl_matrix<double> &, const vnl_matrix<double> &);
 
 
 //--------------- average normal directions --------------------------------

Libs/Utils/Utils.h

@@ -21,6 +21,8 @@
 #endif
 #endif
 
+#include <Eigen/Core>
+
 #include <math.h>
 #include <iostream>
 #include <map>
@@ -101,7 +103,7 @@ class Utils{
 
     // matrix multiplication without an allocation for the output
     template<typename T>
-    static void multiply_into(vnl_matrix<T> &out, const vnl_matrix<T> &lhs, const vnl_matrix<T> &rhs);
+    static void multiply_into(Eigen::MatrixXd &out, const vnl_matrix<T> &lhs, const vnl_matrix<T> &rhs);
 
   //--------------- average normal directions --------------------------------
     /**