From f1e3fda7f63778fdd0a99dac89619f871f85e115 Mon Sep 17 00:00:00 2001 From: Seth Parker Date: Mon, 19 Aug 2024 11:07:38 -0400 Subject: [PATCH] Move implementation details to src --- segmentation/CMakeLists.txt | 2 +- .../lrps/CubicMultithreadedSpline.hpp | 157 ------- .../vc/segmentation/lrps/CubicSplineMT.hpp | 73 ++++ .../vc/segmentation/lrps/FittedCurve.hpp | 4 +- .../include/vc/segmentation/lrps/Spline.hpp | 37 +- segmentation/src/CubicMultithreadedSpline.cpp | 297 ------------- segmentation/src/CubicSplineMT.cpp | 405 ++++++++++++++++++ segmentation/src/FittedCurve.cpp | 12 +- 8 files changed, 507 insertions(+), 480 deletions(-) delete mode 100644 segmentation/include/vc/segmentation/lrps/CubicMultithreadedSpline.hpp create mode 100644 segmentation/include/vc/segmentation/lrps/CubicSplineMT.hpp delete mode 100644 segmentation/src/CubicMultithreadedSpline.cpp create mode 100644 segmentation/src/CubicSplineMT.cpp diff --git a/segmentation/CMakeLists.txt b/segmentation/CMakeLists.txt index 39e81d875..7e79549a8 100644 --- a/segmentation/CMakeLists.txt +++ b/segmentation/CMakeLists.txt @@ -14,7 +14,7 @@ set(srcs src/StructureTensorParticleSim.cpp src/ThinnedFloodFillSegmentation.cpp src/ComputeVolumetricMask.cpp - src/CubicMultithreadedSpline.cpp + src/CubicSplineMT.cpp ) add_library(vc_segmentation ${srcs}) diff --git a/segmentation/include/vc/segmentation/lrps/CubicMultithreadedSpline.hpp b/segmentation/include/vc/segmentation/lrps/CubicMultithreadedSpline.hpp deleted file mode 100644 index 5daf67fa4..000000000 --- a/segmentation/include/vc/segmentation/lrps/CubicMultithreadedSpline.hpp +++ /dev/null @@ -1,157 +0,0 @@ -/* - * File: CubicMultithreadedSpline.cpp - * Author: Julian Schilliger - * - * Created on: September 2023 - * - * Description: CubicMultithreadedSpline class implementation for scalar spline interpolation. - * - * License: MIT License - * - * Copyright (c) 2023 Julian Schilliger - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#ifndef CUBICMULTITHREADEDSPLINE_H -#define CUBICMULTITHREADEDSPLINE_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "vc/segmentation/lrps/Common.hpp" - -using Eigen::VectorXd; -using Eigen::MatrixXd; - -class CubicMultithreadedSpline { -private: - VectorXd a_x_, b_x_, c_x_, d_x_; - VectorXd a_y_, b_y_, c_y_, d_y_; - VectorXd range_xy_; - Eigen::VectorXd subsegment_lengths_; - Eigen::VectorXd cumulative_lengths_; - std::mutex mtx; - int nr_points; - - void cubic_spline_interpolation(const VectorXd& x, const VectorXd& y, - VectorXd& a, VectorXd& b, - VectorXd& c, VectorXd& d); - - void windowed_spline_worker(const VectorXd& x, const VectorXd& y, - std::vector& a_vec, - std::vector& b_vec, - std::vector& c_vec, - std::vector& d_vec, - int start_idx, int end_idx, - int window_size, int buffer_size); - - void windowed_spline_with_buffer_multithreaded(const VectorXd& x, const VectorXd& y, - VectorXd& a_total, VectorXd& b_total, - VectorXd& c_total, VectorXd& d_total, - int window_size = 100, - int buffer_size = 10, - int num_threads = -1); - -static double integrand2D(double t, void *params); - -double spline_length_2D(double b_x, double c_x, double d_x, double b_y, double c_y, double d_y, double t0, double t_sub0, double t_sub1); - -// Compute lengths of 2D spline segments -std::pair compute_subsegment_lengths_2D( - const Eigen::VectorXd& t, - const Eigen::VectorXd& b_x, - const Eigen::VectorXd& c_x, - const Eigen::VectorXd& d_x, - const Eigen::VectorXd& b_y, - const Eigen::VectorXd& c_y, - const Eigen::VectorXd& d_y, - int subseg_count = 10); - -std::pair evaluate_spline_at_t_2D( - double t, - const Eigen::VectorXd& range_xy, - const Eigen::VectorXd& a_x, - const Eigen::VectorXd& b_x, - const Eigen::VectorXd& c_x, - const Eigen::VectorXd& d_x, - const Eigen::VectorXd& a_y, - const Eigen::VectorXd& b_y, - const Eigen::VectorXd& c_y, - const Eigen::VectorXd& d_y, - const Eigen::VectorXd& subsegment_lengths, - const Eigen::VectorXd& cumulative_lengths) const; - -public: - CubicMultithreadedSpline() = default; - CubicMultithreadedSpline(const VectorXd& x, const VectorXd& y); - CubicMultithreadedSpline(const std::vector& vs); - ~CubicMultithreadedSpline(); - // Custom copy constructor - CubicMultithreadedSpline(const CubicMultithreadedSpline& other) { - a_x_ = other.a_x_; - b_x_ = other.b_x_; - c_x_ = other.c_x_; - d_x_ = other.d_x_; - a_y_ = other.a_y_; - b_y_ = other.b_y_; - c_y_ = other.c_y_; - d_y_ = other.d_y_; - range_xy_ = other.range_xy_; - subsegment_lengths_ = other.subsegment_lengths_; - cumulative_lengths_ = other.cumulative_lengths_; - nr_points = other.nr_points; - - // mtx is deliberately not copied - } - - // Custom copy assignment operator - CubicMultithreadedSpline& operator=(const CubicMultithreadedSpline& other) { - if (this != &other) { - a_x_ = other.a_x_; - b_x_ = other.b_x_; - c_x_ = other.c_x_; - d_x_ = other.d_x_; - a_y_ = other.a_y_; - b_y_ = other.b_y_; - c_y_ = other.c_y_; - d_y_ = other.d_y_; - range_xy_ = other.range_xy_; - subsegment_lengths_ = other.subsegment_lengths_; - cumulative_lengths_ = other.cumulative_lengths_; - nr_points = other.nr_points; - - // mtx is deliberately not copied - } - - return *this; - } - // Evaluate the spline at a given value of t - Pixel operator()(double t) const; -}; - -#endif \ No newline at end of file diff --git a/segmentation/include/vc/segmentation/lrps/CubicSplineMT.hpp b/segmentation/include/vc/segmentation/lrps/CubicSplineMT.hpp new file mode 100644 index 000000000..77a39df3f --- /dev/null +++ b/segmentation/include/vc/segmentation/lrps/CubicSplineMT.hpp @@ -0,0 +1,73 @@ +/* + * File: CubicMultithreadedSpline.cpp + * Author: Julian Schilliger + * + * Created on: September 2023 + * + * Description: CubicMultithreadedSpline class implementation for scalar spline + * interpolation. + * + * License: MIT License + * + * Copyright (c) 2023 Julian Schilliger + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#pragma once + +/** @file */ + +#include +#include +#include + +#include + +#include "vc/segmentation/lrps/Common.hpp" + +/** @brief Thread-safe cubic spline */ +class CubicSplineMT +{ +public: + CubicSplineMT() = default; + CubicSplineMT(const Eigen::VectorXd& x, const Eigen::VectorXd& y); + explicit CubicSplineMT(const std::vector& vs); + ~CubicSplineMT() = default; + + /** Copy constructor */ + CubicSplineMT(const CubicSplineMT& other); + + /** Custom copy assignment operator */ + auto operator=(const CubicSplineMT& other) -> CubicSplineMT&; + + /** + * @brief %Spline evaluation at t-space value t in [0, 1] + */ + auto operator()(double t) const -> Pixel; + +private: + Eigen::VectorXd a_x_, b_x_, c_x_, d_x_; + Eigen::VectorXd a_y_, b_y_, c_y_, d_y_; + Eigen::VectorXd range_xy_; + Eigen::VectorXd subsegment_lengths_; + Eigen::VectorXd cumulative_lengths_; + std::mutex mtx_; + std::size_t npoints_{0}; +}; \ No newline at end of file diff --git a/segmentation/include/vc/segmentation/lrps/FittedCurve.hpp b/segmentation/include/vc/segmentation/lrps/FittedCurve.hpp index 7689f05c8..23429c4c7 100644 --- a/segmentation/include/vc/segmentation/lrps/FittedCurve.hpp +++ b/segmentation/include/vc/segmentation/lrps/FittedCurve.hpp @@ -7,7 +7,7 @@ #include #include "vc/segmentation/lrps/Common.hpp" -#include "vc/segmentation/lrps/CubicMultithreadedSpline.hpp" +#include "vc/segmentation/lrps/CubicSplineMT.hpp" namespace volcart::segmentation { @@ -28,7 +28,7 @@ class FittedCurve /** List of sampled points */ std::vector points_; /** Spline representation of curve */ - CubicMultithreadedSpline spline_; + CubicSplineMT spline_; public: /** @name Constructors */ diff --git a/segmentation/include/vc/segmentation/lrps/Spline.hpp b/segmentation/include/vc/segmentation/lrps/Spline.hpp index ed2e0dafd..1bc17ae36 100644 --- a/segmentation/include/vc/segmentation/lrps/Spline.hpp +++ b/segmentation/include/vc/segmentation/lrps/Spline.hpp @@ -12,6 +12,19 @@ namespace volcart::segmentation { + +/** + * @brief Combine X and Y values into an npoints x 2 matrix + */ +template +auto MakeWideMtx(const Vector& xs, const Vector& ys) -> Eigen::MatrixXd +{ + Eigen::MatrixXd mat{2, xs.size()}; + mat.row(0) = Eigen::VectorXd::Map(xs.data(), xs.size()); + mat.row(1) = Eigen::VectorXd::Map(ys.data(), ys.size()); + return mat; +} + /** * @class Spline * @brief Simple spline wrapper around Eigen::Spline @@ -22,8 +35,9 @@ template class Spline { public: - using ScalarVector = std::vector; + using Vector = std::vector; using SplineType = Eigen::Spline; + using Fitting = Eigen::SplineFitting; Spline() = default; @@ -33,11 +47,10 @@ class Spline * @param xs Vector of X values * @param ys Vector of Y values */ - Spline(const ScalarVector& xs, const ScalarVector& ys) : npoints_{xs.size()} + Spline(const Vector& xs, const Vector& ys) : npoints_{xs.size()} { assert(xs.size() == ys.size() && "xs and ys must be same length"); - auto points = make_wide_matrix_(xs, ys); - spline_ = Eigen::SplineFitting::Interpolate(points, Degree); + spline_ = Fitting::Interpolate(MakeWideMtx(xs, ys), Degree); } /** @@ -52,21 +65,9 @@ class Spline private: /** Number of points on the spline */ - std::size_t npoints_; - + std::size_t npoints_{0}; + /** Eigen spline */ SplineType spline_; - - /** - * @brief Combine X and Y values into an npoints_ x 2 matrix - */ - Eigen::MatrixXd make_wide_matrix_( - const ScalarVector& xs, const ScalarVector& ys) - { - Eigen::MatrixXd mat{2, xs.size()}; - mat.row(0) = Eigen::VectorXd::Map(xs.data(), xs.size()); - mat.row(1) = Eigen::VectorXd::Map(ys.data(), ys.size()); - return mat; - } }; /** diff --git a/segmentation/src/CubicMultithreadedSpline.cpp b/segmentation/src/CubicMultithreadedSpline.cpp deleted file mode 100644 index 8ca60b387..000000000 --- a/segmentation/src/CubicMultithreadedSpline.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/* - * File: CubicMultithreadedSpline.cpp - * Author: Julian Schilliger - * - * Created on: September 2023 - * - * Description: CubicMultithreadedSpline class implementation for scalar spline interpolation. - * - * License: MIT License - * - * Copyright (c) 2023 Julian Schilliger - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "vc/segmentation/lrps/CubicMultithreadedSpline.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using Eigen::VectorXd; -using Eigen::MatrixXd; - -using namespace volcart::segmentation; - -// Constructor implementation -CubicMultithreadedSpline::CubicMultithreadedSpline(const VectorXd& x, const VectorXd& y) -{ - range_xy_ = Eigen::VectorXd::LinSpaced((int)x.size(), 0, x.size()-1); - nr_points = x.size(); - windowed_spline_with_buffer_multithreaded(range_xy_, x, a_x_, b_x_, c_x_, d_x_); - windowed_spline_with_buffer_multithreaded(range_xy_, y, a_y_, b_y_, c_y_, d_y_); - std::tie(subsegment_lengths_, cumulative_lengths_) = compute_subsegment_lengths_2D(range_xy_, b_x_, c_x_, d_x_, b_y_, c_y_, d_y_); -} - -CubicMultithreadedSpline::CubicMultithreadedSpline(const std::vector& vs) { - std::vector xs, ys; - std::tie(xs, ys) = Unzip(vs); - - // Convert to Eigen::VectorXd - Eigen::VectorXd xsEigen = Eigen::VectorXd::Map(xs.data(), xs.size()); - Eigen::VectorXd ysEigen = Eigen::VectorXd::Map(ys.data(), ys.size()); - - range_xy_ = Eigen::VectorXd::LinSpaced((int)xsEigen.size(), 0, xsEigen.size()-1); - nr_points = xs.size(); - windowed_spline_with_buffer_multithreaded(range_xy_, xsEigen, a_x_, b_x_, c_x_, d_x_); - windowed_spline_with_buffer_multithreaded(range_xy_, ysEigen, a_y_, b_y_, c_y_, d_y_); - std::tie(subsegment_lengths_, cumulative_lengths_) = compute_subsegment_lengths_2D(range_xy_, b_x_, c_x_, d_x_, b_y_, c_y_, d_y_); -} - -CubicMultithreadedSpline::~CubicMultithreadedSpline() {} - -// Evaluate the spline at a given value of t -Pixel CubicMultithreadedSpline::operator()(double t) const { - auto [x, y] = evaluate_spline_at_t_2D(t, range_xy_, a_x_, b_x_, c_x_, d_x_, a_y_, b_y_, c_y_, d_y_, subsegment_lengths_, cumulative_lengths_); - return Pixel(x, y); -} - -void CubicMultithreadedSpline::cubic_spline_interpolation(const VectorXd& x, const VectorXd& y, - VectorXd& a, VectorXd& b, - VectorXd& c, VectorXd& d) { - int n = x.size() - 1; - VectorXd h = x.segment(1, n) - x.segment(0, n); - - for(int i = 0; i < h.size(); ++i) { - if(h(i) == 0) h(i) = 1e-8; - } - - a = y.segment(0, n); - b = VectorXd::Zero(n); - c = VectorXd::Zero(n + 1); - d = VectorXd::Zero(n); - - MatrixXd A = MatrixXd::Zero(n + 1, n + 1); - VectorXd B = VectorXd::Zero(n + 1); - - A(0, 0) = 1; - A(n, n) = 1; - - for(int i = 1; i < n; ++i) { - A(i, i - 1) = h[i - 1]; - A(i, i) = 2 * (h[i - 1] + h[i]); - A(i, i + 1) = h[i]; - B(i) = 3 * ((y[i + 1] - y[i]) / h[i] - (y[i] - y[i - 1]) / h[i - 1]); - } - - c = A.colPivHouseholderQr().solve(B); - - for(int i = 0; i < n; ++i) { - b(i) = (y[i + 1] - y[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3; - d(i) = (c[i + 1] - c[i]) / (3 * h[i]); - } - - c.conservativeResize(n); -} - -void CubicMultithreadedSpline::windowed_spline_worker(const VectorXd& x, const VectorXd& y, - std::vector& a_vec, - std::vector& b_vec, - std::vector& c_vec, - std::vector& d_vec, - int start_idx, int end_idx, - int window_size, int buffer_size) { - - for (int i = start_idx; i < end_idx; i += window_size) { - int wnd_end_idx = std::min(i + window_size + buffer_size, (int)x.size()); - int wnd_start_idx = std::max(wnd_end_idx - window_size - 2 * buffer_size, 0); - - VectorXd x_window = x.segment(wnd_start_idx, wnd_end_idx - wnd_start_idx); - VectorXd y_window = y.segment(wnd_start_idx, wnd_end_idx - wnd_start_idx); - - VectorXd a, b, c, d; - cubic_spline_interpolation(x_window, y_window, a, b, c, d); - - int idx_start = i - wnd_start_idx; - int idx_end = std::min(i + window_size, (int)x.size()) - wnd_start_idx; - - std::lock_guard lock(mtx); - std::copy(a.data() + idx_start, a.data() + idx_end, a_vec.begin() + i); - std::copy(b.data() + idx_start, b.data() + idx_end, b_vec.begin() + i); - std::copy(c.data() + idx_start, c.data() + idx_end, c_vec.begin() + i); - std::copy(d.data() + idx_start, d.data() + idx_end, d_vec.begin() + i); - } -} - -void CubicMultithreadedSpline::windowed_spline_with_buffer_multithreaded(const VectorXd& x, const VectorXd& y, - VectorXd& a_total, VectorXd& b_total, - VectorXd& c_total, VectorXd& d_total, - int window_size, - int buffer_size, - int num_threads) { - - int n = x.size(); - std::vector a_vec(n, 0.0), b_vec(n, 0.0), c_vec(n, 0.0), d_vec(n, 0.0); - - std::vector threads; - - if (num_threads == -1) { - int num_available_threads = static_cast(std::thread::hardware_concurrency()); - num_threads = num_available_threads; - } - - int steps = std::ceil(((double)n) / ((double)window_size)); - int steps_per_thread = steps / num_threads; - int remainder = steps % num_threads; - int current_step = 0; - for (int i = 0; i < num_threads; ++i) { - int start_idx = current_step * window_size; - int end_idx = (current_step + steps_per_thread) * window_size; - if (i < remainder) { - end_idx += window_size; - current_step += 1; - } - end_idx = std::min(end_idx, n); - - current_step += steps_per_thread; - if (start_idx >= end_idx) { - continue; - } - - threads.emplace_back(std::thread(&CubicMultithreadedSpline::windowed_spline_worker, this, std::ref(x), std::ref(y), - std::ref(a_vec), std::ref(b_vec), std::ref(c_vec), - std::ref(d_vec), start_idx, end_idx, - window_size, buffer_size)); - } - - for (auto& t : threads) { - t.join(); - } - - a_total = Eigen::Map(a_vec.data(), a_vec.size()); - b_total = Eigen::Map(b_vec.data(), b_vec.size()); - c_total = Eigen::Map(c_vec.data(), c_vec.size()); - d_total = Eigen::Map(d_vec.data(), d_vec.size()); -} - -double CubicMultithreadedSpline::integrand2D(double t, void *params) { - double *coeffs = (double *)params; - double b_x = coeffs[0], c_x = coeffs[1], d_x = coeffs[2]; - double b_y = coeffs[3], c_y = coeffs[4], d_y = coeffs[5]; - double t0 = coeffs[6]; - double ds_dx = b_x + 2 * c_x * (t - t0) + 3 * d_x * (t - t0) * (t - t0); - double ds_dy = b_y + 2 * c_y * (t - t0) + 3 * d_y * (t - t0) * (t - t0); - return sqrt(ds_dx * ds_dx + ds_dy * ds_dy); -} - -double CubicMultithreadedSpline::spline_length_2D(double b_x, double c_x, double d_x, double b_y, double c_y, double d_y, double t0, double t_sub0, double t_sub1) { - gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000); - double result, error; - double coeffs[7] = {b_x, c_x, d_x, b_y, c_y, d_y, t0}; - gsl_function F; - F.function = &CubicMultithreadedSpline::integrand2D; - F.params = &coeffs; - gsl_integration_qags(&F, t_sub0, t_sub1, 0, 1e-8, 1000, w, &result, &error); - gsl_integration_workspace_free(w); - return fabs(result); -} - -// Compute lengths of 2D spline segments -std::pair CubicMultithreadedSpline::compute_subsegment_lengths_2D( - const Eigen::VectorXd& t, - const Eigen::VectorXd& b_x, - const Eigen::VectorXd& c_x, - const Eigen::VectorXd& d_x, - const Eigen::VectorXd& b_y, - const Eigen::VectorXd& c_y, - const Eigen::VectorXd& d_y, - int subseg_count) { - std::vector subsegment_lengths; - std::vector cumulative_lengths = {0.0}; - - for (int i = 0; i < t.size() - 1; ++i) { - double t0 = t[i], t1 = t[i + 1]; - for (int j = 0; j < subseg_count; ++j) { - double t_sub0 = t0 + (t1 - t0) * (static_cast(j) / subseg_count); - double t_sub1 = t0 + (t1 - t0) * (static_cast(j + 1) / subseg_count); - double length = spline_length_2D(b_x[i], c_x[i], d_x[i], b_y[i], c_y[i], d_y[i], t0, t_sub0, t_sub1); - subsegment_lengths.push_back(length); - cumulative_lengths.push_back(cumulative_lengths.back() + length); - } - } - return {Eigen::Map(subsegment_lengths.data(), subsegment_lengths.size()), - Eigen::Map(cumulative_lengths.data(), cumulative_lengths.size())}; -} - -std::pair CubicMultithreadedSpline::evaluate_spline_at_t_2D( - double t, - const Eigen::VectorXd& range_xy, - const Eigen::VectorXd& a_x, - const Eigen::VectorXd& b_x, - const Eigen::VectorXd& c_x, - const Eigen::VectorXd& d_x, - const Eigen::VectorXd& a_y, - const Eigen::VectorXd& b_y, - const Eigen::VectorXd& c_y, - const Eigen::VectorXd& d_y, - const Eigen::VectorXd& subsegment_lengths, - const Eigen::VectorXd& cumulative_lengths) const { - - double total_length = cumulative_lengths[cumulative_lengths.size() - 1]; - double target_length = total_length * t; - - // Find the correct subsegment using binary search - auto it = std::lower_bound(cumulative_lengths.data(), cumulative_lengths.data() + cumulative_lengths.size(), target_length); - int idx = std::distance(cumulative_lengths.data(), it) - 1; - if (idx == -1) { - idx = 0; - } - - int seg_count = range_xy.size() - 1; - int subseg_count = subsegment_lengths.size() / seg_count; - int segment_idx = idx / subseg_count; - int subseg_idx = idx % subseg_count; - - // Calculate the remaining length to target within this subsegment - double remaining_length = target_length - cumulative_lengths[idx]; - - // Compute the x position at t - double range_xy0 = range_xy[segment_idx], range_xy1 = range_xy[segment_idx + 1]; - double range_xy_sub0 = range_xy0 + (range_xy1 - range_xy0) * (static_cast(subseg_idx) / subseg_count); - double range_xy_sub1 = range_xy0 + (range_xy1 - range_xy0) * (static_cast(subseg_idx + 1) / subseg_count); - double range_xy_t = range_xy_sub0 + (range_xy_sub1 - range_xy_sub0) * (remaining_length / subsegment_lengths[idx]); - - double d_range_xy = range_xy_t - range_xy0; - // Compute the x position at range_xy_t - double x_t = a_x[segment_idx] + b_x[segment_idx]*d_range_xy + c_x[segment_idx]*pow(d_range_xy, 2) + d_x[segment_idx]*pow(d_range_xy, 3); - // Compute the y position at range_xy_t - double y_t = a_y[segment_idx] + b_y[segment_idx]*d_range_xy + c_y[segment_idx]*pow(d_range_xy, 2) + d_y[segment_idx]*pow(d_range_xy, 3); - - - return {x_t, y_t}; -} \ No newline at end of file diff --git a/segmentation/src/CubicSplineMT.cpp b/segmentation/src/CubicSplineMT.cpp new file mode 100644 index 000000000..0554f0184 --- /dev/null +++ b/segmentation/src/CubicSplineMT.cpp @@ -0,0 +1,405 @@ +/* + * File: CubicSplineMT.cpp + * Author: Julian Schilliger + * + * Created on: September 2023 + * + * Description: CubicSplineMT class implementation for scalar spline + * interpolation. + * + * License: MIT License + * + * Copyright (c) 2023 Julian Schilliger + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "vc/segmentation/lrps/CubicSplineMT.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace volcart::segmentation; +using namespace Eigen; + +namespace +{ + +void cubic_spline_interpolation( + const VectorXd& x, + const VectorXd& y, + VectorXd& a, + VectorXd& b, + VectorXd& c, + VectorXd& d) +{ + int n = x.size() - 1; + VectorXd h = x.segment(1, n) - x.segment(0, n); + + for (int i = 0; i < h.size(); ++i) { + if (h(i) == 0) + h(i) = 1e-8; + } + + a = y.segment(0, n); + b = VectorXd::Zero(n); + c = VectorXd::Zero(n + 1); + d = VectorXd::Zero(n); + + MatrixXd A = MatrixXd::Zero(n + 1, n + 1); + VectorXd B = VectorXd::Zero(n + 1); + + A(0, 0) = 1; + A(n, n) = 1; + + for (int i = 1; i < n; ++i) { + A(i, i - 1) = h[i - 1]; + A(i, i) = 2 * (h[i - 1] + h[i]); + A(i, i + 1) = h[i]; + B(i) = 3 * ((y[i + 1] - y[i]) / h[i] - (y[i] - y[i - 1]) / h[i - 1]); + } + + c = A.colPivHouseholderQr().solve(B); + + for (int i = 0; i < n; ++i) { + b(i) = (y[i + 1] - y[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3; + d(i) = (c[i + 1] - c[i]) / (3 * h[i]); + } + + c.conservativeResize(n); +} + +void windowed_spline_worker( + const VectorXd& x, + const VectorXd& y, + std::mutex& mtx, + std::vector& a_vec, + std::vector& b_vec, + std::vector& c_vec, + std::vector& d_vec, + int start_idx, + int end_idx, + int window_size, + int buffer_size) +{ + + for (int i = start_idx; i < end_idx; i += window_size) { + int wnd_end_idx = + std::min(i + window_size + buffer_size, (int)x.size()); + int wnd_start_idx = + std::max(wnd_end_idx - window_size - 2 * buffer_size, 0); + + VectorXd x_window = + x.segment(wnd_start_idx, wnd_end_idx - wnd_start_idx); + VectorXd y_window = + y.segment(wnd_start_idx, wnd_end_idx - wnd_start_idx); + + VectorXd a, b, c, d; + cubic_spline_interpolation(x_window, y_window, a, b, c, d); + + int idx_start = i - wnd_start_idx; + int idx_end = std::min(i + window_size, (int)x.size()) - wnd_start_idx; + + std::lock_guard lock(mtx); + std::copy(a.data() + idx_start, a.data() + idx_end, a_vec.begin() + i); + std::copy(b.data() + idx_start, b.data() + idx_end, b_vec.begin() + i); + std::copy(c.data() + idx_start, c.data() + idx_end, c_vec.begin() + i); + std::copy(d.data() + idx_start, d.data() + idx_end, d_vec.begin() + i); + } +} + +void windowed_spline_with_buffer_multithreaded( + const VectorXd& x, + const VectorXd& y, + std::mutex& mtx, + VectorXd& a_total, + VectorXd& b_total, + VectorXd& c_total, + VectorXd& d_total, + int window_size = 100, + int buffer_size = 10, + int num_threads = -1) +{ + + int n = x.size(); + std::vector a_vec(n, 0.0), b_vec(n, 0.0), c_vec(n, 0.0), + d_vec(n, 0.0); + + std::vector threads; + + if (num_threads == -1) { + int num_available_threads = + static_cast(std::thread::hardware_concurrency()); + num_threads = num_available_threads; + } + + int steps = std::ceil(((double)n) / ((double)window_size)); + int steps_per_thread = steps / num_threads; + int remainder = steps % num_threads; + int current_step = 0; + for (int i = 0; i < num_threads; ++i) { + int start_idx = current_step * window_size; + int end_idx = (current_step + steps_per_thread) * window_size; + if (i < remainder) { + end_idx += window_size; + current_step += 1; + } + end_idx = std::min(end_idx, n); + + current_step += steps_per_thread; + if (start_idx >= end_idx) { + continue; + } + + threads.emplace_back( + &windowed_spline_worker, std::ref(x), std::ref(y), std::ref(mtx), + std::ref(a_vec), std::ref(b_vec), std::ref(c_vec), std::ref(d_vec), + start_idx, end_idx, window_size, buffer_size); + } + + for (auto& t : threads) { + t.join(); + } + + a_total = Eigen::Map(a_vec.data(), a_vec.size()); + b_total = Eigen::Map(b_vec.data(), b_vec.size()); + c_total = Eigen::Map(c_vec.data(), c_vec.size()); + d_total = Eigen::Map(d_vec.data(), d_vec.size()); +} + +double integrand2D(double t, void* params) +{ + double* coeffs = (double*)params; + double b_x = coeffs[0], c_x = coeffs[1], d_x = coeffs[2]; + double b_y = coeffs[3], c_y = coeffs[4], d_y = coeffs[5]; + double t0 = coeffs[6]; + double ds_dx = b_x + 2 * c_x * (t - t0) + 3 * d_x * (t - t0) * (t - t0); + double ds_dy = b_y + 2 * c_y * (t - t0) + 3 * d_y * (t - t0) * (t - t0); + return sqrt(ds_dx * ds_dx + ds_dy * ds_dy); +} + +double spline_length_2D( + double b_x, + double c_x, + double d_x, + double b_y, + double c_y, + double d_y, + double t0, + double t_sub0, + double t_sub1) +{ + gsl_integration_workspace* w = gsl_integration_workspace_alloc(1000); + double result, error; + double coeffs[7] = {b_x, c_x, d_x, b_y, c_y, d_y, t0}; + gsl_function F; + F.function = &integrand2D; + F.params = &coeffs; + gsl_integration_qags(&F, t_sub0, t_sub1, 0, 1e-8, 1000, w, &result, &error); + gsl_integration_workspace_free(w); + return fabs(result); +} + +// Compute lengths of 2D spline segments +std::pair compute_subsegment_lengths_2D( + const VectorXd& t, + const VectorXd& b_x, + const VectorXd& c_x, + const VectorXd& d_x, + const VectorXd& b_y, + const VectorXd& c_y, + const VectorXd& d_y, + int subseg_count = 10) +{ + std::vector subsegment_lengths; + std::vector cumulative_lengths = {0.0}; + + for (int i = 0; i < t.size() - 1; ++i) { + double t0 = t[i], t1 = t[i + 1]; + for (int j = 0; j < subseg_count; ++j) { + double t_sub0 = + t0 + (t1 - t0) * (static_cast(j) / subseg_count); + double t_sub1 = + t0 + (t1 - t0) * (static_cast(j + 1) / subseg_count); + double length = spline_length_2D( + b_x[i], c_x[i], d_x[i], b_y[i], c_y[i], d_y[i], t0, t_sub0, + t_sub1); + subsegment_lengths.push_back(length); + cumulative_lengths.push_back(cumulative_lengths.back() + length); + } + } + return { + Eigen::Map( + subsegment_lengths.data(), subsegment_lengths.size()), + Eigen::Map( + cumulative_lengths.data(), cumulative_lengths.size())}; +} + +std::pair evaluate_spline_at_t_2D( + double t, + const VectorXd& range_xy, + const VectorXd& a_x, + const VectorXd& b_x, + const VectorXd& c_x, + const VectorXd& d_x, + const VectorXd& a_y, + const VectorXd& b_y, + const VectorXd& c_y, + const VectorXd& d_y, + const VectorXd& subsegment_lengths, + const VectorXd& cumulative_lengths) +{ + + double total_length = cumulative_lengths[cumulative_lengths.size() - 1]; + double target_length = total_length * t; + + // Find the correct subsegment using binary search + auto it = std::lower_bound( + cumulative_lengths.data(), + cumulative_lengths.data() + cumulative_lengths.size(), target_length); + int idx = std::distance(cumulative_lengths.data(), it) - 1; + if (idx == -1) { + idx = 0; + } + + int seg_count = range_xy.size() - 1; + int subseg_count = subsegment_lengths.size() / seg_count; + int segment_idx = idx / subseg_count; + int subseg_idx = idx % subseg_count; + + // Calculate the remaining length to target within this subsegment + double remaining_length = target_length - cumulative_lengths[idx]; + + // Compute the x position at t + double range_xy0 = range_xy[segment_idx], + range_xy1 = range_xy[segment_idx + 1]; + double range_xy_sub0 = + range_xy0 + (range_xy1 - range_xy0) * + (static_cast(subseg_idx) / subseg_count); + double range_xy_sub1 = + range_xy0 + (range_xy1 - range_xy0) * + (static_cast(subseg_idx + 1) / subseg_count); + double range_xy_t = + range_xy_sub0 + (range_xy_sub1 - range_xy_sub0) * + (remaining_length / subsegment_lengths[idx]); + + double d_range_xy = range_xy_t - range_xy0; + // Compute the x position at range_xy_t + double x_t = a_x[segment_idx] + b_x[segment_idx] * d_range_xy + + c_x[segment_idx] * pow(d_range_xy, 2) + + d_x[segment_idx] * pow(d_range_xy, 3); + // Compute the y position at range_xy_t + double y_t = a_y[segment_idx] + b_y[segment_idx] * d_range_xy + + c_y[segment_idx] * pow(d_range_xy, 2) + + d_y[segment_idx] * pow(d_range_xy, 3); + + return {x_t, y_t}; +} + +} // namespace + +// Constructor implementation +CubicSplineMT::CubicSplineMT(const VectorXd& x, const VectorXd& y) +{ + range_xy_ = VectorXd::LinSpaced((int)x.size(), 0, x.size() - 1); + npoints_ = x.size(); + windowed_spline_with_buffer_multithreaded( + range_xy_, x, mtx_, a_x_, b_x_, c_x_, d_x_); + windowed_spline_with_buffer_multithreaded( + range_xy_, y, mtx_, a_y_, b_y_, c_y_, d_y_); + std::tie(subsegment_lengths_, cumulative_lengths_) = + compute_subsegment_lengths_2D( + range_xy_, b_x_, c_x_, d_x_, b_y_, c_y_, d_y_); +} + +CubicSplineMT::CubicSplineMT(const std::vector& vs) +{ + auto [xs, ys] = Unzip(vs); + + // Convert to VectorXd + VectorXd xsEigen = VectorXd::Map(xs.data(), xs.size()); + VectorXd ysEigen = VectorXd::Map(ys.data(), ys.size()); + + range_xy_ = VectorXd::LinSpaced((int)xsEigen.size(), 0, xsEigen.size() - 1); + npoints_ = xs.size(); + windowed_spline_with_buffer_multithreaded( + range_xy_, xsEigen, mtx_, a_x_, b_x_, c_x_, d_x_); + windowed_spline_with_buffer_multithreaded( + range_xy_, ysEigen, mtx_, a_y_, b_y_, c_y_, d_y_); + std::tie(subsegment_lengths_, cumulative_lengths_) = + compute_subsegment_lengths_2D( + range_xy_, b_x_, c_x_, d_x_, b_y_, c_y_, d_y_); +} + +CubicSplineMT::CubicSplineMT(const CubicSplineMT& other) +{ + a_x_ = other.a_x_; + b_x_ = other.b_x_; + c_x_ = other.c_x_; + d_x_ = other.d_x_; + a_y_ = other.a_y_; + b_y_ = other.b_y_; + c_y_ = other.c_y_; + d_y_ = other.d_y_; + range_xy_ = other.range_xy_; + subsegment_lengths_ = other.subsegment_lengths_; + cumulative_lengths_ = other.cumulative_lengths_; + npoints_ = other.npoints_; + + // mtx is deliberately not copied +} + +auto CubicSplineMT::operator=(const CubicSplineMT& other) -> CubicSplineMT& +{ + if (this != &other) { + a_x_ = other.a_x_; + b_x_ = other.b_x_; + c_x_ = other.c_x_; + d_x_ = other.d_x_; + a_y_ = other.a_y_; + b_y_ = other.b_y_; + c_y_ = other.c_y_; + d_y_ = other.d_y_; + range_xy_ = other.range_xy_; + subsegment_lengths_ = other.subsegment_lengths_; + cumulative_lengths_ = other.cumulative_lengths_; + npoints_ = other.npoints_; + // mtx is deliberately not copied + } + + return *this; +} + +// Evaluate the spline at a given value of t +auto CubicSplineMT::operator()(double t) const -> Pixel +{ + auto [x, y] = evaluate_spline_at_t_2D( + t, range_xy_, a_x_, b_x_, c_x_, d_x_, a_y_, b_y_, c_y_, d_y_, + subsegment_lengths_, cumulative_lengths_); + return {x, y}; +} \ No newline at end of file diff --git a/segmentation/src/FittedCurve.cpp b/segmentation/src/FittedCurve.cpp index 04d61d570..61f167294 100644 --- a/segmentation/src/FittedCurve.cpp +++ b/segmentation/src/FittedCurve.cpp @@ -1,18 +1,20 @@ +#include "vc/segmentation/lrps/FittedCurve.hpp" + #include #include #include #include "vc/segmentation/lrps/Derivative.hpp" -#include "vc/segmentation/lrps/FittedCurve.hpp" -#include "vc/segmentation/lrps/CubicMultithreadedSpline.hpp" using namespace volcart::segmentation; auto GenerateTVals(std::size_t count) -> std::vector; -FittedCurve::FittedCurve(const std::vector& vs, int zIndex) : - npoints_(vs.size()), zIndex_(zIndex), ts_(GenerateTVals(npoints_)), - spline_(CubicMultithreadedSpline(vs)) +FittedCurve::FittedCurve(const std::vector& vs, int zIndex) + : npoints_(vs.size()) + , zIndex_(zIndex) + , ts_(GenerateTVals(npoints_)) + , spline_(CubicSplineMT(vs)) { // Calculate new voxel positions from the spline points_.reserve(vs.size());