ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

115
thirdparty/basalt-headers/include/basalt/spline/ceres_local_param.hpp vendored Normal file
View File

@@ -0,0 +1,115 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Generic local parametrization for Sophus Lie group types to be used with
ceres.
*/
/**
File adapted from Sophus
Copyright 2011-2017 Hauke Strasdat
2012-2017 Steven Lovegrove
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
#include <ceres/local_parameterization.h>
#include <sophus/se3.hpp>
namespace basalt {
/// @brief Local parametrization for ceres that can be used with Sophus Lie
/// group implementations.
template <class Groupd>
class LieLocalParameterization : public ceres::LocalParameterization {
public:
virtual ~LieLocalParameterization() {}
using Tangentd = typename Groupd::Tangent;
/// @brief plus operation for Ceres
///
/// T * exp(x)
///
virtual bool Plus(double const* T_raw, double const* delta_raw,
double* T_plus_delta_raw) const {
Eigen::Map<Groupd const> const T(T_raw);
Eigen::Map<Tangentd const> const delta(delta_raw);
Eigen::Map<Groupd> T_plus_delta(T_plus_delta_raw);
T_plus_delta = T * Groupd::exp(delta);
return true;
}
///@brief Jacobian of plus operation for Ceres
///
/// Dx T * exp(x) with x=0
///
virtual bool ComputeJacobian(double const* T_raw,
double* jacobian_raw) const {
Eigen::Map<Groupd const> T(T_raw);
Eigen::Map<Eigen::Matrix<double, Groupd::num_parameters, Groupd::DoF,
Eigen::RowMajor>>
jacobian(jacobian_raw);
jacobian = T.Dx_this_mul_exp_x_at_0();
return true;
}
///@brief Global size
virtual int GlobalSize() const { return Groupd::num_parameters; }
///@brief Local size
virtual int LocalSize() const { return Groupd::DoF; }
};
} // namespace basalt

236
thirdparty/basalt-headers/include/basalt/spline/ceres_spline_helper.h vendored Normal file
View File

@@ -0,0 +1,236 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Helper for implementing Lie group and Euclidean b-splines in ceres.
*/
#pragma once
#include <basalt/spline/spline_common.h>
#include <Eigen/Dense>
namespace basalt {
/// @brief Helper for implementing Lie group and Euclidean b-splines in ceres of
/// order N
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _N>
struct CeresSplineHelper {
static constexpr int N = _N; // Order of the spline.
static constexpr int DEG = _N - 1; // Degree of the spline.
using MatN = Eigen::Matrix<double, _N, _N>;
using VecN = Eigen::Matrix<double, _N, 1>;
static const MatN blending_matrix_;
static const MatN cumulative_blending_matrix_;
static const MatN base_coefficients_;
/// @brief Vector of derivatives of time polynomial.
///
/// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots &
/// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first
/// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1)
/// t^{N-2}\end{bmatrix} \f$.
/// @param Derivative derivative to evaluate
/// @param[out] res_const vector to store the result
/// @param[in] t
template <int Derivative, class Derived>
static inline void baseCoeffsWithTime(
const Eigen::MatrixBase<Derived>& res_const, double t) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N);
Eigen::MatrixBase<Derived>& res =
const_cast<Eigen::MatrixBase<Derived>&>(res_const);
res.setZero();
if (Derivative < N) {
res[Derivative] = base_coefficients_(Derivative, Derivative);
double _t = t;
for (int j = Derivative + 1; j < N; j++) {
res[j] = base_coefficients_(Derivative, j) * _t;
_t = _t * t;
}
}
}
/// @brief Evaluate Lie group cummulative B-spline and time derivatives.
///
/// @param[in] sKnots array of pointers of the spline knots. The size of each
/// knot should be GroupT::num_parameters: 4 for SO(3) and 7 for SE(3).
/// @param[in] u normalized time to compute value of the spline
/// @param[in] inv_dt inverse of the time spacing in seconds between spline
/// knots
/// @param[out] transform_out if not nullptr return the value of the spline
/// @param[out] vel_out if not nullptr velocity (first time derivative) in the
/// body frame
/// @param[out] accel_out if not nullptr acceleration (second time derivative)
/// in the body frame
template <class T, template <class> class GroupT>
static inline void evaluate_lie(
T const* const* sKnots, const double u, const double inv_dt,
GroupT<T>* transform_out = nullptr,
typename GroupT<T>::Tangent* vel_out = nullptr,
typename GroupT<T>::Tangent* accel_out = nullptr,
typename GroupT<T>::Tangent* jerk_out = nullptr) {
using Group = GroupT<T>;
using Tangent = typename GroupT<T>::Tangent;
using Adjoint = typename GroupT<T>::Adjoint;
VecN p, coeff, dcoeff, ddcoeff, dddcoeff;
CeresSplineHelper<N>::template baseCoeffsWithTime<0>(p, u);
coeff = CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
if (vel_out || accel_out || jerk_out) {
CeresSplineHelper<N>::template baseCoeffsWithTime<1>(p, u);
dcoeff = inv_dt * CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
if (accel_out || jerk_out) {
CeresSplineHelper<N>::template baseCoeffsWithTime<2>(p, u);
ddcoeff = inv_dt * inv_dt *
CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
if (jerk_out) {
CeresSplineHelper<N>::template baseCoeffsWithTime<3>(p, u);
dddcoeff = inv_dt * inv_dt * inv_dt *
CeresSplineHelper<N>::cumulative_blending_matrix_ * p;
}
}
}
if (transform_out) {
Eigen::Map<Group const> const p00(sKnots[0]);
*transform_out = p00;
}
Tangent rot_vel, rot_accel, rot_jerk;
if (vel_out || accel_out || jerk_out) rot_vel.setZero();
if (accel_out || jerk_out) rot_accel.setZero();
if (jerk_out) rot_jerk.setZero();
for (int i = 0; i < DEG; i++) {
Eigen::Map<Group const> const p0(sKnots[i]);
Eigen::Map<Group const> const p1(sKnots[i + 1]);
Group r01 = p0.inverse() * p1;
Tangent delta = r01.log();
Group exp_kdelta = Group::exp(delta * coeff[i + 1]);
if (transform_out) (*transform_out) *= exp_kdelta;
if (vel_out || accel_out || jerk_out) {
Adjoint A = exp_kdelta.inverse().Adj();
rot_vel = A * rot_vel;
Tangent rot_vel_current = delta * dcoeff[i + 1];
rot_vel += rot_vel_current;
if (accel_out || jerk_out) {
rot_accel = A * rot_accel;
Tangent accel_lie_bracket =
Group::lieBracket(rot_vel, rot_vel_current);
rot_accel += ddcoeff[i + 1] * delta + accel_lie_bracket;
if (jerk_out) {
rot_jerk = A * rot_jerk;
rot_jerk += dddcoeff[i + 1] * delta +
Group::lieBracket(ddcoeff[i + 1] * rot_vel +
2 * dcoeff[i + 1] * rot_accel -
dcoeff[i + 1] * accel_lie_bracket,
delta);
}
}
}
}
if (vel_out) *vel_out = rot_vel;
if (accel_out) *accel_out = rot_accel;
if (jerk_out) *jerk_out = rot_jerk;
}
/// @brief Evaluate Euclidean B-spline or time derivatives.
///
/// @param[in] sKnots array of pointers of the spline knots. The size of each
/// knot should be DIM.
/// @param[in] u normalized time to compute value of the spline
/// @param[in] inv_dt inverse of the time spacing in seconds between spline
/// knots
/// @param[out] vec_out if DERIV=0 returns value of the spline, otherwise
/// corresponding derivative.
template <class T, int DIM, int DERIV>
static inline void evaluate(T const* const* sKnots, const double u,
const double inv_dt,
Eigen::Matrix<T, DIM, 1>* vec_out) {
if (!vec_out) return;
using VecD = Eigen::Matrix<T, DIM, 1>;
VecN p, coeff;
CeresSplineHelper<N>::template baseCoeffsWithTime<DERIV>(p, u);
coeff =
std::pow(inv_dt, DERIV) * CeresSplineHelper<N>::blending_matrix_ * p;
vec_out->setZero();
for (int i = 0; i < N; i++) {
Eigen::Map<VecD const> const p(sKnots[i]);
(*vec_out) += coeff[i] * p;
}
}
};
template <int _N>
const typename CeresSplineHelper<_N>::MatN
CeresSplineHelper<_N>::base_coefficients_ =
basalt::computeBaseCoefficients<_N, double>();
template <int _N>
const typename CeresSplineHelper<_N>::MatN
CeresSplineHelper<_N>::blending_matrix_ =
basalt::computeBlendingMatrix<_N, double, false>();
template <int _N>
const typename CeresSplineHelper<_N>::MatN
CeresSplineHelper<_N>::cumulative_blending_matrix_ =
basalt::computeBlendingMatrix<_N, double, true>();
} // namespace basalt

