Eigen::Matrix3Xi HexGridFitting::mergeCubeCoordinate()

in IsometricPatternMatcher/HexGridFitting.cpp [585:633]
41 lines of code
10 McCabe index (conditional complexity)

Eigen::Matrix3Xi HexGridFitting::mergeCubeCoordinate(
    const Eigen::Matrix3Xi& cubeCoor1, const Eigen::Matrix3Xi& cubeCoor2,
    int startIdx1, int startIdx2, int& minX, int& maxX, int& minZ, int& maxZ,
    int poseIdx) {
  // merge cube coordinate results from two poses
  Eigen::Matrix3Xi cubeCoor;
  cubeCoor.resize(3, cubeCoor1.cols());
  cubeCoor.fill(std::numeric_limits<int>::max());  // not detected =max int
  // calculate coordinate translation
  Eigen::Matrix3Xi cubeCoorDiff;
  cubeCoorDiff.resize(3, 1);
  cubeCoorDiff << 0, 0, 0;
  if (startIdx1 == startIdx2) {
    CHECK(cubeCoor1.col(startIdx2) == cubeCoor2.col(startIdx2))
        << "Both startIdx should have 0,0,0 cube coordinate";
  } else {
    CHECK(cubeCoor1(0, startIdx2) != std::numeric_limits<int>::max())
        << "we assume the center of transferDot2 has valid cubeCoord1";
    cubeCoorDiff = cubeCoor1.col(startIdx2) - cubeCoor2.col(startIdx2);
  }
  LOG(INFO) << fmt::format("translation is {}, {}, {}", cubeCoorDiff(0, 0),
                           cubeCoorDiff(1, 0), cubeCoorDiff(2, 0));
  // calculate coordinate rotation
  int rotIdx = determineRotation(cubeCoor1, cubeCoor2, cubeCoorDiff);
  LOG(INFO) << fmt::format("rotation is left {} degree", rotIdx * 60);

  // Merge coordinate and update minX, maxX, minZ, maxZ
  for (int i = 0; i < cubeCoor.cols(); i++) {
    if (cubeCoor1(0, i) != std::numeric_limits<int>::max()) {
      // Add cube coordinate from valid cubeCoor1
      cubeCoor.col(i) = cubeCoor1.col(i);
      if (poseIdx == 1) {
        // we only push back index i at first time it is met
        bfsProcessSeq_.push_back(i);
      }
    } else if ((cubeCoor2(0, i) != std::numeric_limits<int>::max())) {
      // Add cube coordinate from valid cubeCoor2 while cubeCoor1 is invalid
      cubeCoor.col(i) = doLeftRotate(cubeCoor2.col(i), rotIdx) + cubeCoorDiff;
      if (minX > cubeCoor(0, i)) minX = cubeCoor(0, i);
      if (minZ > cubeCoor(2, i)) minZ = cubeCoor(2, i);
      if (maxX < cubeCoor(0, i)) maxX = cubeCoor(0, i);
      if (maxZ < cubeCoor(2, i)) maxZ = cubeCoor(2, i);
      bfsProcessSeq_.push_back(i);
    }
  }
  LOG(INFO) << fmt::format("total processed points from 2 poses are {}",
                           bfsProcessSeq_.size());
  return cubeCoor;
}