Scenario.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/linear/NoiseModel.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/geometry/Gal3.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
 
#include <algorithm>
#include <cmath>
#include <iterator>
#include <map>
#include <stdexcept>
#include <string>
 
namespace gtsam {

class GTSAM_EXPORT Scenario {
 public:
  virtual ~Scenario() {}
 
  // Quantities a Scenario needs to specify:
 
  virtual Pose3 pose(double t) const = 0;  
  virtual Vector3 omega_b(double t) const = 0;  
  virtual Vector3 velocity_n(double t) const = 0;  
  virtual Vector3 acceleration_n(double t) const = 0;  
 
  // Derived quantities:
 
  Rot3 rotation(double t) const;
  NavState navState(double t) const;
  Gal3 gal3(double t) const;
 
  Vector3 velocity_b(double t) const;
 
  Vector3 acceleration_b(double t) const;
};

class GTSAM_EXPORT ConstantTwistScenario : public Scenario {
 public:
  ConstantTwistScenario(const Vector3& w, const Vector3& v,
                        const Pose3& nTb0 = Pose3())
      : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
 
  Pose3 pose(double t) const override;
  Vector3 omega_b(double t) const override;
  Vector3 velocity_n(double t) const override;
  Vector3 acceleration_n(double t) const override;
 
 private:
  const Vector6 twist_;
  const Vector3 a_b_;  // constant centripetal acceleration in body = w_b * v_b
  const Pose3 nTb0_;
};

class GTSAM_EXPORT AcceleratingScenario : public Scenario {
 public:
  AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
                       const Vector3& a_n,
                       const Vector3& omega_b = Vector3::Zero())
      : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
 
  Pose3 pose(double t) const override;
  Vector3 omega_b(double t) const override;
  Vector3 velocity_n(double t) const override;
  Vector3 acceleration_n(double t) const override;
 
 private:
  const Rot3 nRb_;
  const Vector3 p0_, v0_, a_n_, omega_b_;
};

class GTSAM_EXPORT DiscreteScenario : public Scenario {
 public:
  DiscreteScenario(const std::map<double, Pose3>& poses,
                   const std::map<double, Vector3>& angularVelocities_b,
                   const std::map<double, Vector3>& velocities_n,
                   const std::map<double, Vector3>& accelerations_n)
      : poses_(poses),
        angularVelocities_b_(angularVelocities_b),
        velocities_n_(velocities_n),
        accelerations_n_(accelerations_n) {
    if (poses_.empty() || angularVelocities_b_.empty() ||
        velocities_n_.empty() || accelerations_n_.empty()) {
      throw std::invalid_argument(
          "Input maps for DiscreteScenario cannot be empty.");
    }
 
    // Since std::map is sorted, the first element has the smallest key
    // and the last element has the largest key. We can access the last
    // element's key efficiently using rbegin().
    double min_t = poses_.begin()->first;
    double max_t = poses_.rbegin()->first;
 
    min_t = std::min(min_t, angularVelocities_b_.begin()->first);
    max_t = std::max(max_t, angularVelocities_b_.rbegin()->first);
 
    min_t = std::min(min_t, velocities_n_.begin()->first);
    max_t = std::max(max_t, velocities_n_.rbegin()->first);
 
    min_t = std::min(min_t, accelerations_n_.begin()->first);
    max_t = std::max(max_t, accelerations_n_.rbegin()->first);
 
    // Calculate and assign the duration.
    t_ = max_t - min_t;
  }

  static DiscreteScenario FromCSV(const std::string& csv_filepath);

  Pose3 pose(double t) const override;
  Vector3 omega_b(double t) const override;
  Vector3 velocity_n(double t) const override;
  Vector3 acceleration_n(double t) const override;
  
  double duration() const;
 
 private:
  std::map<double, Pose3> poses_;
  std::map<double, Vector3> angularVelocities_b_;
  std::map<double, Vector3> velocities_n_;
  std::map<double, Vector3> accelerations_n_;
  double t_;

  template <typename T>
  T interpolate(const std::map<double, T>& values, double t) const {
    // Find the first element with a timestamp >= t
    auto it2 = values.lower_bound(t);
 
    // If t is before or at the very first measurement, return the first value.
    if (it2 == values.begin()) {
      return it2->second;
    }
 
    // If t is after the very last measurement, return the last value.
    if (it2 == values.end()) {
      return values.rbegin()->second;
    }
 
    // Standard case: t is between it1 and it2.
    auto it1 = std::prev(it2);
 
    const double t1 = it1->first;
    const T& value1 = it1->second;
    const double t2 = it2->first;
    const T& value2 = it2->second;
 
    const double dt = t2 - t1;
    // Avoid division by zero if timestamps are identical
    if (std::abs(dt) < 1e-9) {
      return value1;
    }
 
    // Calculate interpolation fraction
    const double alpha = (t - t1) / dt;
 
    // Use GTSAM's manifold interpolation
    return gtsam::interpolate<T>(value1, value2, alpha);
  }
};
 
}  // namespace gtsam