363
thirdparty/basalt-headers/include/basalt/spline/rd_spline.h vendored Normal file
View File

@@ -0,0 +1,363 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Uniform B-spline for euclidean vectors
*/
#pragma once
#include <basalt/spline/spline_common.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/sophus_utils.hpp>
#include <Eigen/Dense>
#include <array>
namespace basalt {
/// @brief Uniform B-spline for euclidean vectors with dimention DIM of order
/// N
///
/// For example, in the particular case scalar values and order N=5, for a time
/// \f$t \in [t_i, t_{i+1})\f$ the value of \f$p(t)\f$ depends only on 5 control
/// points at \f$[t_i, t_{i+1}, t_{i+2}, t_{i+3}, t_{i+4}]\f$. To
/// simplify calculations we transform time to uniform representation \f$s(t) =
/// (t - t_0)/\Delta t \f$, such that control points transform into \f$ s_i \in
/// [0,..,N] \f$. We define function \f$ u(t) = s(t)-s_i \f$ to be a time since
/// the start of the segment. Following the matrix representation of De Boor -
/// Cox formula, the value of the function can be
/// evaluated as follows: \f{align}{
/// p(u(t)) &=
/// \begin{pmatrix} p_{i}\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\\ p_{i+4}
/// \end{pmatrix}^T M_5 \begin{pmatrix} 1 \\ u \\ u^2 \\ u^3 \\ u^4
/// \end{pmatrix},
/// \f}
/// where \f$ p_{i} \f$ are knots and \f$ M_5 \f$ is a blending matrix computed
/// using \ref computeBlendingMatrix \f{align}{
/// M_5 = \frac{1}{4!}
/// \begin{pmatrix} 1 & -4 & 6 & -4 & 1 \\ 11 & -12 & -6 & 12 & -4 \\11 &
/// 12 & -6 & -12 & 6 \\ 1 & 4 & 6 & 4 & -4 \\ 0 & 0 & 0 & 0
/// & 1 \end{pmatrix}.
/// \f}
/// Given this formula, we can evaluate derivatives with respect to time
/// (velocity, acceleration) in the following way:
/// \f{align}{
/// p'(u(t)) &= \frac{1}{\Delta t}
/// \begin{pmatrix} p_{i}\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\\ p_{i+4}
/// \end{pmatrix}^T
/// M_5
/// \begin{pmatrix} 0 \\ 1 \\ 2u \\ 3u^2 \\ 4u^3 \end{pmatrix},
/// \f}
/// \f{align}{
/// p''(u(t)) &= \frac{1}{\Delta t^2}
/// \begin{pmatrix} p_{i}\\ p_{i+1}\\ p_{i+2}\\ p_{i+3}\\ p_{i+4}
/// \end{pmatrix}^T
/// M_5
/// \begin{pmatrix} 0 \\ 0 \\ 2 \\ 6u \\ 12u^2 \end{pmatrix}.
/// \f}
/// Higher time derivatives are evaluated similarly. This class supports
/// vector values for knots \f$ p_{i} \f$. The corresponding derivative vector
/// on the right is computed using \ref baseCoeffsWithTime.
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _DIM, int _N, typename _Scalar = double>
class RdSpline {
public:
static constexpr int N = _N; ///< Order of the spline.
static constexpr int DEG = _N - 1; ///< Degree of the spline.
static constexpr int DIM = _DIM; ///< Dimension of euclidean vector space.
static constexpr _Scalar NS_TO_S = 1e-9; ///< Nanosecond to second conversion
static constexpr _Scalar S_TO_NS = 1e9; ///< Second to nanosecond conversion
using MatN = Eigen::Matrix<_Scalar, _N, _N>;
using VecN = Eigen::Matrix<_Scalar, _N, 1>;
using VecD = Eigen::Matrix<_Scalar, _DIM, 1>;
using MatD = Eigen::Matrix<_Scalar, _DIM, _DIM>;
/// @brief Struct to store the Jacobian of the spline
///
/// Since B-spline of order N has local support (only N knots infuence the
/// value) the Jacobian is zero for all knots except maximum N for value and
/// all derivatives.
struct JacobianStruct {
size_t
start_idx; ///< Start index of the non-zero elements of the Jacobian.
std::array<_Scalar, N> d_val_d_knot; ///< Value of nonzero Jacobians.
};
/// @brief Default constructor
RdSpline() = default;
/// @brief Constructor with knot interval and start time
///
/// @param[in] time_interval_ns knot time interval in nanoseconds
/// @param[in] start_time_ns start time of the spline in nanoseconds
RdSpline(int64_t time_interval_ns, int64_t start_time_ns = 0)
: dt_ns_(time_interval_ns), start_t_ns_(start_time_ns) {
pow_inv_dt_[0] = 1.0;
pow_inv_dt_[1] = S_TO_NS / dt_ns_;
for (size_t i = 2; i < N; i++) {
pow_inv_dt_[i] = pow_inv_dt_[i - 1] * pow_inv_dt_[1];
}
}
/// @brief Cast to different scalar type
template <typename Scalar2>
inline RdSpline<_DIM, _N, Scalar2> cast() const {
RdSpline<_DIM, _N, Scalar2> res;
res.dt_ns_ = dt_ns_;
res.start_t_ns_ = start_t_ns_;
for (int i = 0; i < _N; i++) {
res.pow_inv_dt_[i] = pow_inv_dt_[i];
}
for (const auto& k : knots_) {
res.knots_.emplace_back(k.template cast<Scalar2>());
}
return res;
}
/// @brief Set start time for spline
///
/// @param[in] start_time_ns start time of the spline in nanoseconds
inline void setStartTimeNs(int64_t start_time_ns) {
start_t_ns_ = start_time_ns;
}
/// @brief Maximum time represented by spline
///
/// @return maximum time represented by spline in nanoseconds
int64_t maxTimeNs() const {
return start_t_ns_ + (knots_.size() - N + 1) * dt_ns_ - 1;
}
/// @brief Minimum time represented by spline
///
/// @return minimum time represented by spline in nanoseconds
int64_t minTimeNs() const { return start_t_ns_; }
/// @brief Gererate random trajectory
///
/// @param[in] n number of knots to generate
/// @param[in] static_init if true the first N knots will be the same
/// resulting in static initial condition
void genRandomTrajectory(int n, bool static_init = false) {
if (static_init) {
VecD rnd = VecD::Random() * 5;
for (int i = 0; i < N; i++) {
knots_.push_back(rnd);
}
for (int i = 0; i < n - N; i++) {
knots_.push_back(VecD::Random() * 5);
}
} else {
for (int i = 0; i < n; i++) {
knots_.push_back(VecD::Random() * 5);
}
}
}
/// @brief Add knot to the end of the spline
///
/// @param[in] knot knot to add
inline void knotsPushBack(const VecD& knot) { knots_.push_back(knot); }
/// @brief Remove knot from the back of the spline
inline void knotsPopBack() { knots_.pop_back(); }
/// @brief Return the first knot of the spline
///
/// @return first knot of the spline
inline const VecD& knotsFront() const { return knots_.front(); }
/// @brief Remove first knot of the spline and increase the start time
inline void knotsPopFront() {
start_t_ns_ += dt_ns_;
knots_.pop_front();
}
/// @brief Resize containter with knots
///
/// @param[in] n number of knots
inline void resize(size_t n) { knots_.resize(n); }
/// @brief Return reference to the knot with index i
///
/// @param i index of the knot
/// @return reference to the knot
inline VecD& getKnot(int i) { return knots_[i]; }
/// @brief Return const reference to the knot with index i
///
/// @param i index of the knot
/// @return const reference to the knot
inline const VecD& getKnot(int i) const { return knots_[i]; }
/// @brief Return const reference to deque with knots
///
/// @return const reference to deque with knots
const Eigen::aligned_deque<VecD>& getKnots() const { return knots_; }
/// @brief Return time interval in nanoseconds
///
/// @return time interval in nanoseconds
int64_t getTimeIntervalNs() const { return dt_ns_; }
/// @brief Evaluate value or derivative of the spline
///
/// @param Derivative derivative to evaluate (0 for value)
/// @param[in] time_ns time for evaluating of the spline in nanoseconds
/// @param[out] J if not nullptr, return the Jacobian of the value with
/// respect to knots
/// @return value of the spline or derivative. Euclidean vector of dimention
/// DIM.
template <int Derivative = 0>
VecD evaluate(int64_t time_ns, JacobianStruct* J = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<Derivative>(p, u);
VecN coeff = pow_inv_dt_[Derivative] * (BLENDING_MATRIX * p);
// std::cerr << "p " << p.transpose() << std::endl;
// std::cerr << "coeff " << coeff.transpose() << std::endl;
VecD res;
res.setZero();
for (int i = 0; i < N; i++) {
res += coeff[i] * knots_[s + i];
if (J) {
J->d_val_d_knot[i] = coeff[i];
}
}
if (J) {
J->start_idx = s;
}
return res;
}
/// @brief Alias for first derivative of spline. See \ref evaluate.
inline VecD velocity(int64_t time_ns, JacobianStruct* J = nullptr) const {
return evaluate<1>(time_ns, J);
}
/// @brief Alias for second derivative of spline. See \ref evaluate.
inline VecD acceleration(int64_t time_ns, JacobianStruct* J = nullptr) const {
return evaluate<2>(time_ns, J);
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
/// @brief Vector of derivatives of time polynomial.
///
/// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots &
/// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first
/// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1)
/// t^{N-2}\end{bmatrix} \f$.
/// @param Derivative derivative to evaluate
/// @param[out] res_const vector to store the result
/// @param[in] t
template <int Derivative, class Derived>
static void baseCoeffsWithTime(const Eigen::MatrixBase<Derived>& res_const,
_Scalar t) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N);
Eigen::MatrixBase<Derived>& res =
const_cast<Eigen::MatrixBase<Derived>&>(res_const);
res.setZero();
if (Derivative < N) {
res[Derivative] = BASE_COEFFICIENTS(Derivative, Derivative);
_Scalar ti = t;
for (int j = Derivative + 1; j < N; j++) {
res[j] = BASE_COEFFICIENTS(Derivative, j) * ti;
ti = ti * t;
}
}
}
template <int, int, typename>
friend class RdSpline;
static const MatN
BLENDING_MATRIX; ///< Blending matrix. See \ref computeBlendingMatrix.
static const MatN BASE_COEFFICIENTS; ///< Base coefficients matrix.
///< See \ref computeBaseCoefficients.
Eigen::aligned_deque<VecD> knots_; ///< Knots
int64_t dt_ns_{0}; ///< Knot interval in nanoseconds
int64_t start_t_ns_{0}; ///< Start time in nanoseconds
std::array<_Scalar, _N> pow_inv_dt_; ///< Array with inverse powers of dt
};
template <int _DIM, int _N, typename _Scalar>
const typename RdSpline<_DIM, _N, _Scalar>::MatN
RdSpline<_DIM, _N, _Scalar>::BASE_COEFFICIENTS =
computeBaseCoefficients<_N, _Scalar>();
template <int _DIM, int _N, typename _Scalar>
const typename RdSpline<_DIM, _N, _Scalar>::MatN
RdSpline<_DIM, _N, _Scalar>::BLENDING_MATRIX =
computeBlendingMatrix<_N, _Scalar, false>();
} // namespace basalt

