|
| 1 | +#pragma once |
| 2 | + |
| 3 | +#include "bioimage_cpp/array_view.hxx" |
| 4 | + |
| 5 | +#include <array> |
| 6 | +#include <cmath> |
| 7 | +#include <cstddef> |
| 8 | +#include <cstdint> |
| 9 | +#include <stdexcept> |
| 10 | +#include <string> |
| 11 | + |
| 12 | +namespace bioimage_cpp::transformation { |
| 13 | + |
| 14 | +namespace detail { |
| 15 | + |
| 16 | +template <std::size_t D> |
| 17 | +using Coordinate = std::array<std::ptrdiff_t, D>; |
| 18 | + |
| 19 | +template <std::size_t D> |
| 20 | +using FloatingCoordinate = std::array<double, D>; |
| 21 | + |
| 22 | +template <std::size_t D> |
| 23 | +void require_shape(const ConstArrayView<double> &matrix, const char *name) { |
| 24 | + if (matrix.ndim() != 2 || matrix.shape[0] != static_cast<std::ptrdiff_t>(D) || |
| 25 | + matrix.shape[1] != static_cast<std::ptrdiff_t>(D + 1)) { |
| 26 | + throw std::invalid_argument( |
| 27 | + std::string(name) + " must have shape (" + std::to_string(D) + ", " + |
| 28 | + std::to_string(D + 1) + ")" |
| 29 | + ); |
| 30 | + } |
| 31 | +} |
| 32 | + |
| 33 | +template <std::size_t D> |
| 34 | +void require_shape(const ConstArrayView<std::ptrdiff_t> &starts, const char *name) { |
| 35 | + if (starts.ndim() != 1 || starts.shape[0] != static_cast<std::ptrdiff_t>(D)) { |
| 36 | + throw std::invalid_argument( |
| 37 | + std::string(name) + " must have shape (" + std::to_string(D) + ",)" |
| 38 | + ); |
| 39 | + } |
| 40 | +} |
| 41 | + |
| 42 | +template <std::size_t D, class T> |
| 43 | +void require_view_shape(const ConstArrayView<T> &input, const ArrayView<T> &output) { |
| 44 | + if (input.ndim() != static_cast<std::ptrdiff_t>(D)) { |
| 45 | + throw std::invalid_argument( |
| 46 | + "input must have ndim=" + std::to_string(D) + |
| 47 | + ", got ndim=" + std::to_string(input.ndim()) |
| 48 | + ); |
| 49 | + } |
| 50 | + if (output.ndim() != static_cast<std::ptrdiff_t>(D)) { |
| 51 | + throw std::invalid_argument( |
| 52 | + "output must have ndim=" + std::to_string(D) + |
| 53 | + ", got ndim=" + std::to_string(output.ndim()) |
| 54 | + ); |
| 55 | + } |
| 56 | +} |
| 57 | + |
| 58 | +template <std::size_t D, class T> |
| 59 | +std::ptrdiff_t offset_of(const ArrayView<T> &view, const Coordinate<D> &coord) { |
| 60 | + std::ptrdiff_t offset = 0; |
| 61 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 62 | + offset += coord[axis] * view.strides[axis]; |
| 63 | + } |
| 64 | + return offset; |
| 65 | +} |
| 66 | + |
| 67 | +template <std::size_t D, class T> |
| 68 | +std::ptrdiff_t offset_of(const ConstArrayView<T> &view, const Coordinate<D> &coord) { |
| 69 | + std::ptrdiff_t offset = 0; |
| 70 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 71 | + offset += coord[axis] * view.strides[axis]; |
| 72 | + } |
| 73 | + return offset; |
| 74 | +} |
| 75 | + |
| 76 | +template <std::size_t D, class T> |
| 77 | +bool is_inside_index(const ConstArrayView<T> &input, const Coordinate<D> &coord) { |
| 78 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 79 | + if (coord[axis] < 0 || coord[axis] >= input.shape[axis]) return false; |
| 80 | + } |
| 81 | + return true; |
| 82 | +} |
| 83 | + |
| 84 | +template <std::size_t D, class T> |
| 85 | +bool is_inside_coordinate(const ConstArrayView<T> &input, const FloatingCoordinate<D> &coord) { |
| 86 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 87 | + if (coord[axis] < 0.0 || coord[axis] > static_cast<double>(input.shape[axis] - 1)) { |
| 88 | + return false; |
| 89 | + } |
| 90 | + } |
| 91 | + return true; |
| 92 | +} |
| 93 | + |
| 94 | +template <std::size_t D> |
| 95 | +FloatingCoordinate<D> transform_coordinate( |
| 96 | + const Coordinate<D> &output_coord, |
| 97 | + const ConstArrayView<double> &matrix |
| 98 | +) { |
| 99 | + FloatingCoordinate<D> input_coord{}; |
| 100 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 101 | + double value = matrix.data[axis * matrix.strides[0] + static_cast<std::ptrdiff_t>(D) * matrix.strides[1]]; |
| 102 | + for (std::size_t inner = 0; inner < D; ++inner) { |
| 103 | + value += matrix.data[axis * matrix.strides[0] + inner * matrix.strides[1]] * |
| 104 | + static_cast<double>(output_coord[inner]); |
| 105 | + } |
| 106 | + input_coord[axis] = value; |
| 107 | + } |
| 108 | + return input_coord; |
| 109 | +} |
| 110 | + |
| 111 | +inline double cubic_weight(const double x) { |
| 112 | + // Catmull-Rom / Keys cubic convolution with a = -0.5. |
| 113 | + const double ax = std::abs(x); |
| 114 | + if (ax < 1.0) { |
| 115 | + return (1.5 * ax - 2.5) * ax * ax + 1.0; |
| 116 | + } |
| 117 | + if (ax < 2.0) { |
| 118 | + return ((-0.5 * ax + 2.5) * ax - 4.0) * ax + 2.0; |
| 119 | + } |
| 120 | + return 0.0; |
| 121 | +} |
| 122 | + |
| 123 | +template <std::size_t D, class T> |
| 124 | +double sample_or_fill( |
| 125 | + const ConstArrayView<T> &input, |
| 126 | + const Coordinate<D> &coord, |
| 127 | + const T fill_value |
| 128 | +) { |
| 129 | + if (!is_inside_index(input, coord)) return static_cast<double>(fill_value); |
| 130 | + return static_cast<double>(input.data[offset_of(input, coord)]); |
| 131 | +} |
| 132 | + |
| 133 | +template <std::size_t D, class T> |
| 134 | +double interpolate_nearest( |
| 135 | + const ConstArrayView<T> &input, |
| 136 | + const FloatingCoordinate<D> &coord, |
| 137 | + const T fill_value |
| 138 | +) { |
| 139 | + Coordinate<D> nearest{}; |
| 140 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 141 | + nearest[axis] = static_cast<std::ptrdiff_t>(std::floor(coord[axis] + 0.5)); |
| 142 | + } |
| 143 | + return sample_or_fill(input, nearest, fill_value); |
| 144 | +} |
| 145 | + |
| 146 | +template <std::size_t D, class T> |
| 147 | +double interpolate_linear_impl( |
| 148 | + const ConstArrayView<T> &input, |
| 149 | + const T fill_value, |
| 150 | + Coordinate<D> &sample_coord, |
| 151 | + const Coordinate<D> &lower, |
| 152 | + const std::array<double, D> &fraction, |
| 153 | + const std::size_t axis, |
| 154 | + const double weight |
| 155 | +) { |
| 156 | + if (axis == D) { |
| 157 | + return weight * sample_or_fill(input, sample_coord, fill_value); |
| 158 | + } |
| 159 | + |
| 160 | + sample_coord[axis] = lower[axis]; |
| 161 | + double value = interpolate_linear_impl( |
| 162 | + input, fill_value, sample_coord, lower, fraction, axis + 1, |
| 163 | + weight * (1.0 - fraction[axis]) |
| 164 | + ); |
| 165 | + |
| 166 | + sample_coord[axis] = lower[axis] + 1; |
| 167 | + value += interpolate_linear_impl( |
| 168 | + input, fill_value, sample_coord, lower, fraction, axis + 1, |
| 169 | + weight * fraction[axis] |
| 170 | + ); |
| 171 | + return value; |
| 172 | +} |
| 173 | + |
| 174 | +template <std::size_t D, class T> |
| 175 | +double interpolate_linear( |
| 176 | + const ConstArrayView<T> &input, |
| 177 | + const FloatingCoordinate<D> &coord, |
| 178 | + const T fill_value |
| 179 | +) { |
| 180 | + if (!is_inside_coordinate(input, coord)) return static_cast<double>(fill_value); |
| 181 | + |
| 182 | + Coordinate<D> lower{}; |
| 183 | + Coordinate<D> sample_coord{}; |
| 184 | + std::array<double, D> fraction{}; |
| 185 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 186 | + const double floored = std::floor(coord[axis]); |
| 187 | + lower[axis] = static_cast<std::ptrdiff_t>(floored); |
| 188 | + fraction[axis] = coord[axis] - floored; |
| 189 | + } |
| 190 | + return interpolate_linear_impl( |
| 191 | + input, fill_value, sample_coord, lower, fraction, 0, 1.0 |
| 192 | + ); |
| 193 | +} |
| 194 | + |
| 195 | +template <std::size_t D, class T> |
| 196 | +double interpolate_cubic_impl( |
| 197 | + const ConstArrayView<T> &input, |
| 198 | + Coordinate<D> &sample_coord, |
| 199 | + const std::array<std::array<std::ptrdiff_t, 4>, D> &indices, |
| 200 | + const std::array<std::array<double, 4>, D> &weights, |
| 201 | + const T fill_value, |
| 202 | + const std::size_t axis, |
| 203 | + const double weight |
| 204 | +) { |
| 205 | + if (axis == D) { |
| 206 | + return weight * sample_or_fill(input, sample_coord, fill_value); |
| 207 | + } |
| 208 | + |
| 209 | + double value = 0.0; |
| 210 | + for (std::size_t k = 0; k < 4; ++k) { |
| 211 | + sample_coord[axis] = indices[axis][k]; |
| 212 | + value += interpolate_cubic_impl( |
| 213 | + input, sample_coord, indices, weights, fill_value, axis + 1, |
| 214 | + weight * weights[axis][k] |
| 215 | + ); |
| 216 | + } |
| 217 | + return value; |
| 218 | +} |
| 219 | + |
| 220 | +template <std::size_t D, class T> |
| 221 | +double interpolate_cubic( |
| 222 | + const ConstArrayView<T> &input, |
| 223 | + const FloatingCoordinate<D> &coord, |
| 224 | + const T fill_value |
| 225 | +) { |
| 226 | + if (!is_inside_coordinate(input, coord)) return static_cast<double>(fill_value); |
| 227 | + |
| 228 | + std::array<std::array<std::ptrdiff_t, 4>, D> indices{}; |
| 229 | + std::array<std::array<double, 4>, D> weights{}; |
| 230 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 231 | + const auto base = static_cast<std::ptrdiff_t>(std::floor(coord[axis])); |
| 232 | + for (std::size_t k = 0; k < 4; ++k) { |
| 233 | + const auto index = base + static_cast<std::ptrdiff_t>(k) - 1; |
| 234 | + indices[axis][k] = index; |
| 235 | + weights[axis][k] = cubic_weight(coord[axis] - static_cast<double>(index)); |
| 236 | + } |
| 237 | + } |
| 238 | + |
| 239 | + Coordinate<D> sample_coord{}; |
| 240 | + return interpolate_cubic_impl(input, sample_coord, indices, weights, fill_value, 0, 1.0); |
| 241 | +} |
| 242 | + |
| 243 | +template <class T> |
| 244 | +T cast_output(const double value) { |
| 245 | + return static_cast<T>(value); |
| 246 | +} |
| 247 | + |
| 248 | +template <std::size_t D, class T> |
| 249 | +void advance_coordinate(Coordinate<D> &coord, const ArrayView<T> &output) { |
| 250 | + for (std::size_t axis = D; axis-- > 0;) { |
| 251 | + ++coord[axis]; |
| 252 | + if (coord[axis] < output.shape[axis]) return; |
| 253 | + coord[axis] = 0; |
| 254 | + } |
| 255 | +} |
| 256 | + |
| 257 | +} // namespace detail |
| 258 | + |
| 259 | +template <std::size_t D, class T> |
| 260 | +void affine_transform( |
| 261 | + const ConstArrayView<T> &input, |
| 262 | + ArrayView<T> &output, |
| 263 | + const ConstArrayView<double> &matrix, |
| 264 | + const ConstArrayView<std::ptrdiff_t> &starts, |
| 265 | + const int order, |
| 266 | + const T fill_value |
| 267 | +) { |
| 268 | + detail::require_view_shape<D>(input, output); |
| 269 | + detail::require_shape<D>(matrix, "matrix"); |
| 270 | + detail::require_shape<D>(starts, "starts"); |
| 271 | + if (order != 0 && order != 1 && order != 3) { |
| 272 | + throw std::invalid_argument("order must be 0, 1 or 3, got " + std::to_string(order)); |
| 273 | + } |
| 274 | + |
| 275 | + std::ptrdiff_t total = 1; |
| 276 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 277 | + if (output.shape[axis] < 0) { |
| 278 | + throw std::invalid_argument("output shape must be non-negative"); |
| 279 | + } |
| 280 | + total *= output.shape[axis]; |
| 281 | + } |
| 282 | + if (total == 0) return; |
| 283 | + |
| 284 | + detail::Coordinate<D> local_coord{}; |
| 285 | + for (std::ptrdiff_t linear = 0; linear < total; ++linear) { |
| 286 | + detail::Coordinate<D> output_coord{}; |
| 287 | + for (std::size_t axis = 0; axis < D; ++axis) { |
| 288 | + output_coord[axis] = starts.data[axis * starts.strides[0]] + local_coord[axis]; |
| 289 | + } |
| 290 | + |
| 291 | + const auto input_coord = detail::transform_coordinate(output_coord, matrix); |
| 292 | + double value = 0.0; |
| 293 | + if (order == 0) { |
| 294 | + value = detail::interpolate_nearest(input, input_coord, fill_value); |
| 295 | + } else if (order == 1) { |
| 296 | + value = detail::interpolate_linear(input, input_coord, fill_value); |
| 297 | + } else { |
| 298 | + value = detail::interpolate_cubic(input, input_coord, fill_value); |
| 299 | + } |
| 300 | + |
| 301 | + output.data[detail::offset_of(output, local_coord)] = detail::cast_output<T>(value); |
| 302 | + detail::advance_coordinate(local_coord, output); |
| 303 | + } |
| 304 | +} |
| 305 | + |
| 306 | +} // namespace bioimage_cpp::transformation |
0 commit comments