| 558 | } |
| 559 | |
| 560 | double SDFMap::getDistWithGrad(const Eigen::Vector3d& pos, Eigen::Vector3d& grad) { |
| 561 | if (!isInMap(pos)) { |
| 562 | grad.setZero(); |
| 563 | return 0; |
| 564 | } |
| 565 | |
| 566 | /* trilinear interpolation */ |
| 567 | Eigen::Vector3d pos_m = pos - 0.5 * mp_->resolution_ * Eigen::Vector3d::Ones(); |
| 568 | Eigen::Vector3i idx; |
| 569 | posToIndex(pos_m, idx); |
| 570 | Eigen::Vector3d idx_pos, diff; |
| 571 | indexToPos(idx, idx_pos); |
| 572 | diff = (pos - idx_pos) * mp_->resolution_inv_; |
| 573 | |
| 574 | double values[2][2][2]; |
| 575 | for (int x = 0; x < 2; x++) |
| 576 | for (int y = 0; y < 2; y++) |
| 577 | for (int z = 0; z < 2; z++) { |
| 578 | Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z); |
| 579 | values[x][y][z] = getDistance(current_idx); |
| 580 | } |
| 581 | |
| 582 | double v00 = (1 - diff[0]) * values[0][0][0] + diff[0] * values[1][0][0]; |
| 583 | double v01 = (1 - diff[0]) * values[0][0][1] + diff[0] * values[1][0][1]; |
| 584 | double v10 = (1 - diff[0]) * values[0][1][0] + diff[0] * values[1][1][0]; |
| 585 | double v11 = (1 - diff[0]) * values[0][1][1] + diff[0] * values[1][1][1]; |
| 586 | double v0 = (1 - diff[1]) * v00 + diff[1] * v10; |
| 587 | double v1 = (1 - diff[1]) * v01 + diff[1] * v11; |
| 588 | double dist = (1 - diff[2]) * v0 + diff[2] * v1; |
| 589 | |
| 590 | grad[2] = (v1 - v0) * mp_->resolution_inv_; |
| 591 | grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * mp_->resolution_inv_; |
| 592 | grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]); |
| 593 | grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]); |
| 594 | grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]); |
| 595 | grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]); |
| 596 | grad[0] *= mp_->resolution_inv_; |
| 597 | |
| 598 | return dist; |
| 599 | } |
| 600 | } // namespace fast_planner |
| 601 | // SDFMap |