| 193 | } |
| 194 | |
| 195 | void SDFMap::updateESDF3d() { |
| 196 | Eigen::Vector3i min_esdf = md_->local_bound_min_; |
| 197 | Eigen::Vector3i max_esdf = md_->local_bound_max_; |
| 198 | |
| 199 | if (mp_->optimistic_) { |
| 200 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 201 | for (int y = min_esdf[1]; y <= max_esdf[1]; y++) { |
| 202 | fillESDF( |
| 203 | [&](int z) { |
| 204 | return md_->occupancy_buffer_inflate_[toAddress(x, y, z)] == 1 ? |
| 205 | 0 : |
| 206 | std::numeric_limits<double>::max(); |
| 207 | }, |
| 208 | [&](int z, double val) { md_->tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2], |
| 209 | max_esdf[2], 2); |
| 210 | } |
| 211 | } else { |
| 212 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 213 | for (int y = min_esdf[1]; y <= max_esdf[1]; y++) { |
| 214 | fillESDF( |
| 215 | [&](int z) { |
| 216 | int adr = toAddress(x, y, z); |
| 217 | return (md_->occupancy_buffer_inflate_[adr] == 1 || |
| 218 | md_->occupancy_buffer_[adr] < mp_->clamp_min_log_ - 1e-3) ? |
| 219 | 0 : |
| 220 | std::numeric_limits<double>::max(); |
| 221 | }, |
| 222 | [&](int z, double val) { md_->tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2], |
| 223 | max_esdf[2], 2); |
| 224 | } |
| 225 | } |
| 226 | |
| 227 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 228 | for (int z = min_esdf[2]; z <= max_esdf[2]; z++) { |
| 229 | fillESDF([&](int y) { return md_->tmp_buffer1_[toAddress(x, y, z)]; }, |
| 230 | [&](int y, double val) { md_->tmp_buffer2_[toAddress(x, y, z)] = val; }, min_esdf[1], |
| 231 | max_esdf[1], 1); |
| 232 | } |
| 233 | for (int y = min_esdf[1]; y <= max_esdf[1]; y++) |
| 234 | for (int z = min_esdf[2]; z <= max_esdf[2]; z++) { |
| 235 | fillESDF([&](int x) { return md_->tmp_buffer2_[toAddress(x, y, z)]; }, |
| 236 | [&](int x, double val) { |
| 237 | md_->distance_buffer_[toAddress(x, y, z)] = mp_->resolution_ * std::sqrt(val); |
| 238 | }, |
| 239 | min_esdf[0], max_esdf[0], 0); |
| 240 | } |
| 241 | |
| 242 | if (mp_->signed_dist_) { |
| 243 | // Compute negative distance |
| 244 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 245 | for (int y = min_esdf[1]; y <= max_esdf[1]; y++) { |
| 246 | fillESDF( |
| 247 | [&](int z) { |
| 248 | return md_->occupancy_buffer_inflate_[x * mp_->map_voxel_num_(1) * |
| 249 | mp_->map_voxel_num_(2) + |
| 250 | y * mp_->map_voxel_num_(2) + z] == 0 ? |
| 251 | 0 : |
| 252 | std::numeric_limits<double>::max(); |