Program Listing for File generic-path-generator.h

↰ Return to documentation for file (algorithms/simulation/include/simulation/generic-path-generator.h)

#ifndef SIMULATION_GENERIC_PATH_GENERATOR_H_
#define SIMULATION_GENERIC_PATH_GENERATOR_H_

#include <random>
#include <string>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <aslam/common/memory.h>
#include <gflags/gflags.h>

#include "simulation/visual-inertial-path-generator.h"

namespace test_trajectory_gen {

class GenericPathGenerator : public VisualInertialPathGenerator {
 public:
  explicit GenericPathGenerator(const PathAndLandmarkSettings& settings)
      : VisualInertialPathGenerator(settings), settings_(settings) {}

  virtual void generatePath();

  virtual void generateLandmarks();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  void generatePathConstraintsCircle(
      const double circle_radius_meter, const size_t num_of_path_constraints,
      const Eigen::Vector4d start_offset);

  void generatePathConstraintsEllipse(
      const double kappa, const double lambda, const double num_of_rounds,
      const size_t num_of_path_constraints, const Eigen::Vector4d start_offset);

  void generatePathConstraintsRotationOnly();

  void generatePathConstraintsTranslationOnly();

  void generatePathConstraintsFromFile(const std::string& filename);

  void generatePathConstraintsFromCtor(
      const Aligned<std::vector, Eigen::Vector4d>& pose_waypoints);

  void insertPathConstraints(
      mav_planning_utils::path_planning::Vertex1D* max_position,
      mav_planning_utils::path_planning::Vertex1D* max_yaw);

  void generateLandmarksCircle(
      const double circle_radius_meter,
      const double distance_to_keypoints_meter,
      const double landmark_variance_meter,
      const double vertical_to_radial_variance_factor,
      const size_t num_of_landmarks, const size_t landmark_seed,
      const Eigen::Vector3d start_offset_meter);

  void generateLandmarksEllipse(
      const double distance_to_keypoints_meter, const double kappa,
      const double lambda, const double landmark_variance_meter,
      const double num_of_rounds,
      const double vertical_to_radial_variance_factor,
      const size_t num_of_landmarks, const size_t landmark_seed,
      const Eigen::Vector3d start_offset_meter);

  void generateLandmarksVolume(
      Eigen::Vector3d landmark_offset_meter,
      Eigen::Vector3d landmark_volume_meter,
      const double landmark_variance_meter, const size_t num_of_landmarks,
      const size_t landmark_seed);

  PathAndLandmarkSettings settings_;

  static constexpr int kDerivativeToOptimizeVelocity = 1;

  static constexpr int kDerivativeToOptimizeAcceleration = 2;

  static constexpr int kConstraintPosition = 0;

  static constexpr int kConstraintVelocity = 1;

  // Currently completely ignored in path_planning.h.
  static constexpr double kTimeToNext = 1.0;

  static constexpr int kDerivatives = 4;

  static constexpr double kConstraintValuePosition = 3.0;

  static constexpr double kConstraintValueYaw = 3.0;

  static constexpr int kContinuity = 4;

  Aligned<std::vector, Eigen::Vector4d> pose_waypoints_;

  Aligned<std::vector, mav_planning_utils::path_planning::Vertex4D> sv_;
};

}  // namespace test_trajectory_gen
#endif  // SIMULATION_GENERIC_PATH_GENERATOR_H_