Add SensitivityCalculation and its test

2023-05-12 14:13:56 +08:00
parent 2f98fc8819
commit 77e7d7be99
4 changed files with 397 additions and 3 deletions
--- a/src/transmissionReconstruction/dataFilter/sensitivityCalculations.cpp
+++ b/src/transmissionReconstruction/dataFilter/sensitivityCalculations.cpp
@@ -0,0 +1,224 @@
+#include "sensitivityCalculations.h"
+#include "Function1D.h"
+#include "Function2D.h"
+#include "Function3D.h"
+
+#include <cmath>
+#include <cstddef>
+#include <iostream>
+#include <omp.h>
+
+namespace {
+inline void cross(double *aVA, double *aVB, double *aVOut, int aIncX = 1) {
+  if (!aVA || !aVB || !aVOut)
+    return;
+  aVOut[0 * aIncX] =
+      (aVA[1 * aIncX] * aVB[2 * aIncX] - aVA[2 * aIncX] * aVB[1 * aIncX]);
+  aVOut[1 * aIncX] =
+      (aVA[2 * aIncX] * aVB[0 * aIncX] - aVA[0 * aIncX] * aVB[2 * aIncX]);
+  aVOut[2 * aIncX] =
+      (aVA[0 * aIncX] * aVB[1 * aIncX] - aVA[1 * aIncX] * aVB[0 * aIncX]);
+}
+inline Aurora::Matrix cross(const Aurora::Matrix &aMA,
+                            const Aurora::Matrix &aMB) {
+  if (!aMA.compareShape(aMB)) {
+    std::cerr << "cross must with same shape Matrix!" << std::endl;
+    return Aurora::Matrix();
+  }
+  if (aMA.getDimSize(0) != 3 && aMA.getDimSize(1) != 3) {
+    std::cerr << "cross must with Matrix(3,N) or Matrix(N,3)!" << std::endl;
+    return Aurora::Matrix();
+  }
+  auto output = Aurora::zeros(aMA.getDimSize(0), aMA.getDimSize(1));
+  if (aMA.getDimSize(0) == 3) {
+    for (size_t i = 0; i < aMA.getDimSize(1); ++i) {
+      cross(aMA.getData() + i * 3, aMB.getData() + i * 3,
+            output.getData() + i * 3);
+    }
+  } else {
+    for (size_t i = 0; i < aMA.getDimSize(0); ++i) {
+      cross(aMA.getData() + i, aMB.getData() + i, output.getData() + i,
+            aMA.getDimSize(0));
+    }
+  }
+
+  return output;
+}
+inline Aurora::Matrix getVector(const Aurora::Matrix &aM1,
+                                const Aurora::Matrix &aM2) {
+  auto temp = ::cross(Aurora::transpose(aM1), aM2);
+  temp = temp / Aurora::repmat(Aurora::sqrt(Aurora::sum(temp ^ 2, Aurora::Row)),
+                               1, temp.getDimSize(1));
+  return temp;
+}
+
+inline Aurora::Matrix sliceColumnsFrom4D(const Aurora::Matrix &aMSrc,
+                                         const Aurora::Matrix &aVCols,
+                                         const Aurora::Matrix &aVSlices) {
+  auto output = Aurora::zeros(aMSrc.getDimSize(0), aVCols.getDataSize(),
+                              aVSlices.getDataSize());
+  for (size_t i = 0; i < aVSlices.getDataSize(); i++) {
+    size_t sliceIdx = aVSlices.getData()[i];
+    for (size_t j = 0; j < aVCols.getDataSize(); j++) {
+      size_t colIdx = aVCols.getData()[j];
+      output(Aurora::$, j, i) = aMSrc(Aurora::$, colIdx - 1, sliceIdx - 1);
+    }
+  }
+  return output;
+}
+
+#define PI 3.141592653589793238462
+} // namespace
+
+namespace Recon {
+Aurora::Matrix getSensitivity(const Aurora::Matrix &aMSensmap,
+                              Aurora::Matrix aVSenderNormal,
+                              const Aurora::Matrix &aMDirToReceiver) {
+  aVSenderNormal.forceReshape(aVSenderNormal.getDataSize(), 1, 1);
+  auto dir = aMDirToReceiver;
+  dir = dir / Aurora::repmat(Aurora::sqrt(Aurora::sum(dir ^ 2)),
+                             dir.getDimSize(0), 1);
+  aVSenderNormal = Aurora::repmat(aVSenderNormal, 1, dir.getDimSize(1));
+  auto xyn = Aurora::zeros(1, 3);
+  xyn.getData()[2] = 1;
+  xyn = Aurora::repmat(xyn, dir.getDimSize(1), 1);
+  auto xyd = ::getVector(::cross(Aurora::transpose(xyn), dir), xyn);
+  auto xysn = ::getVector(::cross(Aurora::transpose(xyn), aVSenderNormal), xyn);
+  auto xzn = ::getVector(aVSenderNormal, xyn);
+  auto xzd = ::getVector(::cross(Aurora::transpose(xzn), dir), xzn);
+  auto xzsn = ::getVector(::cross(Aurora::transpose(xzn), aVSenderNormal), xzn);
+  auto xi = Aurora::round(Aurora::acos(Aurora::sum(xyd * xysn, Aurora::Row)) *
+                          180 / PI);
+  auto zi = Aurora::round(Aurora::acos(Aurora::sum(xzd * xzsn, Aurora::Row)) *
+                          180 / PI);
+  for (size_t i = 0; i < xi.getDataSize(); ++i) {
+    if (xi.getData()[i] > 90)
+      xi.getData()[i] -= 90;
+    else if (xi.getData()[i] < 0)
+      xi.getData()[i] += 90;
+    if (zi.getData()[i] > 90)
+      zi.getData()[i] -= 90;
+    else if (zi.getData()[i] < 0)
+      zi.getData()[i] += 90;
+  }
+  double msensx = std::round(((double)aMSensmap.getDimSize(0) - 1.0) * 0.5);
+  double msensY = std::round(((double)aMSensmap.getDimSize(1) - 1.0) * 0.5);
+  xi = xi + 1 + msensx;
+  zi = zi + 1 + msensY;
+  for (size_t i = 0; i < xi.getDataSize(); ++i) {
+    if (xi.getData()[i] < 1) {
+      xi.getData()[i] = 1;
+    } else if (xi.getData()[i] > 181) {
+      xi.getData()[i] = 181;
+    }
+    if (zi.getData()[i] > 181) {
+      zi.getData()[i] = 181;
+    } else if (zi.getData()[i] < 1) {
+      zi.getData()[i] = 1;
+    }
+  }
+  Aurora::nantoval(xi, 1.0);
+  Aurora::nantoval(zi, 1.0);
+  auto idx = Aurora::sub2ind(Aurora::size(aMSensmap), {xi, zi});
+  auto out = Aurora::zeros(idx.getDataSize(), 1, 1);
+  for (size_t i = 0; i < idx.getDataSize(); ++i) {
+    out.getData()[i] = aMSensmap.getData()[(int)idx.getData()[i] - 1];
+  }
+  return out;
+}
+
+Aurora::Matrix precalcSensitivityForTAS(const Aurora::Matrix &aMSensChar,
+                                        const Aurora::Matrix &aMStartPositions,
+                                        const Aurora::Matrix &aMStartNormals,
+                                        const Aurora::Matrix &aMEndPositions) {
+  auto sens = Aurora::zeros(aMEndPositions.getDimSize(1),
+                            aMStartPositions.getDimSize(1));
+  for (size_t i = 0; i < aMStartPositions.getDimSize(1); ++i) {
+    auto dirVector = aMEndPositions -
+                     Aurora::repmat(aMStartPositions(Aurora::$, i).toMatrix(),
+                                    1, aMEndPositions.getDimSize(1));
+    sens(Aurora::$, i) = getSensitivity(
+        aMSensChar, aMStartNormals(Aurora::$, i).toMatrix(), dirVector);
+  }
+  return sens;
+}
+
+std::vector<Aurora::Matrix>
+combineSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aMSenderSens,
+                   const Aurora::Matrix &aMReceiverSens) {
+  double maxReceiverElementRange =
+      Aurora::max(aVReceiverElementRange).getScalar();
+  double maxReceiverTASRange = Aurora::max(aVReceiverTASRange).getScalar();
+  double maxSenderElementRange = Aurora::max(aVSenderElementRange).getScalar();
+  double maxSenderTASRange = Aurora::max(aVSenderTASRange).getScalar();
+  std::vector<Aurora::Matrix> ret;
+  for (size_t i = 0; i < maxSenderTASRange; ++i) {
+    ret.emplace_back(Aurora::zeros(maxReceiverElementRange, maxReceiverTASRange,
+                                   maxSenderElementRange));
+  }
+  size_t countSE = 0;
+  for (size_t i = 0; i < aVSenderTASRange.getDataSize(); i++) {
+    auto se = aVReceiverTASRange.getData()[i];
+    for (size_t j = 0; j < aVSenderElementRange.getDataSize(); j++) {
+      auto sn = aVSenderElementRange.getData()[j];
+      size_t countRE = 0;
+      for (size_t k = 0; k < aVReceiverTASRange.getDataSize(); k++) {
+        auto re = aVReceiverTASRange.getData()[k];
+        for (size_t n = 0; n < aVReceiverElementRange.getDataSize(); n++) {
+          auto rn = aVReceiverElementRange.getData()[n];
+          ret[se - 1](rn - 1, re - 1, sn - 1) =
+              aMSenderSens(countRE, countSE).toMatrix().getScalar() *
+              aMReceiverSens(countSE, countRE).toMatrix().getScalar();
+          countRE++;
+        }
+      }
+      countSE++;
+    }
+  }
+  return ret;
+}
+
+std::vector<Aurora::Matrix>
+precalcSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aVMotorPos, const geo &aTASElements) {
+  size_t dim = aTASElements.receiverPositions[0].getDimSize(0);
+  size_t pos1Idx = (int)aVMotorPos.getData()[0] - 1;
+  auto receiverPositions = aTASElements.receiverPositions[pos1Idx];
+  receiverPositions = ::sliceColumnsFrom4D(
+      receiverPositions, aVReceiverElementRange, aVReceiverTASRange);
+  receiverPositions.forceReshape(dim, receiverPositions.getDataSize() / dim, 1);
+
+  auto receiverNormals = aTASElements.receiverNormals[pos1Idx];
+  receiverNormals = ::sliceColumnsFrom4D(
+      receiverNormals, aVReceiverElementRange, aVReceiverTASRange);
+  receiverNormals.forceReshape(dim, receiverNormals.getDataSize() / dim, 1);
+
+  auto senderPositions = aTASElements.senderPositions[pos1Idx];
+  senderPositions = ::sliceColumnsFrom4D(senderPositions, aVSenderElementRange,
+                                         aVSenderTASRange);
+  senderPositions.forceReshape(dim, senderPositions.getDataSize() / dim, 1);
+
+  auto senderNormals = aTASElements.senderNormals[pos1Idx];
+  senderNormals = ::sliceColumnsFrom4D(senderNormals, aVSenderElementRange,
+                                       aVSenderTASRange);
+  senderNormals.forceReshape(dim, senderNormals.getDataSize() / dim, 1);
+
+  auto senderSens = precalcSensitivityForTAS(
+      aTASElements.sensChar, senderPositions, senderNormals, receiverPositions);
+  auto receiverSens =
+      precalcSensitivityForTAS(aTASElements.sensChar, receiverPositions,
+                               receiverNormals, senderPositions);
+
+  auto sensPrecalc = combineSensitivity(
+      aVSenderTASRange, aVSenderElementRange, aVReceiverTASRange,
+      aVReceiverElementRange, senderSens, receiverSens);
+  return sensPrecalc;
+}
+} // namespace Recon
--- a/src/transmissionReconstruction/dataFilter/sensitivityCalculations.h
+++ b/src/transmissionReconstruction/dataFilter/sensitivityCalculations.h
@@ -0,0 +1,44 @@
+#ifndef __SENSITIVITYCALCULATIONS_H__
+#define __SENSITIVITYCALCULATIONS_H__
+#include <vector>
+
+#include "Matrix.h"
+
+namespace Recon {
+struct geo {
+  //[471][4]
+  Aurora::Matrix headTable;
+  //[181][181]
+  Aurora::Matrix sensChar;
+  std::vector<Aurora::Matrix> senderNormals;
+  std::vector<Aurora::Matrix> receiverNormals;
+  std::vector<Aurora::Matrix> senderPositions;
+  std::vector<Aurora::Matrix> receiverPositions;
+};
+
+Aurora::Matrix getSensitivity(const Aurora::Matrix &aMSensmap,
+                              Aurora::Matrix aVSenderNormal,
+                              const Aurora::Matrix &aMDirToReceiver);
+
+Aurora::Matrix precalcSensitivityForTAS(const Aurora::Matrix &aMSensChar,
+                                        const Aurora::Matrix &aMStartPositions,
+                                        const Aurora::Matrix &aMStartNormals,
+                                        const Aurora::Matrix &aMEndPositions);
+
+std::vector<Aurora::Matrix>
+combineSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aMSenderSens,
+                   const Aurora::Matrix &aMReceiverSens);
+
+std::vector<Aurora::Matrix>
+precalcSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aVMotorPos,
+                   const geo &aTASElements);
+} // namespace Recon
+#endif // __SENSITIVITYCALCULATIONS_H__
--- a/src/transmissionReconstruction/dataPreperation.h
+++ b/src/transmissionReconstruction/dataPreperation.h
@@ -3,8 +3,8 @@
 #include "Matrix.h"
 namespace Recon {

-Aurora::Matrix distanceBetweenTwoPoints(Aurora::Matrix aPtsA,
-                                        Aurora::Matrix aPtsB);
+Aurora::Matrix distanceBetweenTwoPoints(Aurora::Matrix aMPtsA,
+                                        Aurora::Matrix aMPtsB);

 Aurora::Matrix calculateWaterTemperature(Aurora::Matrix aMWaterTempS,
                                         Aurora::Matrix aMWaterTempR,
@@ -12,7 +12,7 @@ Aurora::Matrix calculateWaterTemperature(Aurora::Matrix aMWaterTempS,
                                         Aurora::Matrix aVrl,
                                         Aurora::Matrix aVmp);
                                         
-Aurora::Matrix prepareAScansForTransmissionDetection(const Aurora::Matrix& aMaScanBlock, const Aurora::Matrix aVGainBlock);
+Aurora::Matrix prepareAScansForTransmissionDetection(const Aurora::Matrix& aMAScanBlock, const Aurora::Matrix aVGainBlock);
 } // namespace Recon

 #endif // __DATAPREPERATION_H__