#include <mav_trajectory_generation/trajectory_sampling.h>
#include <mav_trajectory_generation/vertex.h>
#include <trajectory/PolyTrajectory.h>
#include <trajectory/TrajectoryUtils.h>
#include <cassert>
#include <cmath>
#include "utils/Transforms.h"