message: Fix better this time.
diff: @@ -199,26 +199,20 @@ void EsdfIntegrator::updateFromTsdfBlocks(const BlockIndexList& tsdf_blocks,
  } else {
  // If this voxel DID exist before.
  // There are three main options:
  - // (1) raise: esdf or tsdf is fixed, and tsdf is further from surface
  - // than it used to be, or if tsdf isn't fixed by esdf is.
  - // (2) lower: esdf or tsdf is fixed, and tsdf is closer to surface than
  + // (1a): if was fixed before but not anymore, raise.
  + // (1) lower: esdf or tsdf is fixed, and tsdf is closer to surface than
  // it used to be.
  + // (2) raise: esdf or tsdf is fixed, and tsdf is further from surface
  + // than it used to be.
  // (3) sign flip: tsdf and esdf have different signs, otherwise the
  // lower and raise rules apply as above.
  - if ((!tsdf_fixed && esdf_voxel.fixed) ||
  - (esdf_voxel.distance > 0 &&
  - tsdf_voxel.distance - config_.min_diff_m > esdf_voxel.distance) ||
  - (esdf_voxel.distance <= 0 &&
  - tsdf_voxel.distance + config_.min_diff_m < esdf_voxel.distance)) {
  - // Raise.
  - esdf_voxel.fixed = tsdf_fixed;
  - if (esdf_voxel.fixed) {
  - esdf_voxel.distance = tsdf_voxel.distance;
  - } else {
  + if (tsdf_fixed || esdf_voxel.fixed) {
  + if (!tsdf_fixed && esdf_voxel.fixed) {
  + // New case: have to raise the voxel
  esdf_voxel.distance =
  signum(tsdf_voxel.distance) * config_.default_distance_m;
  - }
  esdf_voxel.parent.setZero();
  + esdf_voxel.fixed = false;
  raise_.push(global_index);
  esdf_voxel.in_queue = true;
  open_.push(global_index, esdf_voxel.distance);
  @@ -241,6 +235,26 @@ void EsdfIntegrator::updateFromTsdfBlocks(const BlockIndexList& tsdf_blocks,
  esdf_voxel.in_queue = true;
  open_.push(global_index, esdf_voxel.distance);
  num_lower++;
  + } else if ((esdf_voxel.distance > 0 &&
  + tsdf_voxel.distance - config_.min_diff_m >
  + esdf_voxel.distance) ||
  + (esdf_voxel.distance <= 0 &&
  + tsdf_voxel.distance + config_.min_diff_m <
  + esdf_voxel.distance)) {
  + // Raise.
  + esdf_voxel.fixed = tsdf_fixed;
  + if (esdf_voxel.fixed) {
  + esdf_voxel.distance = tsdf_voxel.distance;
  + } else {
  + esdf_voxel.distance =
  + signum(tsdf_voxel.distance) * config_.default_distance_m;
  + }
  + esdf_voxel.parent.setZero();
  + raise_.push(global_index);
  + esdf_voxel.in_queue = true;
  + open_.push(global_index, esdf_voxel.distance);
  + num_raise++;
  + }
  } else if (signum(tsdf_voxel.distance) != signum(esdf_voxel.distance)) {
  // This means ESDF was positive and TSDF is negative.
  // So lower.
  