| 141 | } |
| 142 | |
| 143 | Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) const |
| 144 | { |
| 145 | double xCenter = size_.x() / 2.0; |
| 146 | double yCenter = size_.y() / 2.0; |
| 147 | int i = std::round(xCenter - (position.x() - position_.x()) / resolution_); |
| 148 | int j = std::round(yCenter - (position.y() - position_.y()) / resolution_); |
| 149 | int k = std::round((position.z() - zIndexStartHeight_) / resolution_); |
| 150 | i = std::max(i, 1); |
| 151 | i = std::min(i, size_.x() - 2); |
| 152 | j = std::max(j, 1); |
| 153 | j = std::min(j, size_.y() - 2); |
| 154 | k = std::max(k, 1); |
| 155 | k = std::min(k, (int)data_.size() - 2); |
| 156 | double dx = (data_[k](i - 1, j) - data_[k](i + 1, j)) / (2 * resolution_); |
| 157 | double dy = (data_[k](i, j - 1) - data_[k](i, j + 1)) / (2 * resolution_); |
| 158 | double dz = (data_[k + 1](i, j) - data_[k - 1](i, j)) / (2 * resolution_); |
| 159 | return Vector3(dx, dy, dz); |
| 160 | } |
| 161 | |
| 162 | void SignedDistanceField::convertToPointCloud(pcl::PointCloud<pcl::PointXYZI>& points) const |
| 163 | { |