#include <trajectory/PolyTrajectory.h>
#include <trajectory/factory/PolyTrajectoryFactory.h>
#include <algorithm>
#include <cmath>
#include "utils/NodeUtils.h"
#include "utils/Transforms.h"