552
thirdparty/basalt-headers/include/basalt/spline/se3_spline.h vendored Normal file
View File

@@ -0,0 +1,552 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Uniform B-spline for SE(3)
*/
#pragma once
#include <basalt/spline/rd_spline.h>
#include <basalt/spline/so3_spline.h>
#include <basalt/utils/assert.h>
#include <basalt/calibration/calib_bias.hpp>
#include <array>
namespace basalt {
/// @brief Uniform B-spline for SE(3) of order N. Internally uses an SO(3) (\ref
/// So3Spline) spline for rotation and 3D Euclidean spline (\ref RdSpline) for
/// translation (split representaion).
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _N, typename _Scalar = double>
class Se3Spline {
public:
static constexpr int N = _N; ///< Order of the spline.
static constexpr int DEG = _N - 1; ///< Degree of the spline.
using MatN = Eigen::Matrix<_Scalar, _N, _N>;
using VecN = Eigen::Matrix<_Scalar, _N, 1>;
using VecNp1 = Eigen::Matrix<_Scalar, _N + 1, 1>;
using Vec3 = Eigen::Matrix<_Scalar, 3, 1>;
using Vec6 = Eigen::Matrix<_Scalar, 6, 1>;
using Vec9 = Eigen::Matrix<_Scalar, 9, 1>;
using Vec12 = Eigen::Matrix<_Scalar, 12, 1>;
using Mat3 = Eigen::Matrix<_Scalar, 3, 3>;
using Mat6 = Eigen::Matrix<_Scalar, 6, 6>;
using Mat36 = Eigen::Matrix<_Scalar, 3, 6>;
using Mat39 = Eigen::Matrix<_Scalar, 3, 9>;
using Mat312 = Eigen::Matrix<_Scalar, 3, 12>;
using Matrix3Array = std::array<Mat3, N>;
using Matrix36Array = std::array<Mat36, N>;
using Matrix6Array = std::array<Mat6, N>;
using SO3 = Sophus::SO3<_Scalar>;
using SE3 = Sophus::SE3<_Scalar>;
using PosJacobianStruct = typename RdSpline<3, N, _Scalar>::JacobianStruct;
using SO3JacobianStruct = typename So3Spline<N, _Scalar>::JacobianStruct;
/// @brief Struct to store the accelerometer residual Jacobian with
/// respect to knots
struct AccelPosSO3JacobianStruct {
size_t start_idx;
std::array<Mat36, N> d_val_d_knot;
};
/// @brief Struct to store the pose Jacobian with respect to knots
struct PosePosSO3JacobianStruct {
size_t start_idx;
std::array<Mat6, N> d_val_d_knot;
};
/// @brief Constructor with knot interval and start time
///
/// @param[in] time_interval_ns knot time interval in nanoseconds
/// @param[in] start_time_ns start time of the spline in nanoseconds
Se3Spline(int64_t time_interval_ns, int64_t start_time_ns = 0)
: pos_spline_(time_interval_ns, start_time_ns),
so3_spline_(time_interval_ns, start_time_ns),
dt_ns_(time_interval_ns) {}
/// @brief Gererate random trajectory
///
/// @param[in] n number of knots to generate
/// @param[in] static_init if true the first N knots will be the same
/// resulting in static initial condition
void genRandomTrajectory(int n, bool static_init = false) {
so3_spline_.genRandomTrajectory(n, static_init);
pos_spline_.genRandomTrajectory(n, static_init);
}
/// @brief Set the knot to particular SE(3) pose
///
/// @param[in] pose SE(3) pose
/// @param[in] i index of the knot
void setKnot(const Sophus::SE3d &pose, int i) {
so3_spline_.getKnot(i) = pose.so3();
pos_spline_.getKnot(i) = pose.translation();
}
/// @brief Reset spline to have num_knots initialized at pose
///
/// @param[in] pose SE(3) pose
/// @param[in] num_knots number of knots to initialize
void setKnots(const Sophus::SE3d &pose, int num_knots) {
so3_spline_.resize(num_knots);
pos_spline_.resize(num_knots);
for (int i = 0; i < num_knots; i++) {
so3_spline_.getKnot(i) = pose.so3();
pos_spline_.getKnot(i) = pose.translation();
}
}
/// @brief Reset spline to the knots from other spline
///
/// @param[in] other spline to copy knots from
void setKnots(const Se3Spline<N, _Scalar> &other) {
BASALT_ASSERT(other.dt_ns_ == dt_ns_);
BASALT_ASSERT(other.pos_spline_.getKnots().size() ==
other.pos_spline_.getKnots().size());
size_t num_knots = other.pos_spline_.getKnots().size();
so3_spline_.resize(num_knots);
pos_spline_.resize(num_knots);
for (size_t i = 0; i < num_knots; i++) {
so3_spline_.getKnot(i) = other.so3_spline_.getKnot(i);
pos_spline_.getKnot(i) = other.pos_spline_.getKnot(i);
}
}
/// @brief Add knot to the end of the spline
///
/// @param[in] knot knot to add
inline void knotsPushBack(const SE3 &knot) {
so3_spline_.knotsPushBack(knot.so3());
pos_spline_.knotsPushBack(knot.translation());
}
/// @brief Remove knot from the back of the spline
inline void knotsPopBack() {
so3_spline_.knotsPopBack();
pos_spline_.knotsPopBack();
}
/// @brief Return the first knot of the spline
///
/// @return first knot of the spline
inline SE3 knotsFront() const {
SE3 res(so3_spline_.knots_front(), pos_spline_.knots_front());
return res;
}
/// @brief Remove first knot of the spline and increase the start time
inline void knotsPopFront() {
so3_spline_.knots_pop_front();
pos_spline_.knots_pop_front();
BASALT_ASSERT(so3_spline_.minTimeNs() == pos_spline_.minTimeNs());
BASALT_ASSERT(so3_spline_.getKnots().size() ==
pos_spline_.getKnots().size());
}
/// @brief Return the last knot of the spline
///
/// @return last knot of the spline
SE3 getLastKnot() {
BASALT_ASSERT(so3_spline_.getKnots().size() ==
pos_spline_.getKnots().size());
SE3 res(so3_spline_.getKnots().back(), pos_spline_.getKnots().back());
return res;
}
/// @brief Return knot with index i
///
/// @param i index of the knot
/// @return knot
SE3 getKnot(size_t i) const {
SE3 res(getKnotSO3(i), getKnotPos(i));
return res;
}
/// @brief Return reference to the SO(3) knot with index i
///
/// @param i index of the knot
/// @return reference to the SO(3) knot
inline SO3 &getKnotSO3(size_t i) { return so3_spline_.getKnot(i); }
/// @brief Return const reference to the SO(3) knot with index i
///
/// @param i index of the knot
/// @return const reference to the SO(3) knot
inline const SO3 &getKnotSO3(size_t i) const {
return so3_spline_.getKnot(i);
}
/// @brief Return reference to the position knot with index i
///
/// @param i index of the knot
/// @return reference to the position knot
inline Vec3 &getKnotPos(size_t i) { return pos_spline_.getKnot(i); }
/// @brief Return const reference to the position knot with index i
///
/// @param i index of the knot
/// @return const reference to the position knot
inline const Vec3 &getKnotPos(size_t i) const {
return pos_spline_.getKnot(i);
}
/// @brief Set start time for spline
///
/// @param[in] start_time_ns start time of the spline in nanoseconds
inline void setStartTimeNs(int64_t s) {
so3_spline_.setStartTimeNs(s);
pos_spline_.setStartTimeNs(s);
}
/// @brief Apply increment to the knot
///
/// The incremernt vector consists of translational and rotational parts \f$
/// [\upsilon, \omega]^T \f$. Given the current pose of the knot \f$ R \in
/// SO(3), p \in \mathbb{R}^3\f$ the updated pose is: \f{align}{ R' &=
/// \exp(\omega) R
/// \\ p' &= p + \upsilon
/// \f}
/// The increment is consistent with \ref
/// PoseState::applyInc.
///
/// @param[in] i index of the knot
/// @param[in] inc 6x1 increment vector
template <typename Derived>
void applyInc(int i, const Eigen::MatrixBase<Derived> &inc) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 6);
pos_spline_.getKnot(i) += inc.template head<3>();
so3_spline_.getKnot(i) =
SO3::exp(inc.template tail<3>()) * so3_spline_.getKnot(i);
}
/// @brief Maximum time represented by spline
///
/// @return maximum time represented by spline in nanoseconds
int64_t maxTimeNs() const {
BASALT_ASSERT_STREAM(so3_spline_.maxTimeNs() == pos_spline_.maxTimeNs(),
"so3_spline.maxTimeNs() " << so3_spline_.maxTimeNs()
<< " pos_spline.maxTimeNs() "
<< pos_spline_.maxTimeNs());
return pos_spline_.maxTimeNs();
}
/// @brief Minimum time represented by spline
///
/// @return minimum time represented by spline in nanoseconds
int64_t minTimeNs() const {
BASALT_ASSERT_STREAM(so3_spline_.minTimeNs() == pos_spline_.minTimeNs(),
"so3_spline.minTimeNs() " << so3_spline_.minTimeNs()
<< " pos_spline.minTimeNs() "
<< pos_spline_.minTimeNs());
return pos_spline_.minTimeNs();
}
/// @brief Number of knots in the spline
size_t numKnots() const { return pos_spline_.getKnots().size(); }
/// @brief Linear acceleration in the world frame.
///
/// @param[in] time_ns time to evaluate linear acceleration in nanoseconds
inline Vec3 transAccelWorld(int64_t time_ns) const {
return pos_spline_.acceleration(time_ns);
}
/// @brief Linear velocity in the world frame.
///
/// @param[in] time_ns time to evaluate linear velocity in nanoseconds
inline Vec3 transVelWorld(int64_t time_ns) const {
return pos_spline_.velocity(time_ns);
}
/// @brief Rotational velocity in the body frame.
///
/// @param[in] time_ns time to evaluate rotational velocity in nanoseconds
inline Vec3 rotVelBody(int64_t time_ns) const {
return so3_spline_.velocityBody(time_ns);
}
/// @brief Evaluate pose.
///
/// @param[in] time_ns time to evaluate pose in nanoseconds
/// @return SE(3) pose at time_ns
SE3 pose(int64_t time_ns) const {
SE3 res;
res.so3() = so3_spline_.evaluate(time_ns);
res.translation() = pos_spline_.evaluate(time_ns);
return res;
}
/// @brief Evaluate pose and compute Jacobian.
///
/// @param[in] time_ns time to evaluate pose in nanoseconds
/// @param[out] J Jacobian of the pose with respect to knots
/// @return SE(3) pose at time_ns
Sophus::SE3d pose(int64_t time_ns, PosePosSO3JacobianStruct *J) const {
Sophus::SE3d res;
typename So3Spline<_N, _Scalar>::JacobianStruct Jr;
typename RdSpline<3, N, _Scalar>::JacobianStruct Jp;
res.so3() = so3_spline_.evaluate(time_ns, &Jr);
res.translation() = pos_spline_.evaluate(time_ns, &Jp);
if (J) {
Eigen::Matrix3d RT = res.so3().inverse().matrix();
J->start_idx = Jr.start_idx;
for (int i = 0; i < N; i++) {
J->d_val_d_knot[i].setZero();
J->d_val_d_knot[i].template topLeftCorner<3, 3>() =
RT * Jp.d_val_d_knot[i];
J->d_val_d_knot[i].template bottomRightCorner<3, 3>() =
RT * Jr.d_val_d_knot[i];
}
}
return res;
}
/// @brief Evaluate pose and compute time Jacobian.
///
/// @param[in] time_ns time to evaluate pose in nanoseconds
/// @param[out] J Jacobian of the pose with time
void d_pose_d_t(int64_t time_ns, Vec6 &J) const {
J.template head<3>() =
so3_spline_.evaluate(time_ns).inverse() * transVelWorld(time_ns);
J.template tail<3>() = rotVelBody(time_ns);
}
/// @brief Evaluate gyroscope residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement gyroscope measurement
/// @param[in] gyro_bias_full gyroscope calibration
/// @return gyroscope residual
Vec3 gyroResidual(int64_t time_ns, const Vec3 &measurement,
const CalibGyroBias<_Scalar> &gyro_bias_full) const {
return so3_spline_.velocityBody(time_ns) -
gyro_bias_full.getCalibrated(measurement);
}
/// @brief Evaluate gyroscope residual and compute Jacobians.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement gyroscope measurement
/// @param[in] gyro_bias_full gyroscope calibration
/// @param[out] J_knots Jacobian with respect to SO(3) spline knots
/// @param[out] J_bias Jacobian with respect to gyroscope calibration
/// @return gyroscope residual
Vec3 gyroResidual(int64_t time_ns, const Vec3 &measurement,
const CalibGyroBias<_Scalar> &gyro_bias_full,
SO3JacobianStruct *J_knots,
Mat312 *J_bias = nullptr) const {
if (J_bias) {
J_bias->setZero();
J_bias->template block<3, 3>(0, 0).diagonal().array() = 1.0;
J_bias->template block<3, 3>(0, 3).diagonal().array() = -measurement[0];
J_bias->template block<3, 3>(0, 6).diagonal().array() = -measurement[1];
J_bias->template block<3, 3>(0, 9).diagonal().array() = -measurement[2];
}
return so3_spline_.velocityBody(time_ns, J_knots) -
gyro_bias_full.getCalibrated(measurement);
}
/// @brief Evaluate accelerometer residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement accelerometer measurement
/// @param[in] accel_bias_full accelerometer calibration
/// @param[in] g gravity
/// @return accelerometer residual
Vec3 accelResidual(int64_t time_ns, const Eigen::Vector3d &measurement,
const CalibAccelBias<_Scalar> &accel_bias_full,
const Eigen::Vector3d &g) const {
Sophus::SO3d R = so3_spline_.evaluate(time_ns);
Eigen::Vector3d accel_world = pos_spline_.acceleration(time_ns);
return R.inverse() * (accel_world + g) -
accel_bias_full.getCalibrated(measurement);
}
/// @brief Evaluate accelerometer residual and Jacobians.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measurement accelerometer measurement
/// @param[in] accel_bias_full accelerometer calibration
/// @param[in] g gravity
/// @param[out] J_knots Jacobian with respect to spline knots
/// @param[out] J_bias Jacobian with respect to accelerometer calibration
/// @param[out] J_g Jacobian with respect to gravity
/// @return accelerometer residual
Vec3 accelResidual(int64_t time_ns, const Vec3 &measurement,
const CalibAccelBias<_Scalar> &accel_bias_full,
const Vec3 &g, AccelPosSO3JacobianStruct *J_knots,
Mat39 *J_bias = nullptr, Mat3 *J_g = nullptr) const {
typename So3Spline<_N, _Scalar>::JacobianStruct Jr;
typename RdSpline<3, N, _Scalar>::JacobianStruct Jp;
Sophus::SO3d R = so3_spline_.evaluate(time_ns, &Jr);
Eigen::Vector3d accel_world = pos_spline_.acceleration(time_ns, &Jp);
Eigen::Matrix3d RT = R.inverse().matrix();
Eigen::Matrix3d tmp = RT * Sophus::SO3d::hat(accel_world + g);
BASALT_ASSERT_STREAM(
Jr.start_idx == Jp.start_idx,
"Jr.start_idx " << Jr.start_idx << " Jp.start_idx " << Jp.start_idx);
BASALT_ASSERT_STREAM(
so3_spline_.getKnots().size() == pos_spline_.getKnots().size(),
"so3_spline.getKnots().size() " << so3_spline_.getKnots().size()
<< " pos_spline.getKnots().size() "
<< pos_spline_.getKnots().size());
J_knots->start_idx = Jp.start_idx;
for (int i = 0; i < N; i++) {
J_knots->d_val_d_knot[i].template topLeftCorner<3, 3>() =
RT * Jp.d_val_d_knot[i];
J_knots->d_val_d_knot[i].template bottomRightCorner<3, 3>() =
tmp * Jr.d_val_d_knot[i];
}
if (J_bias) {
J_bias->setZero();
J_bias->template block<3, 3>(0, 0).diagonal().array() = 1.0;
J_bias->template block<3, 3>(0, 3).diagonal().array() = -measurement[0];
(*J_bias)(1, 6) = -measurement[1];
(*J_bias)(2, 7) = -measurement[1];
(*J_bias)(2, 8) = -measurement[2];
}
if (J_g) {
(*J_g) = RT;
}
Vec3 res =
RT * (accel_world + g) - accel_bias_full.getCalibrated(measurement);
return res;
}
/// @brief Evaluate position residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measured_position position measurement
/// @param[out] Jp if not nullptr, Jacobian with respect to knos of the
/// position spline
/// @return position residual
Sophus::Vector3d positionResidual(int64_t time_ns,
const Vec3 &measured_position,
PosJacobianStruct *Jp = nullptr) const {
return pos_spline_.evaluate(time_ns, Jp) - measured_position;
}
/// @brief Evaluate orientation residual.
///
/// @param[in] time_ns time of the measurement
/// @param[in] measured_orientation orientation measurement
/// @param[out] Jr if not nullptr, Jacobian with respect to knos of the
/// SO(3) spline
/// @return orientation residual
Sophus::Vector3d orientationResidual(int64_t time_ns,
const SO3 &measured_orientation,
SO3JacobianStruct *Jr = nullptr) const {
Sophus::Vector3d res =
(so3_spline_.evaluate(time_ns, Jr) * measured_orientation.inverse())
.log();
if (Jr) {
Eigen::Matrix3d Jrot;
Sophus::leftJacobianSO3(res, Jrot);
for (int i = 0; i < N; i++) {
Jr->d_val_d_knot[i] = Jrot * Jr->d_val_d_knot[i];
}
}
return res;
}
/// @brief Print knots for debugging.
inline void printKnots() const {
for (size_t i = 0; i < pos_spline_.getKnots().size(); i++) {
std::cout << i << ": p:" << pos_spline_.getKnot(i).transpose() << " q: "
<< so3_spline_.getKnot(i).unit_quaternion().coeffs().transpose()
<< std::endl;
}
}
/// @brief Print position knots for debugging.
inline void printPosKnots() const {
for (size_t i = 0; i < pos_spline_.getKnots().size(); i++) {
std::cout << pos_spline_.getKnot(i).transpose() << std::endl;
}
}
/// @brief Knot time interval in nanoseconds.
inline int64_t getDtNs() const { return dt_ns_; }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
RdSpline<3, _N, _Scalar> pos_spline_; ///< Position spline
So3Spline<_N, _Scalar> so3_spline_; ///< Orientation spline
int64_t dt_ns_; ///< Knot interval in nanoseconds
};
} // namespace basalt

