void Octree::extrapolate_signal()

in octree/octree/octree.cpp [666:806]

• 124 lines of code
• 42 McCabe index (conditional complexity)

void Octree::extrapolate_signal() {
  const int depth = oct_info_.depth();
  const int full_layer = oct_info_.full_layer();
  const int nnum_depth = oct_info_.node_num(depth);
  const float imul = 2.0f / sqrtf(3.0f);

  const int channel_normal = avg_normals_[depth].size() / nnum_depth;
  const int channel_feature = avg_features_[depth].size() / nnum_depth;
  const int channel_label = avg_labels_[depth].size() / nnum_depth;

  const bool has_dis = !displacement_[depth].empty();
  const bool has_normal = !avg_normals_[depth].empty();
  const bool has_feature = !avg_features_[depth].empty();
  const bool has_label = !avg_labels_[depth].empty();

  for (int d = depth; d >= full_layer; --d) {
    const int nnum_d = oct_info_.node_num(d);
    const vector<int>& children_d = children_[d];
    vector<float>& normal_d = avg_normals_[d];
    vector<float>& label_d = avg_labels_[d];
    vector<float>& feature_d = avg_features_[d];
    vector<float>& displacement_d = displacement_[d];

    for (int i = 0; i < nnum_d; ++i) {
      if (node_type(children_d[i]) != kLeaf) continue;
      int id = i % 8;
      int i_base = i - id;

      float count = ESP; // the non-empty node number
      for (int j = i_base; j < i_base + 8; ++j) {
        if (node_type(children_d[j]) != kLeaf) count += 1.0f;
      }

      vector<float> n_avg(channel_normal, 0.0f);
      if (has_normal) {
        for (int j = i_base; j < i_base + 8; ++j) {
          if (node_type(children_d[j]) == kLeaf) continue;
          for (int c = 0; c < channel_normal; ++c) {
            n_avg[c] += normal_d[c * nnum_d + j];
          }
        }

        float ilen = 1.0f / (norm2(n_avg) + ESP);
        for (int c = 0; c < channel_normal; ++c) {
          n_avg[c] *= ilen;
          normal_d[c * nnum_d + i] = n_avg[c];  // output
        }
      }

      vector<float> f_avg(channel_feature, 0.0f);
      if (has_feature) {
        for (int j = i_base; j < i_base + 8; ++j) {
          if (node_type(children_d[j]) == kLeaf) continue;
          for (int c = 0; c < channel_feature; ++c) {
            f_avg[c] += feature_d[c * nnum_d + j];
          }
        }

        for (int c = 0; c < channel_feature; ++c) {
          f_avg[c] /= count;
          feature_d[c * nnum_d + i] = f_avg[c]; // output
        }
      }

      vector<int> l_avg(max_label_, 0);
      if (has_label) {
        int valid_num = 0;
        for (int j = i_base; j < i_base + 8; ++j) {
          int l = static_cast<int>(label_d[j]);
          if (l < 0) { continue; }  // invalid labels
          l_avg[l] += 1;
          valid_num += 1;
        }
        if (valid_num > 0) {
          label_d[i] = static_cast<float>(std::distance(l_avg.begin(),
                      std::max_element(l_avg.begin(), l_avg.end())));
        }
      }

      if (has_dis && count > 0.5f) {
        float xyzs[8][3] = {
          {0, 0, 0}, {0, 0, 1.0f}, {0, 1.0f, 0}, {0, 1.0f, 1.0f},
          {1.0f, 0, 0}, {1.0f, 0, 1.0f}, {1.0f, 1.0f, 0}, {1.0f, 1.0f, 1.0f},
        };
        float dis = 0;
        for (int j = i_base; j < i_base + 8; ++j) {
          if (node_type(children_d[j]) == kLeaf) continue;
          dis += displacement_d[j];
          for (int c = 0; c < channel_normal; ++c) {
            dis += normal_d[c * nnum_d + j] * (xyzs[j % 8][c] - xyzs[id][c]) * imul;
          }
        }
        dis /= count;
        if (dis > 3.0f) dis = 3.0f;
        if (dis < -3.0f) dis = -3.0f;
        if (fabsf(dis) < 1.0f) {
          //bool has_intersection = false;
          // make the voxel has no intersection with the current voxel
          uint32 cube_cases = 0;
          for (int k = 0; k < 8; ++k) {
            float fval = dis;
            for (int j = 0; j < 3; ++j) {
              fval += (0.5f - MarchingCube::corner_[k][j]) * n_avg[j] * imul;
            }
            if (fval < 0) cube_cases |= (1 << k);
          }
          if (cube_cases != 255 && cube_cases != 0) {
            dis = dis < 0 ? -1.0f : 1.0f;
          }
        }

        displacement_d[i] = dis;
      }

      if (has_dis && count < 0.5f) {
        // find the closest point
        int j_min = -1;
        float pti[3], ptj[3], dis_min = 1.0e30f;
        key2xyz(pti, keys_[d][i], d);
        for (int j = 0; j < nnum_d; ++j) {
          if (node_type(children_d[j]) == kLeaf) continue;
          key2xyz(ptj, keys_[d][j], d);
          float dd = fabsf(pti[0] - ptj[0]) + fabsf(pti[1] - ptj[1]) + fabsf(pti[2] - ptj[2]);
          if (dd < dis_min) {
            dis_min = dd;
            j_min = j;
          }
        }
        // calc the displacement
        float dis = displacement_d[j_min];
        key2xyz(ptj, keys_[d][j_min], d);
        for (int c = 0; c < channel_normal; ++c) {
          dis += normal_d[c * nnum_d + j_min] * (ptj[c] - pti[c]) * imul;
        }
        if (dis > 0.0f) dis = 2.0f;
        if (dis < 0.0f) dis = -2.0f;
        displacement_d[i] = dis;
      }
    }
  }
}