CumulativeSplineTrajectory.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/basis/IrwinHall.h>
#include <gtsam/basis/KernelBase.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/expressions.h>
 
#include <algorithm>
#include <cmath>
#include <functional>
#include <stdexcept>
#include <utility>
#include <vector>
 
namespace gtsam {

template <class T>
class CumulativeSplineTrajectory {
 public:
  using TangentVector = typename traits<T>::TangentVector;
  using TangentExpression = Expression<TangentVector>;

  explicit CumulativeSplineTrajectory(
      double density = 1.0, const KernelBase& kernel = kernels::IrwinHallCDF2,
      const std::vector<Expression<T>>& points = {}, bool padFront = false)
      : density_(density),
        kernel_(kernel),
        points_(points),
        padFront_(padFront),
        kernelOffset_(padFront ? kernel.getEnd() : kernel.getBeginning()),
        windowPre_(padFront ? 0
                            : static_cast<int>(std::ceil(kernel.getLength()))),
        windowPost_(padFront ? static_cast<int>(std::ceil(kernel.getLength()))
                             : 0) {
    if (density <= 0.0) {
      throw std::invalid_argument(
          "CumulativeSplineTrajectory density must be positive");
    }
  }

  CumulativeSplineTrajectory(double density, bool padFront)
      : CumulativeSplineTrajectory(density, kernels::IrwinHallCDF2, {},
                                   padFront) {}

  double density() const { return density_; }

  const KernelBase& kernel() const { return kernel_; }

  bool padFront() const { return padFront_; }

  void addControlPoint(const Expression<T>& point) { points_.push_back(point); }

  void addControlPoint(const T& point) { points_.emplace_back(point); }

  const std::vector<Expression<T>>& getControlPoints() const { return points_; }

  Expression<T> sampleTrajectory(const Double_& timestamp,
                                 double windowStart = 0.0,
                                 double windowEnd = -1.0) const {
    const auto [start, end] = controlPointWindow(windowStart, windowEnd);
    const Double_ kernelTime = density_ * timestamp + Double_(kernelOffset_);
    return kernelInterpolate(kernel_, kernelTime, points_, start, end);
  }

  T sampleTrajectory(double timestamp, double windowStart = 0.0,
                     double windowEnd = -1.0) const {
    return sampleTrajectory(Double_(timestamp), windowStart, windowEnd)
        .value(Values());
  }

  TangentExpression sampleTrajectoryDerivative(const Double_& timestamp,
                                               double windowStart = 0.0,
                                               double windowEnd = -1.0,
                                               size_t derivative = 1) const {
    const auto [start, end] = controlPointWindow(windowStart, windowEnd);
    const Double_ kernelTime = density_ * timestamp + Double_(kernelOffset_);
    return Double_(std::pow(density_, derivative)) *
           kernelInterpolateDerivative(kernel_, kernelTime, points_, start, end,
                                       derivative);
  }

  TangentVector sampleTrajectoryDerivative(double timestamp,
                                           double windowStart = 0.0,
                                           double windowEnd = -1.0,
                                           size_t derivative = 1) const {
    return sampleTrajectoryDerivative(Double_(timestamp), windowStart,
                                      windowEnd, derivative)
        .value(Values());
  }
 
 private:
  std::pair<size_t, size_t> controlPointWindow(double windowStart,
                                               double windowEnd) const {
    if (points_.empty()) {
      throw std::invalid_argument(
          "CumulativeSplineTrajectory requires at least one control point");
    }
 
    int start =
        static_cast<int>(std::floor(windowStart * density_)) - windowPre_;
    int end =
        windowEnd < 0.0
            ? static_cast<int>(points_.size())
            : static_cast<int>(std::ceil(windowEnd * density_)) + windowPost_;
    start = std::max(0, start);
    end = std::min(end, static_cast<int>(points_.size()));
    if (start >= end) {
      throw std::invalid_argument(
          "CumulativeSplineTrajectory sample window contains no control "
          "points");
    }
    return {static_cast<size_t>(start), static_cast<size_t>(end)};
  }

  static Expression<T> kernelInterpolate(
      const KernelBase& kernel, const Double_& timestamp,
      const std::vector<Expression<T>>& points, size_t start, size_t end) {
    const std::vector<Expression<T>> pointRange(points.begin() + start,
                                                points.begin() + end);
    const std::vector<Double_> weights =
        sampleKernel(kernel, timestamp, start, end, 0);
    return cumulativePathSum(pointRange, weights);
  }

  static TangentExpression kernelInterpolateDerivative(
      const KernelBase& kernel, const Double_& timestamp,
      const std::vector<Expression<T>>& points, size_t start, size_t end,
      size_t derivative) {
    const std::vector<Expression<T>> pointRange(points.begin() + start,
                                                points.begin() + end);
    const std::vector<Double_> weights =
        sampleKernel(kernel, timestamp, start, end, derivative);
    return cumulativePathSumDerivative(pointRange, weights);
  }

  static std::vector<Double_> sampleKernel(const KernelBase& kernel,
                                           const Double_& timestamp,
                                           size_t start, size_t end,
                                           size_t derivative) {
    std::vector<Double_> samples;
    samples.reserve(end - start);
    const std::function<double(const double&, OptionalJacobian<1, 1>)>
        evaluateKernel =
            [&kernel, derivative](const double& x, OptionalJacobian<1, 1> H) {
              return kernel.evaluateDerivative(derivative, x, H);
            };
 
    for (size_t index = start; index < end; ++index) {
      const Double_ kernelTime =
          timestamp - Double_(static_cast<double>(index));
      samples.emplace_back(evaluateKernel, kernelTime);
    }
    return samples;
  }

  static Expression<T> cumulativePathSum(
      const std::vector<Expression<T>>& points,
      const std::vector<Double_>& cumulativeWeights) {
    return expmap(points.front(),
                  cumulativePathSumDerivative(points, cumulativeWeights));
  }

  static TangentExpression cumulativePathSumDerivative(
      const std::vector<Expression<T>>& points,
      const std::vector<Double_>& weights) {
    TangentExpression tangent(TangentVector::Zero());
    for (size_t index = 1; index < points.size(); ++index) {
      tangent += weights[index] * logmap(points[index - 1], points[index]);
    }
    return tangent;
  }
 
  const double density_;               
  const KernelBase& kernel_;           
  std::vector<Expression<T>> points_;  
  const bool padFront_;                
  const double kernelOffset_;          
  const int windowPre_;                
  const int windowPost_;               
};
 
}  // namespace gtsam