in octree/octree/octree.cpp [454:664]
void Octree::calc_signal(const bool calc_normal_err, const bool calc_dist_err) {
const int depth = oct_info_.depth();
const int depth_adp = oct_info_.adaptive_layer();
const int nnum_depth = oct_info_.node_num(depth);
const float imul = 2.0f / sqrtf(3.0f);
const vector<int>& children_depth = children_[depth];
const vector<float>& normal_depth = avg_normals_[depth];
const vector<float>& pt_depth = avg_pts_[depth];
const vector<float>& feature_depth = avg_features_[depth];
const vector<float>& label_depth = avg_labels_[depth];
const int channel_pt = pt_depth.size() / nnum_depth;
const int channel_normal = normal_depth.size() / nnum_depth;
const int channel_feature = feature_depth.size() / nnum_depth;
const int channel_label = label_depth.size() / nnum_depth;
const bool has_pt = !pt_depth.empty();
const bool has_dis = !displacement_[depth].empty();
const bool has_normal = !normal_depth.empty();
const bool has_feature = !feature_depth.empty();
const bool has_label = !label_depth.empty();
if (calc_normal_err) normal_err_[depth].resize(nnum_depth, 1.0e20f);
if (calc_dist_err) distance_err_[depth].resize(nnum_depth, 1.0e20f);
for (int d = depth - 1; d >= 0; --d) {
const vector<int>& dnum_d = dnum_[d];
const vector<int>& didx_d = didx_[d];
const vector<int>& children_d = children_[d];
const vector<uintk>& key_d = keys_[d];
const float scale = static_cast<float>(1 << (depth - d));
vector<float>& normal_d = avg_normals_[d];
vector<float>& pt_d = avg_pts_[d];
vector<float>& label_d = avg_labels_[d];
vector<float>& feature_d = avg_features_[d];
vector<float>& displacement_d = displacement_[d];
vector<float>& normal_err_d = normal_err_[d];
vector<float>& distance_err_d = distance_err_[d];
const int nnum_d = oct_info_.node_num(d);
int channel_dis = has_normal ? 1 : 4;
if (has_normal) normal_d.assign(nnum_d * channel_normal, 0.0f);
if (has_pt) pt_d.assign(nnum_d * channel_pt, 0.0f);
if (has_feature) feature_d.assign(nnum_d * channel_feature, 0.0f);
if (has_label) label_d.assign(nnum_d * channel_label, -1.0f);// !!! init as -1
if (has_dis) displacement_d.assign(channel_dis * nnum_d, 0.0f);
if (calc_normal_err) normal_err_d.assign(nnum_d, 1.0e20f); // !!! initialized
if (calc_dist_err) distance_err_d.assign(nnum_d, 1.0e20f); // !!! as 1.0e20f
for (int i = 0; i < nnum_d; ++i) {
if (node_type(children_d[i]) == kLeaf) continue;
vector<float> n_avg(channel_normal, 0.0f);
if (has_normal) {
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
for (int c = 0; c < channel_normal; ++c) {
n_avg[c] += normal_depth[c * nnum_depth + j];
}
}
float len = norm2(n_avg);
if (len < 1.0e-6f) {
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
for (int c = 0; c < channel_normal; ++c) {
n_avg[c] = normal_depth[c * nnum_depth + j];
}
}
len = norm2(n_avg) + ESP;
}
for (int c = 0; c < channel_normal; ++c) {
n_avg[c] /= len;
normal_d[c * nnum_d + i] = n_avg[c]; // output
}
}
float count = ESP; // the non-empty leaf node in the finest layer
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) != kLeaf) count += 1.0f;
}
vector<float> pt_avg(channel_pt, 0.0f);
if (has_pt) {
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
for (int c = 0; c < channel_pt; ++c) {
pt_avg[c] += pt_depth[c * nnum_depth + j];
}
}
for (int c = 0; c < channel_pt; ++c) {
pt_avg[c] /= count * scale; // !!! note the scale
pt_d[c * nnum_d + i] = pt_avg[c]; // output
}
}
vector<float> f_avg(channel_feature, 0.0f);
if (has_feature) {
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
for (int c = 0; c < channel_feature; ++c) {
f_avg[c] += feature_depth[c * nnum_depth + 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 = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
int l = static_cast<int>(label_depth[j]);
if (l < 0) continue;
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())));
}
}
uintk ptu_base[3];
compute_pt(ptu_base, key_d[i], d);
float pt_base[3] = { float(ptu_base[0]), float(ptu_base[1]), float(ptu_base[2]) };
if (has_dis) {
float dis_avg[4] = { 0.0f, 0.0f, 0.0f, 0.0f };
for (int c = 0; c < 3; ++c) {
float fract_part = pt_avg[c] - pt_base[c];
dis_avg[c] = fract_part - 0.5f;
if (has_normal) {
dis_avg[3] += dis_avg[c] * n_avg[c];
} else {
dis_avg[3] = 1.0f;
}
}
if (!has_normal) {
for (int c = 0; c < 3; ++c) {
displacement_d[c * nnum_d + i] = dis_avg[c];
}
displacement_d[3 * nnum_d + i] = dis_avg[3] * imul;
} else {
displacement_d[i] = dis_avg[3] * imul; // IMPORTANT: RESCALE
}
}
float nm_err = 0.0f;
if (calc_normal_err && has_normal && d >= depth_adp) {
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
for (int c = 0; c < 3; ++c) {
float tmp = normal_depth[c * nnum_depth + j] - n_avg[c];
nm_err += tmp * tmp;
}
}
nm_err /= count;
normal_err_d[i] = nm_err;
}
if (calc_dist_err && has_pt && d >= depth_adp) {
// the error from the original geometry to the averaged geometry
float distance_max1 = -1.0f;
// !!! note the scale
float pt_avg1[3] = { pt_avg[0] * scale, pt_avg[1] * scale, pt_avg[2] * scale };
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
float dis = 0.0f;
for (int c = 0; c < 3; ++c) {
dis += (pt_depth[c * nnum_depth + j] - pt_avg1[c]) * n_avg[c];
}
dis = fabsf(dis);
if (dis > distance_max1) distance_max1 = dis;
}
// the error from the averaged geometry to the original geometry
float distance_max2 = -1;
vector<float> vtx;
intersect_cube(vtx, pt_avg.data(), pt_base, n_avg.data());
if (vtx.empty()) distance_max2 = 5.0e10f; // !!! the degenerated case, ||n_avg|| == 0
for (auto& v : vtx) v *= scale; // !!! note the scale
for (int k = 0; k < vtx.size() / 3; ++k) {
// min
float distance_min = 1.0e30f;
for (int j = didx_d[i]; j < didx_d[i] + dnum_d[i]; ++j) {
if (node_type(children_depth[j]) == kLeaf) continue;
float dis = 0.0f;
for (int c = 0; c < 3; ++c) {
float ptc = pt_depth[c * nnum_depth + j] - vtx[3 * k + c];
dis += ptc * ptc;
}
dis = sqrtf(dis);
if (dis < distance_min) distance_min = dis;
}
// max
if (distance_min > distance_max2) distance_max2 = distance_min;
}
distance_err_d[i] = std::max<float>(distance_max2, distance_max1);
}
}
}
}