790
thirdparty/basalt-headers/include/basalt/spline/so3_spline.h vendored Normal file
View File

@@ -0,0 +1,790 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Uniform cumulative B-spline for SO(3)
*/
#pragma once
#include <basalt/spline/spline_common.h>
#include <basalt/utils/assert.h>
#include <basalt/utils/sophus_utils.hpp>
#include <Eigen/Dense>
#include <sophus/so3.hpp>
#include <array>
namespace basalt {
/// @brief Uniform cummulative B-spline for SO(3) of order N
///
/// For example, in the particular case scalar values and order N=5, for a time
/// \f$t \in [t_i, t_{i+1})\f$ the value of \f$p(t)\f$ depends only on 5 control
/// points at \f$[t_i, t_{i+1}, t_{i+2}, t_{i+3}, t_{i+4}]\f$. To
/// simplify calculations we transform time to uniform representation \f$s(t) =
/// (t - t_0)/\Delta t \f$, such that control points transform into \f$ s_i \in
/// [0,..,N] \f$. We define function \f$ u(t) = s(t)-s_i \f$ to be a time since
/// the start of the segment. Following the cummulative matrix representation of
/// De Boor - Cox formula, the value of the function can be evaluated as
/// follows: \f{align}{
/// R(u(t)) &= R_i
/// \prod_{j=1}^{4}{\exp(k_{j}\log{(R_{i+j-1}^{-1}R_{i+j})})},
/// \\ \begin{pmatrix} k_0 \\ k_1 \\ k_2 \\ k_3 \\ k_4 \end{pmatrix}^T &=
/// M_{c5} \begin{pmatrix} 1 \\ u \\ u^2 \\ u^3 \\ u^4
/// \end{pmatrix},
/// \f}
/// where \f$ R_{i} \in SO(3) \f$ are knots and \f$ M_{c5} \f$ is a cummulative
/// blending matrix computed using \ref computeBlendingMatrix \f{align}{
/// M_{c5} = \frac{1}{4!}
/// \begin{pmatrix} 24 & 0 & 0 & 0 & 0 \\ 23 & 4 & -6 & 4 & -1 \\ 12 & 16 & 0
/// & -8 & 3 \\ 1 & 4 & 6 & 4 & -3 \\ 0 & 0 & 0 & 0 & 1 \end{pmatrix}.
/// \f}
///
/// See [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860) for more details.
template <int _N, typename _Scalar = double>
class So3Spline {
public:
static constexpr int N = _N; ///< Order of the spline.
static constexpr int DEG = _N - 1; ///< Degree of the spline.
static constexpr _Scalar NS_TO_S = 1e-9; ///< Nanosecond to second conversion
static constexpr _Scalar S_TO_NS = 1e9; ///< Second to nanosecond conversion
using MatN = Eigen::Matrix<_Scalar, _N, _N>;
using VecN = Eigen::Matrix<_Scalar, _N, 1>;
using Vec3 = Eigen::Matrix<_Scalar, 3, 1>;
using Mat3 = Eigen::Matrix<_Scalar, 3, 3>;
using SO3 = Sophus::SO3<_Scalar>;
/// @brief Struct to store the Jacobian of the spline
///
/// Since B-spline of order N has local support (only N knots infuence the
/// value) the Jacobian is zero for all knots except maximum N for value and
/// all derivatives.
struct JacobianStruct {
size_t start_idx;
std::array<Mat3, _N> d_val_d_knot;
};
/// @brief Constructor with knot interval and start time
///
/// @param[in] time_interval_ns knot time interval in nanoseconds
/// @param[in] start_time_ns start time of the spline in nanoseconds
So3Spline(int64_t time_interval_ns, int64_t start_time_ns = 0)
: dt_ns_(time_interval_ns), start_t_ns_(start_time_ns) {
pow_inv_dt_[0] = 1.0;
pow_inv_dt_[1] = S_TO_NS / dt_ns_;
pow_inv_dt_[2] = pow_inv_dt_[1] * pow_inv_dt_[1];
pow_inv_dt_[3] = pow_inv_dt_[2] * pow_inv_dt_[1];
}
/// @brief Maximum time represented by spline
///
/// @return maximum time represented by spline in nanoseconds
int64_t maxTimeNs() const {
return start_t_ns_ + (knots_.size() - N + 1) * dt_ns_ - 1;
}
/// @brief Minimum time represented by spline
///
/// @return minimum time represented by spline in nanoseconds
int64_t minTimeNs() const { return start_t_ns_; }
/// @brief Gererate random trajectory
///
/// @param[in] n number of knots to generate
/// @param[in] static_init if true the first N knots will be the same
/// resulting in static initial condition
void genRandomTrajectory(int n, bool static_init = false) {
if (static_init) {
Vec3 rnd = Vec3::Random() * M_PI;
for (int i = 0; i < N; i++) {
knots_.push_back(SO3::exp(rnd));
}
for (int i = 0; i < n - N; i++) {
knots_.push_back(knots_.back() * SO3::exp(Vec3::Random() * M_PI / 2));
}
} else {
knots_.push_back(SO3::exp(Vec3::Random() * M_PI));
for (int i = 1; i < n; i++) {
knots_.push_back(knots_.back() * SO3::exp(Vec3::Random() * M_PI / 2));
}
}
}
/// @brief Set start time for spline
///
/// @param[in] start_time_ns start time of the spline in nanoseconds
inline void setStartTimeNs(int64_t s) { start_t_ns_ = s; }
/// @brief Add knot to the end of the spline
///
/// @param[in] knot knot to add
inline void knotsPushBack(const SO3& knot) { knots_.push_back(knot); }
/// @brief Remove knot from the back of the spline
inline void knotsPopBack() { knots_.pop_back(); }
/// @brief Return the first knot of the spline
///
/// @return first knot of the spline
inline const SO3& knotsFront() const { return knots_.front(); }
/// @brief Remove first knot of the spline and increase the start time
inline void knotsPopFront() {
start_t_ns_ += dt_ns_;
knots_.pop_front();
}
/// @brief Resize containter with knots
///
/// @param[in] n number of knots
inline void resize(size_t n) { knots_.resize(n); }
/// @brief Return reference to the knot with index i
///
/// @param i index of the knot
/// @return reference to the knot
inline SO3& getKnot(int i) { return knots_[i]; }
/// @brief Return const reference to the knot with index i
///
/// @param i index of the knot
/// @return const reference to the knot
inline const SO3& getKnot(int i) const { return knots_[i]; }
/// @brief Return const reference to deque with knots
///
/// @return const reference to deque with knots
const Eigen::aligned_deque<SO3>& getKnots() const { return knots_; }
/// @brief Return time interval in nanoseconds
///
/// @return time interval in nanoseconds
int64_t getTimeIntervalNs() const { return dt_ns_; }
/// @brief Evaluate SO(3) B-spline
///
/// @param[in] time_ns time for evaluating the value of the spline in
/// nanoseconds
/// @param[out] J if not nullptr, return the Jacobian of the value with
/// respect to knots
/// @return SO(3) value of the spline
SO3 evaluate(int64_t time_ns, JacobianStruct* J = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
SO3 res = knots_[s];
Mat3 J_helper;
if (J) {
J->start_idx = s;
J_helper.setIdentity();
}
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
Vec3 kdelta = delta * coeff[i + 1];
if (J) {
Mat3 Jl_inv_delta;
Mat3 Jl_k_delta;
Sophus::leftJacobianInvSO3(delta, Jl_inv_delta);
Sophus::leftJacobianSO3(kdelta, Jl_k_delta);
J->d_val_d_knot[i] = J_helper;
J_helper = coeff[i + 1] * res.matrix() * Jl_k_delta * Jl_inv_delta *
p0.inverse().matrix();
J->d_val_d_knot[i] -= J_helper;
}
res *= SO3::exp(kdelta);
}
if (J) {
J->d_val_d_knot[DEG] = J_helper;
}
return res;
}
/// @brief Evaluate rotational velocity (first time derivative) of SO(3)
/// B-spline in the body frame
/// First, let's note that for scalars \f$ k, \Delta k \f$ the following
/// holds: \f$ \exp((k+\Delta k)\phi) = \exp(k\phi)\exp(\Delta k\phi), \phi
/// \in \mathbb{R}^3\f$. This is due to the fact that rotations around the
/// same axis are commutative.
///
/// Let's take SO(3) B-spline with N=3 as an example. The evolution in time of
/// rotation from the body frame to the world frame is described with \f[
/// R_{wb}(t) = R(t) = R_i \exp(k_1(t) \log(R_{i}^{-1}R_{i+1})) \exp(k_2(t)
/// \log(R_{i+1}^{-1}R_{i+2})), \f] where \f$ k_1, k_2 \f$ are spline
/// coefficients (see detailed description of \ref So3Spline). Since
/// expressions under logmap do not depend on time we can rename them to
/// constants.
/// \f[ R(t) = R_i \exp(k_1(t) ~ d_1) \exp(k_2(t) ~ d_2). \f]
///
/// With linear approximation of the spline coefficient evolution over time
/// \f$ k_1(t_0 + \Delta t) = k_1(t_0) + k_1'(t_0)\Delta t \f$ we can write
/// \f{align}
/// R(t_0 + \Delta t) &= R_i \exp( (k_1(t_0) + k_1'(t_0) \Delta t) ~ d_1)
/// \exp((k_2(t_0) + k_2'(t_0) \Delta t) ~ d_2)
/// \\ &= R_i \exp(k_1(t_0) ~ d_1) \exp(k_1'(t_0)~ d_1 \Delta t )
/// \exp(k_2(t_0) ~ d_2) \exp(k_2'(t_0) ~ d_2 \Delta t )
/// \\ &= R_i \exp(k_1(t_0) ~ d_1)
/// \exp(k_2(t_0) ~ d_2) \exp(R_{a}^T k_1'(t_0)~ d_1 \Delta t )
/// \exp(k_2'(t_0) ~ d_2 \Delta t )
/// \\ &= R_i \exp(k_1(t_0) ~ d_1)
/// \exp(k_2(t_0) ~ d_2) \exp((R_{a}^T k_1'(t_0)~ d_1 +
/// k_2'(t_0) ~ d_2) \Delta t )
/// \\ &= R(t_0) \exp((R_{a}^T k_1'(t_0)~ d_1 +
/// k_2'(t_0) ~ d_2) \Delta t )
/// \\ &= R(t_0) \exp( \omega \Delta t ),
/// \f} where \f$ \Delta t \f$ is small, \f$ R_{a} \in SO(3) = \exp(k_2(t_0) ~
/// d_2) \f$ and \f$ \omega \f$ is the rotational velocity in the body frame.
/// More explicitly we have the formula for rotational velocity in the body
/// frame \f[ \omega = R_{a}^T k_1'(t_0)~ d_1 + k_2'(t_0) ~ d_2. \f]
/// Derivatives of spline coefficients can be computed with \ref
/// baseCoeffsWithTime similar to \ref RdSpline (detailed description). With
/// the recursive formula computations generalize to different orders of
/// spline N.
///
/// @param[in] time_ns time for evaluating velocity of the spline in
/// nanoseconds
/// @return rotational velocity (3x1 vector)
Vec3 velocityBody(int64_t time_ns) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
Vec3 rot_vel;
rot_vel.setZero();
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
rot_vel = SO3::exp(-delta * coeff[i + 1]) * rot_vel;
rot_vel += delta * dcoeff[i + 1];
}
return rot_vel;
}
/// @brief Evaluate rotational velocity (first time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating velocity of the spline in
/// nanoseconds
/// @param[out] J if not nullptr, return the Jacobian of the rotational
/// velocity in body frame with respect to knots
/// @return rotational velocity (3x1 vector)
Vec3 velocityBody(int64_t time_ns, JacobianStruct* J) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
Vec3 delta_vec[DEG];
Mat3 R_tmp[DEG];
SO3 accum;
SO3 exp_k_delta[DEG];
Mat3 Jr_delta_inv[DEG];
Mat3 Jr_kdelta[DEG];
for (int i = DEG - 1; i >= 0; i--) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
delta_vec[i] = r01.log();
Sophus::rightJacobianInvSO3(delta_vec[i], Jr_delta_inv[i]);
Jr_delta_inv[i] *= p1.inverse().matrix();
Vec3 k_delta = coeff[i + 1] * delta_vec[i];
Sophus::rightJacobianSO3(-k_delta, Jr_kdelta[i]);
R_tmp[i] = accum.matrix();
exp_k_delta[i] = Sophus::SO3d::exp(-k_delta);
accum *= exp_k_delta[i];
}
Mat3 d_vel_d_delta[DEG];
d_vel_d_delta[0] = dcoeff[1] * R_tmp[0] * Jr_delta_inv[0];
Vec3 rot_vel = delta_vec[0] * dcoeff[1];
for (int i = 1; i < DEG; i++) {
d_vel_d_delta[i] =
R_tmp[i - 1] * SO3::hat(rot_vel) * Jr_kdelta[i] * coeff[i + 1] +
R_tmp[i] * dcoeff[i + 1];
d_vel_d_delta[i] *= Jr_delta_inv[i];
rot_vel = exp_k_delta[i] * rot_vel + delta_vec[i] * dcoeff[i + 1];
}
if (J) {
J->start_idx = s;
for (int i = 0; i < N; i++) {
J->d_val_d_knot[i].setZero();
}
for (int i = 0; i < DEG; i++) {
J->d_val_d_knot[i] -= d_vel_d_delta[i];
J->d_val_d_knot[i + 1] += d_vel_d_delta[i];
}
}
return rot_vel;
}
/// @brief Evaluate rotational acceleration (second time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating acceleration of the spline in
/// nanoseconds
/// @param[out] vel_body if not nullptr, return the rotational velocity in the
/// body frame (3x1 vector) (side computation)
/// @return rotational acceleration (3x1 vector)
Vec3 accelerationBody(int64_t time_ns, Vec3* vel_body = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
baseCoeffsWithTime<2>(p, u);
VecN ddcoeff = pow_inv_dt_[2] * BLENDING_MATRIX * p;
SO3 r_accum;
Vec3 rot_vel;
rot_vel.setZero();
Vec3 rot_accel;
rot_accel.setZero();
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
SO3 rot = SO3::exp(-delta * coeff[i + 1]);
rot_vel = rot * rot_vel;
Vec3 vel_current = dcoeff[i + 1] * delta;
rot_vel += vel_current;
rot_accel = rot * rot_accel;
rot_accel += ddcoeff[i + 1] * delta + rot_vel.cross(vel_current);
}
if (vel_body) {
*vel_body = rot_vel;
}
return rot_accel;
}
/// @brief Evaluate rotational acceleration (second time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating acceleration of the spline in
/// nanoseconds
/// @param[out] J_accel if not nullptr, return the Jacobian of the rotational
/// acceleration in body frame with respect to knots
/// @param[out] vel_body if not nullptr, return the rotational velocity in the
/// body frame (3x1 vector) (side computation)
/// @param[out] J_vel if not nullptr, return the Jacobian of the rotational
/// velocity in the body frame (side computation)
/// @return rotational acceleration (3x1 vector)
Vec3 accelerationBody(int64_t time_ns, JacobianStruct* J_accel,
Vec3* vel_body = nullptr,
JacobianStruct* J_vel = nullptr) const {
BASALT_ASSERT(J_accel);
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
baseCoeffsWithTime<2>(p, u);
VecN ddcoeff = pow_inv_dt_[2] * BLENDING_MATRIX * p;
Vec3 delta_vec[DEG];
Mat3 exp_k_delta[DEG];
Mat3 Jr_delta_inv[DEG];
Mat3 Jr_kdelta[DEG];
Vec3 rot_vel;
rot_vel.setZero();
Vec3 rot_accel;
rot_accel.setZero();
Vec3 rot_vel_arr[DEG];
Vec3 rot_accel_arr[DEG];
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
delta_vec[i] = r01.log();
Sophus::rightJacobianInvSO3(delta_vec[i], Jr_delta_inv[i]);
Jr_delta_inv[i] *= p1.inverse().matrix();
Vec3 k_delta = coeff[i + 1] * delta_vec[i];
Sophus::rightJacobianSO3(-k_delta, Jr_kdelta[i]);
exp_k_delta[i] = Sophus::SO3d::exp(-k_delta).matrix();
rot_vel = exp_k_delta[i] * rot_vel;
Vec3 vel_current = dcoeff[i + 1] * delta_vec[i];
rot_vel += vel_current;
rot_accel = exp_k_delta[i] * rot_accel;
rot_accel += ddcoeff[i + 1] * delta_vec[i] + rot_vel.cross(vel_current);
rot_vel_arr[i] = rot_vel;
rot_accel_arr[i] = rot_accel;
}
Mat3 d_accel_d_delta[DEG];
Mat3 d_vel_d_delta[DEG];
d_vel_d_delta[DEG - 1] = coeff[DEG] * exp_k_delta[DEG - 1] *
SO3::hat(rot_vel_arr[DEG - 2]) *
Jr_kdelta[DEG - 1] +
Mat3::Identity() * dcoeff[DEG];
d_accel_d_delta[DEG - 1] =
coeff[DEG] * exp_k_delta[DEG - 1] * SO3::hat(rot_accel_arr[DEG - 2]) *
Jr_kdelta[DEG - 1] +
Mat3::Identity() * ddcoeff[DEG] +
dcoeff[DEG] * (SO3::hat(rot_vel_arr[DEG - 1]) -
SO3::hat(delta_vec[DEG - 1]) * d_vel_d_delta[DEG - 1]);
Mat3 pj;
pj.setIdentity();
Vec3 sj;
sj.setZero();
for (int i = DEG - 2; i >= 0; i--) {
sj += dcoeff[i + 2] * pj * delta_vec[i + 1];
pj *= exp_k_delta[i + 1];
d_vel_d_delta[i] = Mat3::Identity() * dcoeff[i + 1];
if (i >= 1) {
d_vel_d_delta[i] += coeff[i + 1] * exp_k_delta[i] *
SO3::hat(rot_vel_arr[i - 1]) * Jr_kdelta[i];
}
d_accel_d_delta[i] =
Mat3::Identity() * ddcoeff[i + 1] +
dcoeff[i + 1] * (SO3::hat(rot_vel_arr[i]) -
SO3::hat(delta_vec[i]) * d_vel_d_delta[i]);
if (i >= 1) {
d_accel_d_delta[i] += coeff[i + 1] * exp_k_delta[i] *
SO3::hat(rot_accel_arr[i - 1]) * Jr_kdelta[i];
}
d_vel_d_delta[i] = pj * d_vel_d_delta[i];
d_accel_d_delta[i] =
pj * d_accel_d_delta[i] - SO3::hat(sj) * d_vel_d_delta[i];
}
if (J_vel) {
J_vel->start_idx = s;
for (int i = 0; i < N; i++) {
J_vel->d_val_d_knot[i].setZero();
}
for (int i = 0; i < DEG; i++) {
Mat3 val = d_vel_d_delta[i] * Jr_delta_inv[i];
J_vel->d_val_d_knot[i] -= val;
J_vel->d_val_d_knot[i + 1] += val;
}
}
if (J_accel) {
J_accel->start_idx = s;
for (int i = 0; i < N; i++) {
J_accel->d_val_d_knot[i].setZero();
}
for (int i = 0; i < DEG; i++) {
Mat3 val = d_accel_d_delta[i] * Jr_delta_inv[i];
J_accel->d_val_d_knot[i] -= val;
J_accel->d_val_d_knot[i + 1] += val;
}
}
if (vel_body) {
*vel_body = rot_vel;
}
return rot_accel;
}
/// @brief Evaluate rotational jerk (third time derivative) of SO(3)
/// B-spline in the body frame
///
/// @param[in] time_ns time for evaluating jerk of the spline in
/// nanoseconds
/// @param[out] vel_body if not nullptr, return the rotational velocity in the
/// body frame (3x1 vector) (side computation)
/// @param[out] accel_body if not nullptr, return the rotational acceleration
/// in the body frame (3x1 vector) (side computation)
/// @return rotational jerk (3x1 vector)
Vec3 jerkBody(int64_t time_ns, Vec3* vel_body = nullptr,
Vec3* accel_body = nullptr) const {
int64_t st_ns = (time_ns - start_t_ns_);
BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << time_ns
<< " start_t_ns " << start_t_ns_);
int64_t s = st_ns / dt_ns_;
double u = double(st_ns % dt_ns_) / double(dt_ns_);
BASALT_ASSERT_STREAM(s >= 0, "s " << s);
BASALT_ASSERT_STREAM(
size_t(s + N) <= knots_.size(),
"s " << s << " N " << N << " knots.size() " << knots_.size());
VecN p;
baseCoeffsWithTime<0>(p, u);
VecN coeff = BLENDING_MATRIX * p;
baseCoeffsWithTime<1>(p, u);
VecN dcoeff = pow_inv_dt_[1] * BLENDING_MATRIX * p;
baseCoeffsWithTime<2>(p, u);
VecN ddcoeff = pow_inv_dt_[2] * BLENDING_MATRIX * p;
baseCoeffsWithTime<3>(p, u);
VecN dddcoeff = pow_inv_dt_[3] * BLENDING_MATRIX * p;
Vec3 rot_vel;
rot_vel.setZero();
Vec3 rot_accel;
rot_accel.setZero();
Vec3 rot_jerk;
rot_jerk.setZero();
for (int i = 0; i < DEG; i++) {
const SO3& p0 = knots_[s + i];
const SO3& p1 = knots_[s + i + 1];
SO3 r01 = p0.inverse() * p1;
Vec3 delta = r01.log();
SO3 rot = SO3::exp(-delta * coeff[i + 1]);
rot_vel = rot * rot_vel;
Vec3 vel_current = dcoeff[i + 1] * delta;
rot_vel += vel_current;
rot_accel = rot * rot_accel;
Vec3 rot_vel_cross_vel_current = rot_vel.cross(vel_current);
rot_accel += ddcoeff[i + 1] * delta + rot_vel_cross_vel_current;
rot_jerk = rot * rot_jerk;
rot_jerk += dddcoeff[i + 1] * delta +
(ddcoeff[i + 1] * rot_vel + 2 * dcoeff[i + 1] * rot_accel -
dcoeff[i + 1] * rot_vel_cross_vel_current)
.cross(delta);
}
if (vel_body) {
*vel_body = rot_vel;
}
if (accel_body) {
*accel_body = rot_accel;
}
return rot_jerk;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
protected:
/// @brief Vector of derivatives of time polynomial.
///
/// Computes a derivative of \f$ \begin{bmatrix}1 & t & t^2 & \dots &
/// t^{N-1}\end{bmatrix} \f$ with repect to time. For example, the first
/// derivative would be \f$ \begin{bmatrix}0 & 1 & 2 t & \dots & (N-1)
/// t^{N-2}\end{bmatrix} \f$.
/// @param Derivative derivative to evaluate
/// @param[out] res_const vector to store the result
/// @param[in] t
template <int Derivative, class Derived>
static void baseCoeffsWithTime(const Eigen::MatrixBase<Derived>& res_const,
_Scalar t) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, N);
Eigen::MatrixBase<Derived>& res =
const_cast<Eigen::MatrixBase<Derived>&>(res_const);
res.setZero();
if (Derivative < N) {
res[Derivative] = BASE_COEFFICIENTS(Derivative, Derivative);
_Scalar ti = t;
for (int j = Derivative + 1; j < N; j++) {
res[j] = BASE_COEFFICIENTS(Derivative, j) * ti;
ti = ti * t;
}
}
}
static const MatN
BLENDING_MATRIX; ///< Blending matrix. See \ref computeBlendingMatrix.
static const MatN BASE_COEFFICIENTS; ///< Base coefficients matrix.
///< See \ref computeBaseCoefficients.
Eigen::aligned_deque<SO3> knots_; ///< Knots
int64_t dt_ns_; ///< Knot interval in nanoseconds
int64_t start_t_ns_; ///< Start time in nanoseconds
std::array<_Scalar, 4> pow_inv_dt_; ///< Array with inverse powers of dt
}; // namespace basalt
template <int _N, typename _Scalar>
const typename So3Spline<_N, _Scalar>::MatN
So3Spline<_N, _Scalar>::BASE_COEFFICIENTS =
computeBaseCoefficients<_N, _Scalar>();
template <int _N, typename _Scalar>
const typename So3Spline<_N, _Scalar>::MatN
So3Spline<_N, _Scalar>::BLENDING_MATRIX =
computeBlendingMatrix<_N, _Scalar, true>();
} // namespace basalt

