From ed56f5a3b5e48ef2f7ab72d4406919b869a4a44f Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Mon, 11 Dec 2023 18:50:57 -0800 Subject: [PATCH] feat: cubic bspline Jacobian wrt. the control points --- cpp/farm_ng/core/misc/time_series_test.cpp | 2 +- cpp/sophus/CMakeLists.txt | 4 +- cpp/sophus/interp/CMakeLists.txt | 20 + cpp/sophus/{lie => }/interp/average.h | 0 cpp/sophus/{lie => }/interp/interpolate.h | 0 .../{lie => }/interp/interpolate_test.cpp | 4 +- cpp/sophus/interp/spline/CMakeLists.txt | 22 + cpp/sophus/interp/spline/bspline.h | 180 ++++++ cpp/sophus/interp/spline/common.h | 26 + .../interp/spline/details/bspline_segment.h | 161 ++++++ .../interp/spline/details/cubic_basis.h | 65 +++ .../spline/details/group_bspline_segment.h | 286 ++++++++++ cpp/sophus/interp/spline/group_bspline.h | 355 ++++++++++++ .../interp => interp/spline}/spline_test.cpp | 56 +- cpp/sophus/lie/CMakeLists.txt | 2 - cpp/sophus/lie/group_manifold.h | 2 +- .../impl/translation_factor_group_product.h | 6 + cpp/sophus/lie/interp/CMakeLists.txt | 24 - cpp/sophus/lie/interp/spline.h | 530 ------------------ 19 files changed, 1180 insertions(+), 565 deletions(-) create mode 100644 cpp/sophus/interp/CMakeLists.txt rename cpp/sophus/{lie => }/interp/average.h (100%) rename cpp/sophus/{lie => }/interp/interpolate.h (100%) rename cpp/sophus/{lie => }/interp/interpolate_test.cpp (99%) create mode 100644 cpp/sophus/interp/spline/CMakeLists.txt create mode 100644 cpp/sophus/interp/spline/bspline.h create mode 100644 cpp/sophus/interp/spline/common.h create mode 100644 cpp/sophus/interp/spline/details/bspline_segment.h create mode 100644 cpp/sophus/interp/spline/details/cubic_basis.h create mode 100644 cpp/sophus/interp/spline/details/group_bspline_segment.h create mode 100644 cpp/sophus/interp/spline/group_bspline.h rename cpp/sophus/{lie/interp => interp/spline}/spline_test.cpp (77%) delete mode 100644 cpp/sophus/lie/interp/CMakeLists.txt delete mode 100644 cpp/sophus/lie/interp/spline.h diff --git a/cpp/farm_ng/core/misc/time_series_test.cpp b/cpp/farm_ng/core/misc/time_series_test.cpp index 5d27d10b..12d782e2 100644 --- a/cpp/farm_ng/core/misc/time_series_test.cpp +++ b/cpp/farm_ng/core/misc/time_series_test.cpp @@ -15,7 +15,7 @@ #include "farm_ng/core/misc/time_series.h" #include "farm_ng/core/misc/conversions.h" -#include "sophus/lie/interp/interpolate.h" +#include "sophus/interp/interpolate.h" #include #include diff --git a/cpp/sophus/CMakeLists.txt b/cpp/sophus/CMakeLists.txt index 99611ce2..427e6625 100644 --- a/cpp/sophus/CMakeLists.txt +++ b/cpp/sophus/CMakeLists.txt @@ -23,6 +23,7 @@ add_subdirectory(linalg) add_subdirectory(calculus) add_subdirectory(color) add_subdirectory(lie) +add_subdirectory(interp) add_subdirectory(geometry) add_subdirectory(image) add_subdirectory(sensor) @@ -36,7 +37,8 @@ target_link_libraries(sophus INTERFACE sophus_calculus sophus_linalg sophus_lie - sophus_lie_interp + sophus_interp + sophus_interp_spline sophus_geometry ) add_library (Sophus::sophus ALIAS sophus) diff --git a/cpp/sophus/interp/CMakeLists.txt b/cpp/sophus/interp/CMakeLists.txt new file mode 100644 index 00000000..31bbe6d9 --- /dev/null +++ b/cpp/sophus/interp/CMakeLists.txt @@ -0,0 +1,20 @@ +#[[ +sophus_interp + +]] + +farm_ng_add_library(sophus_interp + NAMESPACE Sophus + INCLUDE_DIR ../../../ + HEADERS + average.h + interpolate.h +) +target_link_libraries(sophus_interp INTERFACE sophus_lie) + +farm_ng_add_test(interpolate + PARENT_LIBRARY sophus_interp + LINK_LIBRARIES sophus_interp + LABELS small) + +add_subdirectory(spline) diff --git a/cpp/sophus/lie/interp/average.h b/cpp/sophus/interp/average.h similarity index 100% rename from cpp/sophus/lie/interp/average.h rename to cpp/sophus/interp/average.h diff --git a/cpp/sophus/lie/interp/interpolate.h b/cpp/sophus/interp/interpolate.h similarity index 100% rename from cpp/sophus/lie/interp/interpolate.h rename to cpp/sophus/interp/interpolate.h diff --git a/cpp/sophus/lie/interp/interpolate_test.cpp b/cpp/sophus/interp/interpolate_test.cpp similarity index 99% rename from cpp/sophus/lie/interp/interpolate_test.cpp rename to cpp/sophus/interp/interpolate_test.cpp index 3d29b38e..4d2551cf 100644 --- a/cpp/sophus/lie/interp/interpolate_test.cpp +++ b/cpp/sophus/interp/interpolate_test.cpp @@ -6,9 +6,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/interp/interpolate.h" +#include "sophus/interp/interpolate.h" -#include "sophus/lie/interp/average.h" +#include "sophus/interp/average.h" #include "sophus/lie/scaling_translation.h" #include diff --git a/cpp/sophus/interp/spline/CMakeLists.txt b/cpp/sophus/interp/spline/CMakeLists.txt new file mode 100644 index 00000000..1f2e1da7 --- /dev/null +++ b/cpp/sophus/interp/spline/CMakeLists.txt @@ -0,0 +1,22 @@ +#[[ +sophus_interp_spline + +]] + +farm_ng_add_library(sophus_interp_spline + NAMESPACE Sophus + INCLUDE_DIR ../../../../ + HEADERS + bspline.h + group_bspline.h + details/bspline_segment.h + details/cubic_basis.h + details/group_bspline_segment.h +) +target_link_libraries(sophus_interp_spline INTERFACE sophus_lie) + + +farm_ng_add_test(spline + PARENT_LIBRARY sophus_interp_spline + LINK_LIBRARIES sophus_interp_spline + LABELS small) diff --git a/cpp/sophus/interp/spline/bspline.h b/cpp/sophus/interp/spline/bspline.h new file mode 100644 index 00000000..ee3e1f62 --- /dev/null +++ b/cpp/sophus/interp/spline/bspline.h @@ -0,0 +1,180 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#pragma once + +#include "sophus/interp/spline/common.h" +#include "sophus/interp/spline/details/bspline_segment.h" + +namespace sophus { +template +class CubicBSplineImpl { + public: + using Scalar = TScalar; + using Vector = Eigen::Vector; + + CubicBSplineImpl(std::vector const& control_points, double delta_t) + : control_points_(control_points), delta_t_(delta_t) { + SOPHUS_ASSERT( + control_points.size() >= 2u, ", but {}", control_points.size()); + } + + [[nodiscard]] Vector interpolate(int i, double u) const { + SOPHUS_ASSERT_GE(i, 0); + SOPHUS_ASSERT_LE(i, this->getNumSegments()); + + details::SegmentCase segment_case = + i == 0 + ? details::SegmentCase::first + : (i == this->getNumSegments() - 1 ? details::SegmentCase::last + : details::SegmentCase::normal); + + int idx_prev = std::max(0, i - 1); + int idx_0 = i; + int idx_1 = i + 1; + int idx_2 = std::min(i + 2, int(this->control_points_.size()) - 1); + + return details::CubicBSplineSegment( + segment_case, + control_points_[idx_prev], + control_points_[idx_0], + control_points_[idx_1], + control_points_[idx_2]) + .interpolate(u); + } + + [[nodiscard]] Eigen::Matrix dxiInterpolate( + int i, double u, int control_point_idx) const { + SOPHUS_ASSERT_GE(i, 0); + SOPHUS_ASSERT_LE(i, this->getNumSegments()); + + details::SegmentCase segment_case = + i == 0 + ? details::SegmentCase::first + : (i == this->getNumSegments() - 1 ? details::SegmentCase::last + : details::SegmentCase::normal); + + int idx_prev = std::max(0, i - 1); + int idx_0 = i; + int idx_1 = i + 1; + int idx_2 = std::min(i + 2, int(this->control_points_.size()) - 1); + + details::CubicBSplineSegment spline_segment( + segment_case, + control_points_[idx_prev], + control_points_[idx_0], + control_points_[idx_1], + control_points_[idx_2]); + + Eigen::Matrix dxi; + dxi.setZero(); + + if (idx_prev == control_point_idx) { + dxi += spline_segment.dxiInterpolate(u, 0); + } + if (idx_0 == control_point_idx) { + dxi += spline_segment.dxiInterpolate(u, 1); + } + if (idx_1 == control_point_idx) { + dxi += spline_segment.dxiInterpolate(u, 2); + } + if (idx_2 == control_point_idx) { + dxi += spline_segment.dxiInterpolate(u, 3); + } + return dxi; + } + + [[nodiscard]] std::vector const& controlPoints() const { + return control_points_; + } + + std::vector& controlPoint() { return control_points_; } + + [[nodiscard]] int getNumSegments() const { + return int(control_points_.size()) - 1; + } + + [[nodiscard]] double deltaT() const { return delta_t_; } + + private: + std::vector control_points_; + double delta_t_; +}; + +template +class CubicBSpline { + public: + using Scalar = TScalar; + using Vector = Eigen::Vector; + + CubicBSpline(std::vector control_points, double t0, double delta_t) + : impl_(std::move(control_points), delta_t), t0_(t0) {} + + [[nodiscard]] Vector interpolate(double t) const { + SegmentCoordinate index_and_u = this->indexAndU(t); + return impl_.interpolate(index_and_u.segment_idx, index_and_u.fraction); + } + + [[nodiscard]] Eigen::Matrix dxiInterpolate( + double t, int control_point_idx) const { + SegmentCoordinate index_and_u = this->indexAndU(t); + return impl_.dxiInterpolate( + index_and_u.segment_idx, index_and_u.fraction, control_point_idx); + } + + [[nodiscard]] double t0() const { return t0_; } + + [[nodiscard]] double tmax() const { + return t0_ + impl_.deltaT() * getNumSegments(); + } + + [[nodiscard]] std::vector const& controlPoints() const { + return impl_.controlPoints(); + } + + std::vector& controlPoints() { return impl_.controlPoints(); } + + [[nodiscard]] int getNumSegments() const { return impl_.getNumSegments(); } + + [[nodiscard]] double s(double t) const { return (t - t0_) / impl_.deltaT(); } + + [[nodiscard]] double deltaT() const { return impl_.deltaT(); } + + [[nodiscard]] SegmentCoordinate indexAndU(double t) const { + SOPHUS_ASSERT_GE(t, t0_); + SOPHUS_ASSERT_LE(t, this->tmax()); + + double s = this->s(t); + double segment_idx = NAN; + SegmentCoordinate index_and_u; + index_and_u.fraction = std::modf(s, &segment_idx); + index_and_u.segment_idx = int(segment_idx); + if (index_and_u.fraction > sophus::kEpsilonF64) { + return index_and_u; + } + + // u ~=~ 0.0 + if (index_and_u.segment_idx < getNumSegments() / 2) { + // First half of spline, keep as is (i, 0.0). + return index_and_u; + } + // Second half of spline, use (i-1, 1.0) instead. This way we can + // represent t == tmax (and not just t impl_; + + double t0_; +}; + +} // namespace sophus diff --git a/cpp/sophus/interp/spline/common.h b/cpp/sophus/interp/spline/common.h new file mode 100644 index 00000000..e391b4bd --- /dev/null +++ b/cpp/sophus/interp/spline/common.h @@ -0,0 +1,26 @@ + +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +/// @file +// Basis spline implementation on Lie Group following: +// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013 +// http://www.bmva.org/bmvc/2013/Papers/paper0093/paper0093.pdf + +#pragma once + +#include "sophus/common/common.h" + +namespace sophus { + +struct SegmentCoordinate { + int segment_idx; + double fraction; +}; + +} // namespace sophus diff --git a/cpp/sophus/interp/spline/details/bspline_segment.h b/cpp/sophus/interp/spline/details/bspline_segment.h new file mode 100644 index 00000000..acd99657 --- /dev/null +++ b/cpp/sophus/interp/spline/details/bspline_segment.h @@ -0,0 +1,161 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#pragma once + +#include "sophus/common/common.h" +#include "sophus/interp/spline/details/cubic_basis.h" + +namespace sophus { +namespace details { + +template +class CubicBSplineFn { + public: + using Scalar = TScalar; + using Vector = Eigen::Vector; + + static Vector interpolate( + Vector const& control_point, + std::array const& control_vectors, + double u) { + Eigen::Vector3 b = CubicBSplineBasis::b(u); + return control_point + b[0] * control_vectors[0] + + b[1] * control_vectors[1] + b[2] * control_vectors[2]; + } + + static Eigen::Matrix dxiInterpolate( + double u, int quadruple_idx) { + if (quadruple_idx == 0) { + return Eigen::Matrix::Identity(); + } + Eigen::Vector3 b = CubicBSplineBasis::b(u); + + return b[quadruple_idx - 1] * Eigen::Matrix::Identity(); + } +}; + +template +struct CubicBSplineSegment { + public: + using Scalar = TScalar; + using Vector = Eigen::Vector; + + CubicBSplineSegment( + SegmentCase segment_case, + Vector const& control_point0, + Vector const& control_point1, + Vector const& control_point2, + Vector const& control_point3) + : segment_case_(segment_case), + control_point0_(control_point0), + control_point1_(control_point1), + control_point2_(control_point2), + control_point3_(control_point3) {} + + CubicBSplineSegment(CubicBSplineSegment const&) = delete; + CubicBSplineSegment(CubicBSplineSegment&&) = delete; + + auto operator=(CubicBSplineSegment const&) -> CubicBSplineSegment = delete; + auto operator=(CubicBSplineSegment&&) -> CubicBSplineSegment = delete; + + Vector interpolate(double u) { + switch (segment_case_) { + case SegmentCase::first: { + return CubicBSplineFn::interpolate( + control_point0_, + std::array( + {Vector::Zero(), + control_point2_ - control_point1_, + control_point3_ - control_point2_}), + u); + } + case SegmentCase::normal: { + return CubicBSplineFn::interpolate( + control_point0_, + std::array( + {control_point1_ - control_point0_, + control_point2_ - control_point1_, + control_point3_ - control_point2_}), + u); + } + case SegmentCase::last: { + return CubicBSplineFn::interpolate( + control_point0_, + std::array( + {control_point1_ - control_point0_, + control_point2_ - control_point1_, + Vector::Zero()}), + u); + } + } + + SOPHUS_PANIC("logic error"); + } + + Eigen::Matrix dxiInterpolate( + double u, int quadruple_idx) { + switch (segment_case_) { + case SegmentCase::first: { + if (quadruple_idx == 0) { + return Eigen::Matrix::Zero(); + } + if (quadruple_idx == 1) { + return CubicBSplineFn::dxiInterpolate(u, 0) - + CubicBSplineFn::dxiInterpolate(u, 2); + } + if (quadruple_idx == 2) { + return CubicBSplineFn::dxiInterpolate(u, 2) - + CubicBSplineFn::dxiInterpolate(u, 3); + } + return CubicBSplineFn::dxiInterpolate(u, 3); + } + case SegmentCase::normal: { + if (quadruple_idx == 0) { + return CubicBSplineFn::dxiInterpolate(u, 0) - + CubicBSplineFn::dxiInterpolate(u, 1); + } + if (quadruple_idx == 1) { + return CubicBSplineFn::dxiInterpolate(u, 1) - + CubicBSplineFn::dxiInterpolate(u, 2); + } + if (quadruple_idx == 2) { + return CubicBSplineFn::dxiInterpolate(u, 2) - + CubicBSplineFn::dxiInterpolate(u, 3); + } + return CubicBSplineFn::dxiInterpolate(u, 3); + } + case SegmentCase::last: { + if (quadruple_idx == 0) { + return CubicBSplineFn::dxiInterpolate(u, 0) - + CubicBSplineFn::dxiInterpolate(u, 1); + } + if (quadruple_idx == 1) { + return CubicBSplineFn::dxiInterpolate(u, 1) - + CubicBSplineFn::dxiInterpolate(u, 2); + } + if (quadruple_idx == 2) { + return CubicBSplineFn::dxiInterpolate(u, 2); + } + return Eigen::Matrix::Zero(); + } + } + + SOPHUS_PANIC("logic error"); + } + + private: + SegmentCase segment_case_; + Vector const& control_point0_; + Vector const& control_point1_; + Vector const& control_point2_; + Vector const& control_point3_; +}; + +} // namespace details +} // namespace sophus diff --git a/cpp/sophus/interp/spline/details/cubic_basis.h b/cpp/sophus/interp/spline/details/cubic_basis.h new file mode 100644 index 00000000..d87476fe --- /dev/null +++ b/cpp/sophus/interp/spline/details/cubic_basis.h @@ -0,0 +1,65 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +/// @file +// Basis spline implementation on Lie Group following: +// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013 +// http://www.bmva.org/bmvc/2013/Papers/paper0093/paper0093.pdf + +#pragma once + +#include "sophus/common/common.h" +#include "sophus/lie/lie_group.h" + +namespace sophus { +namespace details { + +template +class CubicBSplineBasis { + public: + static Eigen::Matrix c() { + Eigen::Matrix c; + TScalar const o(0); + + // clang-format off + c << TScalar(5./6), TScalar(3./6), -TScalar(3./6), TScalar(1./6), + TScalar(1./6), TScalar(3./6), TScalar(3./6), -TScalar(2./6), + o, o, o, TScalar(1./6); + // clang-format on + return c; + } + + static Eigen::Vector3 b(TScalar const& u) { + // SOPHUS_ASSERT(u >= TScalar(0), "but %", u); + // SOPHUS_ASSERT(u < TScalar(1), "but %", u); + TScalar u_square(u * u); + return c() * Eigen::Vector4(TScalar(1), u, u_square, u * u_square); + } + + static Eigen::Vector3 dtB(TScalar const& u, TScalar const& delta_t) { + // SOPHUS_ASSERT(u >= TScalar(0), "but %", u); + // SOPHUS_ASSERT(u < TScalar(1), "but %", u); + return (TScalar(1) / delta_t) * c() * + Eigen::Vector4( + TScalar(0), TScalar(1), TScalar(2) * u, TScalar(3) * u * u); + } + + static Eigen::Vector3 dt2B( + TScalar const& u, TScalar const& delta_t) { + // SOPHUS_ASSERT(u >= TScalar(0), "but %", u); + // SOPHUS_ASSERT(u < TScalar(1), "but %", u); + return (TScalar(1) / (delta_t * delta_t)) * c() * + Eigen::Vector4( + TScalar(0), TScalar(0), TScalar(2), TScalar(6) * u); + } +}; + +enum class SegmentCase { first, normal, last }; + +} // namespace details +} // namespace sophus diff --git a/cpp/sophus/interp/spline/details/group_bspline_segment.h b/cpp/sophus/interp/spline/details/group_bspline_segment.h new file mode 100644 index 00000000..3046923d --- /dev/null +++ b/cpp/sophus/interp/spline/details/group_bspline_segment.h @@ -0,0 +1,286 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +/// @file +// Basis spline implementation on Lie Group following: +// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013 +// http://www.bmva.org/bmvc/2013/Papers/paper0093/paper0093.pdf + +#pragma once + +#include "sophus/common/common.h" +#include "sophus/interp/spline/details/cubic_basis.h" +#include "sophus/lie/lie_group.h" + +namespace sophus { +namespace details { + +template +class CubicLieGroupBSplineFn { + public: + using LieGroup = TGroup; + static int const kDof = LieGroup::kDof; + using Scalar = typename LieGroup::Scalar; + using Transformation = + Eigen::Matrix; + using Tangent = typename LieGroup::Tangent; + + static LieGroup parentFromSpline( + LieGroup const& parent_ts_control_point, + std::tuple const& control_tangent_vectors, + double u) { + auto aa = a(control_tangent_vectors, u); + return parent_ts_control_point * std::get<0>(aa) * std::get<1>(aa) * + std::get<2>(aa); + } + + static Transformation dtParentFromSpline( + LieGroup const& parent_ts_control_point, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + auto aa = a(control_tangent_vectors, u); + auto dt_aa = dtA(aa, control_tangent_vectors, u, delta_t); + return parent_ts_control_point.matrix() * + ((std::get<0>(dt_aa) * std::get<1>(aa).matrix() * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(dt_aa) * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(aa).matrix() * + std::get<2>(dt_aa))); + } + + static Transformation dt2ParentFromSpline( + LieGroup const& parent_ts_control_point, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + using TScalar = typename LieGroup::Scalar; + auto aa = a(control_tangent_vectors, u); + auto dt_aa = dtA(aa, control_tangent_vectors, u, delta_t); + auto dt2_aa = dt2A(aa, dt_aa, control_tangent_vectors, u, delta_t); + + return parent_ts_control_point.matrix() * + ((std::get<0>(dt2_aa) * std::get<1>(aa).matrix() * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(dt2_aa) * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(aa).matrix() * + std::get<2>(dt2_aa)) + + TScalar(2) * ((std::get<0>(dt_aa) * std::get<1>(dt_aa) * + std::get<2>(aa).matrix()) + + (std::get<0>(dt_aa) * std::get<1>(aa).matrix() * + std::get<2>(dt_aa)) + + (std::get<0>(aa).matrix() * std::get<1>(dt_aa) * + std::get<2>(dt_aa)))); + } + + static std::tuple a( + std::tuple const& control_tangent_vectors, + double u) { + Eigen::Vector3d b = CubicBSplineBasis::b(u); + return std::make_tuple( + LieGroup::exp(b[0] * std::get<0>(control_tangent_vectors)), + LieGroup::exp(b[1] * std::get<1>(control_tangent_vectors)), + LieGroup::exp(b[2] * std::get<2>(control_tangent_vectors))); + } + + static Transformation dx1A( + Tangent const& control_tangent_vectors, double u, int i) { + Eigen::Vector3d b = CubicBSplineBasis::b(u); + Eigen::Vector t; + t[i] = Scalar(1.0); + return LieGroup::hat(t) * LieGroup::exp(b[0] * t).matrix(); + } + + static std::tuple dtA( + std::tuple const& aa, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + Eigen::Vector3d dt_b = CubicBSplineBasis::dtB(u, delta_t); + return std::make_tuple( + dt_b[0] * std::get<0>(aa).matrix() * + LieGroup::hat(std::get<0>(control_tangent_vectors)), + dt_b[1] * std::get<1>(aa).matrix() * + LieGroup::hat(std::get<1>(control_tangent_vectors)), + dt_b[2] * std::get<2>(aa).matrix() * + LieGroup::hat(std::get<2>(control_tangent_vectors))); + } + + static std::tuple dt2A( + std::tuple const& aa, + std::tuple const& dt_aa, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + Eigen::Vector3d dt_b = CubicBSplineBasis::dtB(u, delta_t); + Eigen::Vector3d dt2_b = CubicBSplineBasis::dt2B(u, delta_t); + + return std::make_tuple( + (dt_b[0] * std::get<0>(dt_aa).matrix() + + dt2_b[0] * std::get<0>(aa).matrix()) * + LieGroup::hat(std::get<0>(control_tangent_vectors)), + (dt_b[1] * std::get<1>(dt_aa).matrix() + + dt2_b[1] * std::get<1>(aa).matrix()) * + LieGroup::hat(std::get<1>(control_tangent_vectors)), + (dt_b[2] * std::get<2>(dt_aa).matrix() + + dt2_b[2] * std::get<2>(aa).matrix()) * + LieGroup::hat(std::get<2>(control_tangent_vectors))); + } +}; + +template +struct CubicLieGroupBSplineSegment { + public: + using Scalar = typename TGroup::Scalar; + using T = Scalar; + using Transformation = + Eigen::Matrix; + using Params = typename TGroup::Params; + + CubicLieGroupBSplineSegment( + SegmentCase segment_case, + Params const& params0, + Params const& params1, + Params const& params2, + Params const& params3) + : segment_case_(segment_case), + params0_(params0), + params1_(params1), + params2_(params2), + params3_(params3) {} + + CubicLieGroupBSplineSegment(CubicLieGroupBSplineSegment const&) = delete; + CubicLieGroupBSplineSegment(CubicLieGroupBSplineSegment&&) = delete; + + auto operator=(CubicLieGroupBSplineSegment const&) + -> CubicLieGroupBSplineSegment = delete; + auto operator=(CubicLieGroupBSplineSegment&&) + -> CubicLieGroupBSplineSegment = delete; + + [[nodiscard]] TGroup worldFromFooPrev() const { + return TGroup::fromParams(params0_); + } + [[nodiscard]] TGroup worldFromFoo0() const { + return TGroup::fromParams(params1_); + } + + [[nodiscard]] TGroup worldFromFoo1() const { + return TGroup::fromParams(params2_); + } + + [[nodiscard]] TGroup worldFromFoo2() const { + return TGroup::fromParams(params3_); + } + TGroup parentFromSpline(double u) { + switch (segment_case_) { + case SegmentCase::first: + return CubicLieGroupBSplineFn::parentFromSpline( + worldFromFoo0(), + std::make_tuple( + (worldFromFoo0().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo2()).log()), + u); + case SegmentCase::normal: + return CubicLieGroupBSplineFn::parentFromSpline( + worldFromFooPrev(), + std::make_tuple( + (worldFromFooPrev().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo2()).log()), + u); + case SegmentCase::last: + return CubicLieGroupBSplineFn::parentFromSpline( + worldFromFooPrev(), + std::make_tuple( + (worldFromFooPrev().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo1()).log()), + u); + } + SOPHUS_PANIC("logic error"); + } + + Transformation dtParentFromSpline(double u, double delta_t) { + switch (segment_case_) { + case SegmentCase::first: + return CubicLieGroupBSplineFn::dtParentFromSpline( + worldFromFoo0(), + std::make_tuple( + (worldFromFoo0().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo2()).log()), + u, + delta_t); + case SegmentCase::normal: + return CubicLieGroupBSplineFn::dtParentFromSpline( + worldFromFooPrev(), + std::make_tuple( + (worldFromFooPrev().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo2()).log()), + u, + delta_t); + case SegmentCase::last: + return CubicLieGroupBSplineFn::dtParentFromSpline( + worldFromFooPrev(), + std::make_tuple( + (worldFromFooPrev().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo1()).log()), + u, + delta_t); + } + SOPHUS_PANIC("logic error"); + } + + Transformation dt2ParentFromSpline(double u, double delta_t) { + switch (segment_case_) { + case SegmentCase::first: + return CubicLieGroupBSplineFn::dt2ParentFromSpline( + worldFromFoo0(), + std::make_tuple( + (worldFromFoo0().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo2()).log()), + u, + delta_t); + case SegmentCase::normal: + return CubicLieGroupBSplineFn::dt2ParentFromSpline( + worldFromFooPrev(), + std::make_tuple( + (worldFromFooPrev().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo2()).log()), + u, + delta_t); + case SegmentCase::last: + return CubicLieGroupBSplineFn::dt2ParentFromSpline( + worldFromFooPrev(), + std::make_tuple( + (worldFromFooPrev().inverse() * worldFromFoo0()).log(), + (worldFromFoo0().inverse() * worldFromFoo1()).log(), + (worldFromFoo1().inverse() * worldFromFoo1()).log()), + u, + delta_t); + } + SOPHUS_PANIC("logic error"); + } + + private: + SegmentCase segment_case_; + Params params0_; + Params params1_; + Params params2_; + Params params3_; +}; + +} // namespace details +} // namespace sophus diff --git a/cpp/sophus/interp/spline/group_bspline.h b/cpp/sophus/interp/spline/group_bspline.h new file mode 100644 index 00000000..eed05920 --- /dev/null +++ b/cpp/sophus/interp/spline/group_bspline.h @@ -0,0 +1,355 @@ +// Copyright (c) 2011, Hauke Strasdat +// Copyright (c) 2012, Steven Lovegrove +// Copyright (c) 2021, farm-ng, inc. +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +/// @file +// Basis spline implementation on Lie Group following: +// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013 +// http://www.bmva.org/bmvc/2013/Papers/paper0093/paper0093.pdf + +#pragma once + +#include "sophus/interp/spline/common.h" +#include "sophus/interp/spline/details/group_bspline_segment.h" + +namespace sophus { + +template +class CubicLieGroupBSplineFn { + public: + using LieGroup = TGroup; + static int const kDof = LieGroup::kDof; + using Scalar = typename LieGroup::Scalar; + using Transformation = + Eigen::Matrix; + using Tangent = typename LieGroup::Tangent; + + static LieGroup parentFromSpline( + LieGroup const& parent_ts_control_point, + std::tuple const& control_tangent_vectors, + double u) { + auto aa = a(control_tangent_vectors, u); + return parent_ts_control_point * std::get<0>(aa) * std::get<1>(aa) * + std::get<2>(aa); + } + + static Transformation dtParentFromSpline( + LieGroup const& parent_ts_control_point, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + auto aa = a(control_tangent_vectors, u); + auto dt_aa = dtA(aa, control_tangent_vectors, u, delta_t); + return parent_ts_control_point.matrix() * + ((std::get<0>(dt_aa) * std::get<1>(aa).matrix() * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(dt_aa) * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(aa).matrix() * + std::get<2>(dt_aa))); + } + + static Transformation dt2ParentFromSpline( + LieGroup const& parent_ts_control_point, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + using TScalar = typename LieGroup::Scalar; + auto aa = a(control_tangent_vectors, u); + auto dt_aa = dtA(aa, control_tangent_vectors, u, delta_t); + auto dt2_aa = dt2A(aa, dt_aa, control_tangent_vectors, u, delta_t); + + return parent_ts_control_point.matrix() * + ((std::get<0>(dt2_aa) * std::get<1>(aa).matrix() * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(dt2_aa) * + std::get<2>(aa).matrix()) + + (std::get<0>(aa).matrix() * std::get<1>(aa).matrix() * + std::get<2>(dt2_aa)) + + TScalar(2) * ((std::get<0>(dt_aa) * std::get<1>(dt_aa) * + std::get<2>(aa).matrix()) + + (std::get<0>(dt_aa) * std::get<1>(aa).matrix() * + std::get<2>(dt_aa)) + + (std::get<0>(aa).matrix() * std::get<1>(dt_aa) * + std::get<2>(dt_aa)))); + } + + static std::tuple a( + std::tuple const& control_tangent_vectors, + double u) { + Eigen::Vector3d b = details::CubicBSplineBasis::b(u); + return std::make_tuple( + LieGroup::exp(b[0] * std::get<0>(control_tangent_vectors)), + LieGroup::exp(b[1] * std::get<1>(control_tangent_vectors)), + LieGroup::exp(b[2] * std::get<2>(control_tangent_vectors))); + } + + static Transformation dx1A( + Tangent const& control_tangent_vectors, double u, int i) { + Eigen::Vector3d b = details::CubicBSplineBasis::b(u); + Eigen::Vector t; + t[i] = Scalar(1.0); + return LieGroup::hat(t) * LieGroup::exp(b[0] * t).matrix(); + } + + static std::tuple dtA( + std::tuple const& aa, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + Eigen::Vector3d dt_b = details::CubicBSplineBasis::dtB(u, delta_t); + return std::make_tuple( + dt_b[0] * std::get<0>(aa).matrix() * + LieGroup::hat(std::get<0>(control_tangent_vectors)), + dt_b[1] * std::get<1>(aa).matrix() * + LieGroup::hat(std::get<1>(control_tangent_vectors)), + dt_b[2] * std::get<2>(aa).matrix() * + LieGroup::hat(std::get<2>(control_tangent_vectors))); + } + + static std::tuple dt2A( + std::tuple const& aa, + std::tuple const& dt_aa, + std::tuple const& control_tangent_vectors, + double u, + double delta_t) { + Eigen::Vector3d dt_b = details::CubicBSplineBasis::dtB(u, delta_t); + Eigen::Vector3d dt2_b = + details::CubicBSplineBasis::dt2B(u, delta_t); + + return std::make_tuple( + (dt_b[0] * std::get<0>(dt_aa).matrix() + + dt2_b[0] * std::get<0>(aa).matrix()) * + LieGroup::hat(std::get<0>(control_tangent_vectors)), + (dt_b[1] * std::get<1>(dt_aa).matrix() + + dt2_b[1] * std::get<1>(aa).matrix()) * + LieGroup::hat(std::get<1>(control_tangent_vectors)), + (dt_b[2] * std::get<2>(dt_aa).matrix() + + dt2_b[2] * std::get<2>(aa).matrix()) * + LieGroup::hat(std::get<2>(control_tangent_vectors))); + } +}; + +template +class CubicLieGroupBSplineImpl { + public: + using LieGroup = TGroup; + using Scalar = typename LieGroup::Scalar; + using Transformation = + Eigen::Matrix; + using Tangent = typename LieGroup::Tangent; + + CubicLieGroupBSplineImpl( + std::vector const& parent_ts_control_point, double delta_t) + : parent_from_control_point_transforms_(parent_ts_control_point), + delta_transform_(delta_t) { + SOPHUS_ASSERT( + parent_from_control_point_transforms_.size() >= 2u, + ", but {}", + parent_from_control_point_transforms_.size()); + } + + [[nodiscard]] LieGroup parentFromSpline(int i, double u) const { + SOPHUS_ASSERT(i >= 0, "i = {}", i); + SOPHUS_ASSERT( + i < this->getNumSegments(), + "i = {}; this->getNumSegments() = {}; " + "parent_from_control_point_transforms_.size() = {}", + i, + this->getNumSegments(), + parent_from_control_point_transforms_.size()); + + details::SegmentCase segment_case = + i == 0 + ? details::SegmentCase::first + : (i == this->getNumSegments() - 1 ? details::SegmentCase::last + : details::SegmentCase::normal); + + int idx_prev = std::max(0, i - 1); + int idx_0 = i; + int idx_1 = i + 1; + int idx_2 = std::min( + i + 2, int(this->parent_from_control_point_transforms_.size()) - 1); + + return details::CubicLieGroupBSplineSegment( + segment_case, + parent_from_control_point_transforms_[idx_prev].params(), + parent_from_control_point_transforms_[idx_0].params(), + parent_from_control_point_transforms_[idx_1].params(), + parent_from_control_point_transforms_[idx_2].params()) + .parentFromSpline(u); + } + + [[nodiscard]] Transformation dtParentFromSpline(int i, double u) const { + SOPHUS_ASSERT(i >= 0, "i = {}", i); + SOPHUS_ASSERT( + i < this->getNumSegments(), + "i = {}; this->getNumSegments() = {}; " + "parent_from_control_point_transforms_.size() = {}", + i, + this->getNumSegments(), + parent_from_control_point_transforms_.size()); + + details::SegmentCase segment_case = + i == 0 + ? details::SegmentCase::first + : (i == this->getNumSegments() - 1 ? details::SegmentCase::last + : details::SegmentCase::normal); + + int idx_prev = std::max(0, i - 1); + int idx_0 = i; + int idx_1 = i + 1; + int idx_2 = std::min( + i + 2, int(this->parent_from_control_point_transforms_.size()) - 1); + + return details::CubicLieGroupBSplineSegment( + segment_case, + parent_from_control_point_transforms_[idx_prev].params(), + parent_from_control_point_transforms_[idx_0].params(), + parent_from_control_point_transforms_[idx_1].params(), + parent_from_control_point_transforms_[idx_2].params()) + .dtParentFromSpline(u, delta_transform_); + } + + [[nodiscard]] Transformation dt2ParentFromSpline(int i, double u) const { + SOPHUS_ASSERT(i >= 0, "i = {}", i); + SOPHUS_ASSERT( + i < this->getNumSegments(), + "i = {}; this->getNumSegments() = {}; " + "parent_from_control_point_transforms_.size() = {}", + i, + this->getNumSegments(), + parent_from_control_point_transforms_.size()); + + details::SegmentCase segment_case = + i == 0 + ? details::SegmentCase::first + : (i == this->getNumSegments() - 1 ? details::SegmentCase::last + : details::SegmentCase::normal); + + int idx_prev = std::max(0, i - 1); + int idx_0 = i; + int idx_1 = i + 1; + int idx_2 = std::min( + i + 2, int(this->parent_from_control_point_transforms_.size()) - 1); + + return details::CubicLieGroupBSplineSegment( + segment_case, + parent_from_control_point_transforms_[idx_prev].params(), + parent_from_control_point_transforms_[idx_0].params(), + parent_from_control_point_transforms_[idx_1].params(), + parent_from_control_point_transforms_[idx_2].params()) + .dt2ParentFromSpline(u, delta_transform_); + } + + [[nodiscard]] std::vector const& parentFromsControlPoint() const { + return parent_from_control_point_transforms_; + } + + std::vector& parentFromsControlPoint() { + return parent_from_control_point_transforms_; + } + + [[nodiscard]] int getNumSegments() const { + return int(parent_from_control_point_transforms_.size()) - 1; + } + + [[nodiscard]] double deltaT() const { return delta_transform_; } + + private: + std::vector parent_from_control_point_transforms_; + double delta_transform_; +}; + +template +class CubicLieGroupBSpline { + public: + using LieGroup = TGroup; + using Scalar = typename LieGroup::Scalar; + using Transformation = + Eigen::Matrix; + using Tangent = typename LieGroup::Tangent; + + CubicLieGroupBSpline( + std::vector parent_ts_control_point, double t0, double delta_t) + : impl_(std::move(parent_ts_control_point), delta_t), t0_(t0) {} + + [[nodiscard]] LieGroup parentFromSpline(double t) const { + SegmentCoordinate index_and_u = this->indexAndU(t); + + return impl_.parentFromSpline( + index_and_u.segment_idx, index_and_u.fraction); + } + + [[nodiscard]] Transformation dtParentFromSpline(double t) const { + SegmentCoordinate index_and_u = this->indexAndU(t); + return impl_.dtParentFromSpline( + index_and_u.segment_idx, index_and_u.fraction); + } + + [[nodiscard]] Transformation dt2ParentFromSpline(double t) const { + SegmentCoordinate index_and_u = this->indexAndU(t); + return impl_.dt2ParentFromSpline( + index_and_u.segment_idx, index_and_u.fraction); + } + + [[nodiscard]] double t0() const { return t0_; } + + [[nodiscard]] double tmax() const { + return t0_ + impl_.deltaT() * getNumSegments(); + } + + [[nodiscard]] std::vector const& parentFromsControlPoint() const { + return impl_.parentFromsControlPoint(); + } + + std::vector& parentFromsControlPoint() { + return impl_.parentFromsControlPoint(); + } + + [[nodiscard]] int getNumSegments() const { return impl_.getNumSegments(); } + + [[nodiscard]] double s(double t) const { return (t - t0_) / impl_.deltaT(); } + + [[nodiscard]] double deltaT() const { return impl_.deltaT(); } + + [[nodiscard]] SegmentCoordinate indexAndU(double t) const { + SOPHUS_ASSERT(t >= t0_, "{} vs. {}", t, t0_); + SOPHUS_ASSERT(t <= this->tmax(), "{} vs. {}", t, this->tmax()); + + double s = this->s(t); + double i = NAN; + SegmentCoordinate index_and_u; + index_and_u.fraction = std::modf(s, &i); + index_and_u.segment_idx = int(i); + if (index_and_u.fraction > sophus::kEpsilonF64) { + return index_and_u; + } + + // u ~=~ 0.0 + if (index_and_u.segment_idx < getNumSegments() / 2) { + // First half of spline, keep as is (i, 0.0). + return index_and_u; + } + // Second half of spline, use (i-1, 1.0) instead. This way we can + // represent t == tmax (and not just t impl_; + + double t0_; +}; + +template +using BasisSpline = CubicLieGroupBSpline; + +} // namespace sophus diff --git a/cpp/sophus/lie/interp/spline_test.cpp b/cpp/sophus/interp/spline/spline_test.cpp similarity index 77% rename from cpp/sophus/lie/interp/spline_test.cpp rename to cpp/sophus/interp/spline/spline_test.cpp index c2e183c2..08320ba9 100644 --- a/cpp/sophus/lie/interp/spline_test.cpp +++ b/cpp/sophus/interp/spline/spline_test.cpp @@ -6,9 +6,10 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include "sophus/lie/interp/spline.h" - #include "sophus/calculus/num_diff.h" +#include "sophus/interp/interpolate.h" +#include "sophus/interp/spline/bspline.h" +#include "sophus/interp/spline/group_bspline.h" #include "sophus/lie/isometry2.h" #include "sophus/lie/isometry3.h" #include "sophus/lie/rotation2.h" @@ -65,7 +66,7 @@ struct SplinePropTestSuite { control_poses.push_back(t_world_inter); } - BasisSplineImpl spline(control_poses, 1.0); + CubicLieGroupBSplineImpl spline(control_poses, 1.0); Group t = spline.parentFromSpline(0.0, 1.0); Group t2 = spline.parentFromSpline(1.0, 0.0); @@ -149,10 +150,12 @@ decltype(pointExamples()) pointExamples(); //! @endcond -TEST(lie_groups, linterpolate_prop_tests) { +TEST(lie_group_bspline, lie_group_bspline_prop_test) { SplinePropTestSuite>::runAllTests("Scaling2"); SplinePropTestSuite>::runAllTests("Scaling3"); + SplinePropTestSuite>::runAllTests("Translation1"); + SplinePropTestSuite>::runAllTests("Translation2"); SplinePropTestSuite>::runAllTests("Translation3"); SplinePropTestSuite>::runAllTests( @@ -172,4 +175,49 @@ TEST(lie_groups, linterpolate_prop_tests) { SplinePropTestSuite>::runAllTests("Similarity2"); SplinePropTestSuite>::runAllTests("Similarity3"); } + +TEST(bspline, cartesian_vs_lie_group_bspline) { + auto translation_elements = Translation3F64::elementExamples(); + + std::vector control_points; + for (auto const& t : translation_elements) { + control_points.push_back(t.params()); + } + + double t0 = -2.0; + double delta_t = 0.17; + CubicLieGroupBSpline group_spline( + translation_elements, t0, delta_t); + CubicBSpline spline(control_points, t0, delta_t); + + SOPHUS_ASSERT_EQ(group_spline.getNumSegments(), spline.getNumSegments()); + SOPHUS_ASSERT_EQ(group_spline.t0(), spline.t0()); + SOPHUS_ASSERT_EQ(group_spline.tmax(), spline.tmax()); + + for (double t = group_spline.t0(); t < group_spline.tmax(); t += 0.02) { + SOPHUS_ASSERT_WITHIN_REL( + group_spline.parentFromSpline(t).params(), + spline.interpolate(t), + 0.0001, + ""); + + for (size_t i = 0; i < translation_elements.size(); ++i) { + Eigen::Matrix dx = spline.dxiInterpolate(t, i); + + Eigen::Matrix const num_dx = + vectorFieldNumDiff( + [&](Eigen::Vector x) -> Eigen::Vector3d { + auto control_points_copy = control_points; + control_points_copy[i] = x; + CubicBSpline spline( + control_points_copy, t0, delta_t); + return spline.interpolate(t); + }, + control_points[i], + 0.001); + + SOPHUS_ASSERT_WITHIN_REL(num_dx, dx, 0.0001, ""); + } + } +} } // namespace sophus::test diff --git a/cpp/sophus/lie/CMakeLists.txt b/cpp/sophus/lie/CMakeLists.txt index bfe3e4ce..2c83fe8a 100644 --- a/cpp/sophus/lie/CMakeLists.txt +++ b/cpp/sophus/lie/CMakeLists.txt @@ -46,5 +46,3 @@ foreach(test_basename ${sophus_lie_src_prefixes}) LINK_LIBRARIES sophus_lie LABELS small) endforeach() - -add_subdirectory(interp) diff --git a/cpp/sophus/lie/group_manifold.h b/cpp/sophus/lie/group_manifold.h index 98d7ba29..b656383f 100644 --- a/cpp/sophus/lie/group_manifold.h +++ b/cpp/sophus/lie/group_manifold.h @@ -8,7 +8,7 @@ #pragma once -#include "sophus/lie/interp/average.h" +#include "sophus/interp/average.h" #include "sophus/lie/lie_group.h" namespace sophus { diff --git a/cpp/sophus/lie/impl/translation_factor_group_product.h b/cpp/sophus/lie/impl/translation_factor_group_product.h index 1e0c45ac..ef5aa225 100644 --- a/cpp/sophus/lie/impl/translation_factor_group_product.h +++ b/cpp/sophus/lie/impl/translation_factor_group_product.h @@ -296,6 +296,9 @@ class TranslationFactorGroupProduct { // for tests static auto tangentExamples() -> std::vector { + if constexpr (FactorGroup::kDof == 0) { + return exampleTranslations(); + } std::vector examples; for (auto const& group_tangent : FactorGroup::tangentExamples()) { for (auto const& translation_tangents : exampleTranslations()) { @@ -307,6 +310,9 @@ class TranslationFactorGroupProduct { static auto paramsExamples() -> std::vector { std::vector examples; + if constexpr (FactorGroup::kDof == 0) { + return exampleTranslations(); + } for (auto const& factor_group_params : FactorGroup::paramsExamples()) { for (auto const& right_params : exampleTranslations()) { examples.push_back(params(factor_group_params, right_params)); diff --git a/cpp/sophus/lie/interp/CMakeLists.txt b/cpp/sophus/lie/interp/CMakeLists.txt deleted file mode 100644 index 4858ae07..00000000 --- a/cpp/sophus/lie/interp/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -#[[ -sophus_lie_interp - -]] - -farm_ng_add_library(sophus_lie_interp - NAMESPACE Sophus - INCLUDE_DIR ../../../ - HEADERS - average.h - interpolate.h - spline.h -) -target_link_libraries(sophus_lie_interp INTERFACE sophus_lie) - -farm_ng_add_test(interpolate - PARENT_LIBRARY sophus_lie_interp - LINK_LIBRARIES sophus_lie_interp - LABELS small) - -farm_ng_add_test(spline - PARENT_LIBRARY sophus_lie_interp - LINK_LIBRARIES sophus_lie_interp - LABELS small) diff --git a/cpp/sophus/lie/interp/spline.h b/cpp/sophus/lie/interp/spline.h deleted file mode 100644 index 4cebc0c4..00000000 --- a/cpp/sophus/lie/interp/spline.h +++ /dev/null @@ -1,530 +0,0 @@ -// Copyright (c) 2011, Hauke Strasdat -// Copyright (c) 2012, Steven Lovegrove -// Copyright (c) 2021, farm-ng, inc. -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -/// @file -// Basis spline implementation on Lie Group following: -// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013 -// http://www.bmva.org/bmvc/2013/Papers/paper0093/paper0093.pdf - -#pragma once - -#include "sophus/common/common.h" -#include "sophus/lie/interp/interpolate.h" -#include "sophus/lie/lie_group.h" - -namespace sophus { - -template -class SplineBasisFunction { - public: - static Eigen::Matrix c() { - Eigen::Matrix c; - TScalar const o(0); - - // clang-format off - c << TScalar(5./6), TScalar(3./6), -TScalar(3./6), TScalar(1./6), - TScalar(1./6), TScalar(3./6), TScalar(3./6), -TScalar(2./6), - o, o, o, TScalar(1./6); - // clang-format on - return c; - } - - static Eigen::Vector3 b(TScalar const& u) { - // SOPHUS_ASSERT(u >= TScalar(0), "but %", u); - // SOPHUS_ASSERT(u < TScalar(1), "but %", u); - TScalar u_square(u * u); - return c() * Eigen::Vector4(TScalar(1), u, u_square, u * u_square); - } - - static Eigen::Vector3 dtB(TScalar const& u, TScalar const& delta_t) { - // SOPHUS_ASSERT(u >= TScalar(0), "but %", u); - // SOPHUS_ASSERT(u < TScalar(1), "but %", u); - return (TScalar(1) / delta_t) * c() * - Eigen::Vector4( - TScalar(0), TScalar(1), TScalar(2) * u, TScalar(3) * u * u); - } - - static Eigen::Vector3 dt2B( - TScalar const& u, TScalar const& delta_t) { - // SOPHUS_ASSERT(u >= TScalar(0), "but %", u); - // SOPHUS_ASSERT(u < TScalar(1), "but %", u); - return (TScalar(1) / (delta_t * delta_t)) * c() * - Eigen::Vector4( - TScalar(0), TScalar(0), TScalar(2), TScalar(6) * u); - } -}; - -template -class BasisSplineFn { - public: - using LieGroup = TGroup; - using Scalar = typename LieGroup::Scalar; - using Transformation = - Eigen::Matrix; - using Tangent = typename LieGroup::Tangent; - - static LieGroup parentFromSpline( - LieGroup const& parent_ts_control_point, - std::tuple const& control_tagent_vectors, - double u) { - auto aa = a(control_tagent_vectors, u); - return parent_ts_control_point * std::get<0>(aa) * std::get<1>(aa) * - std::get<2>(aa); - } - - static Transformation dtParentFromSpline( - LieGroup const& parent_ts_control_point, - std::tuple const& control_tagent_vectors, - double u, - double delta_t) { - auto aa = a(control_tagent_vectors, u); - auto dt_aa = dtA(aa, control_tagent_vectors, u, delta_t); - return parent_ts_control_point.matrix() * - ((std::get<0>(dt_aa) * std::get<1>(aa).matrix() * - std::get<2>(aa).matrix()) + - (std::get<0>(aa).matrix() * std::get<1>(dt_aa) * - std::get<2>(aa).matrix()) + - (std::get<0>(aa).matrix() * std::get<1>(aa).matrix() * - std::get<2>(dt_aa))); - } - - static Transformation dt2ParentFromSpline( - LieGroup const& parent_ts_control_point, - std::tuple const& control_tagent_vectors, - double u, - double delta_t) { - using TScalar = typename LieGroup::Scalar; - auto aa = a(control_tagent_vectors, u); - auto dt_aa = dtA(aa, control_tagent_vectors, u, delta_t); - auto dt2_aa = dt2A(aa, dt_aa, control_tagent_vectors, u, delta_t); - - return parent_ts_control_point.matrix() * - ((std::get<0>(dt2_aa) * std::get<1>(aa).matrix() * - std::get<2>(aa).matrix()) + - (std::get<0>(aa).matrix() * std::get<1>(dt2_aa) * - std::get<2>(aa).matrix()) + - (std::get<0>(aa).matrix() * std::get<1>(aa).matrix() * - std::get<2>(dt2_aa)) + - TScalar(2) * ((std::get<0>(dt_aa) * std::get<1>(dt_aa) * - std::get<2>(aa).matrix()) + - (std::get<0>(dt_aa) * std::get<1>(aa).matrix() * - std::get<2>(dt_aa)) + - (std::get<0>(aa).matrix() * std::get<1>(dt_aa) * - std::get<2>(dt_aa)))); - } - - private: - static std::tuple a( - std::tuple const& control_tagent_vectors, - double u) { - Eigen::Vector3d b = SplineBasisFunction::b(u); - return std::make_tuple( - LieGroup::exp(b[0] * std::get<0>(control_tagent_vectors)), - LieGroup::exp(b[1] * std::get<1>(control_tagent_vectors)), - LieGroup::exp(b[2] * std::get<2>(control_tagent_vectors))); - } - - static std::tuple dtA( - std::tuple const& aa, - std::tuple const& control_tagent_vectors, - double u, - double delta_t) { - Eigen::Vector3d dt_b = SplineBasisFunction::dtB(u, delta_t); - return std::make_tuple( - dt_b[0] * std::get<0>(aa).matrix() * - LieGroup::hat(std::get<0>(control_tagent_vectors)), - dt_b[1] * std::get<1>(aa).matrix() * - LieGroup::hat(std::get<1>(control_tagent_vectors)), - dt_b[2] * std::get<2>(aa).matrix() * - LieGroup::hat(std::get<2>(control_tagent_vectors))); - } - - static std::tuple dt2A( - std::tuple const& aa, - std::tuple const& dt_aa, - std::tuple const& control_tagent_vectors, - double u, - double delta_t) { - Eigen::Vector3d dt_b = SplineBasisFunction::dtB(u, delta_t); - Eigen::Vector3d dt2_b = SplineBasisFunction::dt2B(u, delta_t); - - return std::make_tuple( - (dt_b[0] * std::get<0>(dt_aa).matrix() + - dt2_b[0] * std::get<0>(aa).matrix()) * - LieGroup::hat(std::get<0>(control_tagent_vectors)), - (dt_b[1] * std::get<1>(dt_aa).matrix() + - dt2_b[1] * std::get<1>(aa).matrix()) * - LieGroup::hat(std::get<1>(control_tagent_vectors)), - (dt_b[2] * std::get<2>(dt_aa).matrix() + - dt2_b[2] * std::get<2>(aa).matrix()) * - LieGroup::hat(std::get<2>(control_tagent_vectors))); - } -}; - -enum class SegmentCase { first, normal, last }; - -template -struct BasisSplineSegment { - public: - using Scalar = typename TGroup::Scalar; - using T = Scalar; - using Transformation = - Eigen::Matrix; - using Params = typename TGroup::Params; - - BasisSplineSegment( - SegmentCase segment_case, - Params const& params0, - Params const& params1, - Params const& params2, - Params const& params3) - : segment_case_(segment_case), - params0_(params0), - params1_(params1), - params2_(params2), - params3_(params3) {} - - BasisSplineSegment(BasisSplineSegment const&) = delete; - BasisSplineSegment(BasisSplineSegment&&) = delete; - - auto operator=(BasisSplineSegment const&) -> BasisSplineSegment = delete; - auto operator=(BasisSplineSegment&&) -> BasisSplineSegment = delete; - - [[nodiscard]] TGroup worldFromFooPrev() const { - return TGroup::fromParams(params0_); - } - [[nodiscard]] TGroup worldFromFoo0() const { - return TGroup::fromParams(params1_); - } - - [[nodiscard]] TGroup worldFromFoo1() const { - return TGroup::fromParams(params2_); - } - - [[nodiscard]] TGroup worldFromFoo2() const { - return TGroup::fromParams(params3_); - } - TGroup parentFromSpline(double u) { - switch (segment_case_) { - case SegmentCase::first: - return BasisSplineFn::parentFromSpline( - worldFromFoo0(), - std::make_tuple( - (worldFromFoo0().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo2()).log()), - u); - case SegmentCase::normal: - return BasisSplineFn::parentFromSpline( - worldFromFooPrev(), - std::make_tuple( - (worldFromFooPrev().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo2()).log()), - u); - case SegmentCase::last: - return BasisSplineFn::parentFromSpline( - worldFromFooPrev(), - std::make_tuple( - (worldFromFooPrev().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo1()).log()), - u); - } - SOPHUS_PANIC("logic error"); - } - - Transformation dtParentFromSpline(double u, double delta_t) { - switch (segment_case_) { - case SegmentCase::first: - return BasisSplineFn::dtParentFromSpline( - worldFromFoo0(), - std::make_tuple( - (worldFromFoo0().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo2()).log()), - u, - delta_t); - case SegmentCase::normal: - return BasisSplineFn::dtParentFromSpline( - worldFromFooPrev(), - std::make_tuple( - (worldFromFooPrev().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo2()).log()), - u, - delta_t); - case SegmentCase::last: - return BasisSplineFn::dtParentFromSpline( - worldFromFooPrev(), - std::make_tuple( - (worldFromFooPrev().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo1()).log()), - u, - delta_t); - } - SOPHUS_PANIC("logic error"); - } - - Transformation dt2ParentFromSpline(double u, double delta_t) { - switch (segment_case_) { - case SegmentCase::first: - return BasisSplineFn::dt2ParentFromSpline( - worldFromFoo0(), - std::make_tuple( - (worldFromFoo0().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo2()).log()), - u, - delta_t); - case SegmentCase::normal: - return BasisSplineFn::dt2ParentFromSpline( - worldFromFooPrev(), - std::make_tuple( - (worldFromFooPrev().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo2()).log()), - u, - delta_t); - case SegmentCase::last: - return BasisSplineFn::dt2ParentFromSpline( - worldFromFooPrev(), - std::make_tuple( - (worldFromFooPrev().inverse() * worldFromFoo0()).log(), - (worldFromFoo0().inverse() * worldFromFoo1()).log(), - (worldFromFoo1().inverse() * worldFromFoo1()).log()), - u, - delta_t); - } - SOPHUS_PANIC("logic error"); - } - - private: - SegmentCase segment_case_; - Params const& params0_; - Params const& params1_; - Params const& params2_; - Params const& params3_; -}; - -template -class BasisSplineImpl { - public: - using LieGroup = TGroup; - using Scalar = typename LieGroup::Scalar; - using Transformation = - Eigen::Matrix; - using Tangent = typename LieGroup::Tangent; - - BasisSplineImpl( - std::vector const& parent_ts_control_point, double delta_t) - : parent_from_control_point_transforms_(parent_ts_control_point), - delta_transform_(delta_t) { - SOPHUS_ASSERT( - parent_from_control_point_transforms_.size() >= 2u, - ", but {}", - parent_from_control_point_transforms_.size()); - } - - [[nodiscard]] LieGroup parentFromSpline(int i, double u) const { - SOPHUS_ASSERT(i >= 0, "i = {}", i); - SOPHUS_ASSERT( - i < this->getNumSegments(), - "i = {}; this->getNumSegments() = {}; " - "parent_from_control_point_transforms_.size() = {}", - i, - this->getNumSegments(), - parent_from_control_point_transforms_.size()); - - SegmentCase segment_case = - i == 0 ? SegmentCase::first - : (i == this->getNumSegments() - 1 ? SegmentCase::last - : SegmentCase::normal); - - int idx_prev = std::max(0, i - 1); - int idx_0 = i; - int idx_1 = i + 1; - int idx_2 = std::min( - i + 2, int(this->parent_from_control_point_transforms_.size()) - 1); - - return BasisSplineSegment( - segment_case, - parent_from_control_point_transforms_[idx_prev].params(), - parent_from_control_point_transforms_[idx_0].params(), - parent_from_control_point_transforms_[idx_1].params(), - parent_from_control_point_transforms_[idx_2].params()) - .parentFromSpline(u); - } - - [[nodiscard]] Transformation dtParentFromSpline(int i, double u) const { - SOPHUS_ASSERT(i >= 0, "i = {}", i); - SOPHUS_ASSERT( - i < this->getNumSegments(), - "i = {}; this->getNumSegments() = {}; " - "parent_from_control_point_transforms_.size() = {}", - i, - this->getNumSegments(), - parent_from_control_point_transforms_.size()); - - SegmentCase segment_case = - i == 0 ? SegmentCase::first - : (i == this->getNumSegments() - 1 ? SegmentCase::last - : SegmentCase::normal); - - int idx_prev = std::max(0, i - 1); - int idx_0 = i; - int idx_1 = i + 1; - int idx_2 = std::min( - i + 2, int(this->parent_from_control_point_transforms_.size()) - 1); - - return BasisSplineSegment( - segment_case, - parent_from_control_point_transforms_[idx_prev].params(), - parent_from_control_point_transforms_[idx_0].params(), - parent_from_control_point_transforms_[idx_1].params(), - parent_from_control_point_transforms_[idx_2].params()) - .dtParentFromSpline(u, delta_transform_); - } - - [[nodiscard]] Transformation dt2ParentFromSpline(int i, double u) const { - SOPHUS_ASSERT(i >= 0, "i = {}", i); - SOPHUS_ASSERT( - i < this->getNumSegments(), - "i = {}; this->getNumSegments() = {}; " - "parent_from_control_point_transforms_.size() = {}", - i, - this->getNumSegments(), - parent_from_control_point_transforms_.size()); - - SegmentCase segment_case = - i == 0 ? SegmentCase::first - : (i == this->getNumSegments() - 1 ? SegmentCase::last - : SegmentCase::normal); - - int idx_prev = std::max(0, i - 1); - int idx_0 = i; - int idx_1 = i + 1; - int idx_2 = std::min( - i + 2, int(this->parent_from_control_point_transforms_.size()) - 1); - - return BasisSplineSegment( - segment_case, - parent_from_control_point_transforms_[idx_prev].params(), - parent_from_control_point_transforms_[idx_0].params(), - parent_from_control_point_transforms_[idx_1].params(), - parent_from_control_point_transforms_[idx_2].params()) - .dt2ParentFromSpline(u, delta_transform_); - } - - [[nodiscard]] std::vector const& parentFromsControlPoint() const { - return parent_from_control_point_transforms_; - } - - std::vector& parentFromsControlPoint() { - return parent_from_control_point_transforms_; - } - - [[nodiscard]] int getNumSegments() const { - return int(parent_from_control_point_transforms_.size()) - 1; - } - - [[nodiscard]] double deltaT() const { return delta_transform_; } - - private: - std::vector parent_from_control_point_transforms_; - double delta_transform_; -}; - -struct IndexAndU { - int i; - double u; -}; - -template -class BasisSpline { - public: - using LieGroup = TGroup; - using Scalar = typename LieGroup::Scalar; - using Transformation = - Eigen::Matrix; - using Tangent = typename LieGroup::Tangent; - - BasisSpline( - std::vector parent_ts_control_point, double t0, double delta_t) - : impl_(std::move(parent_ts_control_point), delta_t), t0_(t0) {} - - [[nodiscard]] LieGroup parentFromSpline(double t) const { - IndexAndU index_and_u = this->indexAndU(t); - - return impl_.parentFromSpline(index_and_u.i, index_and_u.u); - } - - [[nodiscard]] Transformation dtParentFromSpline(double t) const { - IndexAndU index_and_u = this->indexAndU(t); - return impl_.dtParentFromSpline(index_and_u.i, index_and_u.u); - } - - [[nodiscard]] Transformation dt2ParentFromSpline(double t) const { - IndexAndU index_and_u = this->indexAndU(t); - return impl_.dt2ParentFromSpline(index_and_u.i, index_and_u.u); - } - - [[nodiscard]] double t0() const { return t0_; } - - [[nodiscard]] double tmax() const { - return t0_ + impl_.deltaT() * getNumSegments(); - } - - [[nodiscard]] std::vector const& parentFromsControlPoint() const { - return impl_.parentFromsControlPoint(); - } - - std::vector& parentFromsControlPoint() { - return impl_.parentFromsControlPoint(); - } - - [[nodiscard]] int getNumSegments() const { return impl_.getNumSegments(); } - - [[nodiscard]] double s(double t) const { return (t - t0_) / impl_.deltaT(); } - - [[nodiscard]] double deltaT() const { return impl_.deltaT(); } - - [[nodiscard]] IndexAndU indexAndU(double t) const { - SOPHUS_ASSERT(t >= t0_, "{} vs. {}", t, t0_); - SOPHUS_ASSERT(t <= this->tmax(), "{} vs. {}", t, this->tmax()); - - double s = this->s(t); - double i = NAN; - IndexAndU index_and_u; - index_and_u.u = std::modf(s, &i); - index_and_u.i = int(i); - if (index_and_u.u > sophus::kEpsilonF64) { - return index_and_u; - } - - // u ~=~ 0.0 - if (t < 0.5 * this->tmax()) { - // First half of spline, keep as is (i, 0.0). - return index_and_u; - } - // Second half of spline, use (i-1, 1.0) instead. This way we can represent - // t == tmax (and not just t impl_; - - double t0_; -}; - -} // namespace sophus