void HexGridFitting::getStorageMapFromPoseSeq()

in IsometricPatternMatcher/HexGridFitting.cpp [634:667]

• 29 lines of code
• 3 McCabe index (conditional complexity)

void HexGridFitting::getStorageMapFromPoseSeq(
    const std::vector<Eigen::Matrix2Xd>& transferDotsGroup) {
  int minX = 0;
  int maxX = 0;
  int minZ = 0;
  int maxZ = 0;
  // get cube coordinate from first pose: base pose
  Eigen::Matrix2Xd transferDotsBase = transferDotsGroup.at(0);
  Eigen::Matrix3Xi cubeCoorBase;
  std::vector<int> bfsProcessSeqBase;
  int startIdxBase = getCubeCoordinate(transferDotsBase, minX, maxX, minZ, maxZ,
                                       cubeCoorBase, bfsProcessSeqBase);
  if (transferDotsGroup.size() == 1) {
    bfsProcessSeq_ = bfsProcessSeqBase;
  }

  // loop over transferred dot group: calculate and merge cube coordinate
  for (int poseIdx = 1; poseIdx < transferDotsGroup.size(); ++poseIdx) {
    Eigen::Matrix2Xd transferDotsNew = transferDotsGroup.at(poseIdx);
    CHECK(transferDotsBase.cols() == transferDotsNew.cols())
        << "transferDots should have same size";
    // get cube coordinate from transferred dot of new pose
    Eigen::Matrix3Xi cubeCoorNew;
    std::vector<int> bfsProcessSeqNew;
    int startIdxNew = getCubeCoordinate(transferDotsNew, minX, maxX, minZ, maxZ,
                                        cubeCoorNew, bfsProcessSeqNew);
    cubeCoorBase =
        mergeCubeCoordinate(cubeCoorBase, cubeCoorNew, startIdxBase,
                            startIdxNew, minX, maxX, minZ, maxZ, poseIdx);
  }
  // build binary code
  binaryCode_ = Eigen::VectorXi::Constant(transferDotsBase.cols(), 1, 2);
  buildBinaryCode(cubeCoorBase, minX, maxX, minZ, maxZ);
}