137
thirdparty/basalt-headers/include/basalt/spline/spline_common.h vendored Normal file
View File

@@ -0,0 +1,137 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt-headers.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
@file
@brief Common functions for B-spline evaluation
*/
#pragma once
#include <Eigen/Dense>
#include <cstdint>
namespace basalt {
/// @brief Compute binomial coefficient.
///
/// Computes number of combinations that include k objects out of n.
/// @param[in] n
/// @param[in] k
/// @return binomial coefficient
constexpr inline uint64_t binomialCoefficient(uint64_t n, uint64_t k) {
if (k > n) {
return 0;
}
uint64_t r = 1;
for (uint64_t d = 1; d <= k; ++d) {
r *= n--;
r /= d;
}
return r;
}
/// @brief Compute blending matrix for uniform B-spline evaluation.
///
/// @param _N order of the spline
/// @param _Scalar scalar type to use
/// @param _Cumulative if the spline should be cumulative
template <int _N, typename _Scalar = double, bool _Cumulative = false>
Eigen::Matrix<_Scalar, _N, _N> computeBlendingMatrix() {
Eigen::Matrix<double, _N, _N> m;
m.setZero();
for (int i = 0; i < _N; ++i) {
for (int j = 0; j < _N; ++j) {
double sum = 0;
for (int s = j; s < _N; ++s) {
sum += std::pow(-1.0, s - j) * binomialCoefficient(_N, s - j) *
std::pow(_N - s - 1.0, _N - 1.0 - i);
}
m(j, i) = binomialCoefficient(_N - 1, _N - 1 - i) * sum;
}
}
if (_Cumulative) {
for (int i = 0; i < _N; i++) {
for (int j = i + 1; j < _N; j++) {
m.row(i) += m.row(j);
}
}
}
uint64_t factorial = 1;
for (int i = 2; i < _N; ++i) {
factorial *= i;
}
return (m / factorial).template cast<_Scalar>();
}
/// @brief Compute base coefficient matrix for polynomials of size N.
///
/// In each row starting from 0 contains the derivative coefficients of the
/// polynomial. For _N=5 we get the following matrix: \f[ \begin{bmatrix}
/// 1 & 1 & 1 & 1 & 1
/// \\0 & 1 & 2 & 3 & 4
/// \\0 & 0 & 2 & 6 & 12
/// \\0 & 0 & 0 & 6 & 24
/// \\0 & 0 & 0 & 0 & 24
/// \\ \end{bmatrix}
/// \f]
/// Functions \ref RdSpline::baseCoeffsWithTime and \ref
/// So3Spline::baseCoeffsWithTime use this matrix to compute derivatives of the
/// time polynomial.
///
/// @param _N order of the polynomial
/// @param _Scalar scalar type to use
template <int _N, typename _Scalar = double>
Eigen::Matrix<_Scalar, _N, _N> computeBaseCoefficients() {
Eigen::Matrix<double, _N, _N> base_coefficients;
base_coefficients.setZero();
base_coefficients.row(0).setOnes();
constexpr int DEG = _N - 1;
int order = DEG;
for (int n = 1; n < _N; n++) {
for (int i = DEG - order; i < _N; i++) {
base_coefficients(n, i) = (order - DEG + i) * base_coefficients(n - 1, i);
}
order--;
}
return base_coefficients.template cast<_Scalar>();
}
} // namespace basalt