| 179 | } |
| 180 | |
| 181 | void SDFMap::updateESDF3d() |
| 182 | { |
| 183 | Eigen::Vector3i min_esdf; |
| 184 | Eigen::Vector3i max_esdf; |
| 185 | if (is_lidar_) { |
| 186 | min_esdf = md_->local_bound_min_; |
| 187 | max_esdf = md_->local_bound_max_; |
| 188 | std::vector<Eigen::Vector3i> ESDF_local_bound; |
| 189 | |
| 190 | ESDF_local_bound.push_back(min_esdf); |
| 191 | ESDF_local_bound.push_back(max_esdf); |
| 192 | |
| 193 | std_msgs::Int32MultiArray msg; |
| 194 | for (const auto& vector_elem : ESDF_local_bound) { |
| 195 | msg.data.push_back(vector_elem.x()); |
| 196 | msg.data.push_back(vector_elem.y()); |
| 197 | msg.data.push_back(vector_elem.z()); |
| 198 | } |
| 199 | lidar_local_update_bound_pub.publish(msg); |
| 200 | } |
| 201 | else if (md_->camera_local_update_bound_) { |
| 202 | min_esdf = md_->lidar_update_bound_[0]; |
| 203 | max_esdf = md_->lidar_update_bound_[1]; |
| 204 | // md_->camera_local_update_bound_ = false; |
| 205 | md_->local_bound_min_ = min_esdf; |
| 206 | md_->local_bound_max_ = max_esdf; |
| 207 | } |
| 208 | if (mp_->optimistic_) { |
| 209 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 210 | for (int y = min_esdf[1]; y <= max_esdf[1]; y++) { |
| 211 | fillESDF( |
| 212 | [&](int z) { |
| 213 | return md_->occupancy_buffer_inflate_[toAddress(x, y, z)] == 1 ? |
| 214 | 0 : |
| 215 | std::numeric_limits<double>::max(); |
| 216 | }, |
| 217 | [&](int z, double val) { md_->tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2], |
| 218 | max_esdf[2], 2); |
| 219 | } |
| 220 | } |
| 221 | else { |
| 222 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 223 | for (int y = min_esdf[1]; y <= max_esdf[1]; y++) { |
| 224 | fillESDF( |
| 225 | [&](int z) { |
| 226 | int adr = toAddress(x, y, z); |
| 227 | return (md_->occupancy_buffer_inflate_[adr] == 1 || |
| 228 | md_->occupancy_buffer_[adr] < mp_->clamp_min_log_ - 1e-3) ? |
| 229 | 0 : |
| 230 | std::numeric_limits<double>::max(); |
| 231 | }, |
| 232 | [&](int z, double val) { md_->tmp_buffer1_[toAddress(x, y, z)] = val; }, min_esdf[2], |
| 233 | max_esdf[2], 2); |
| 234 | } |
| 235 | } |
| 236 | |
| 237 | for (int x = min_esdf[0]; x <= max_esdf[0]; x++) |
| 238 | for (int z = min_esdf[2]; z <= max_esdf[2]; z++) { |
no test coverage detected