trajectory.h

#ifndef CALICO_TRAJECTORY_H_
#define CALICO_TRAJECTORY_H_
 
#include "absl/container/flat_hash_map.h"
#include "absl/status/status.h"
#include "calico/bspline.h"
#include "calico/typedefs.h"
#include "ceres/problem.h"
#include "ceres/rotation.h"
 
 
namespace calico {
 
struct TrajectoryEvaluationParams {
  int spline_index;
  double knot0;
  double knot1;
  double stamp;
  int num_control_points;
  Eigen::MatrixXd basis_matrix;
};
 
class Trajectory {
 public:
  static constexpr int kDefaultSplineOrder = 6;
 
  static constexpr double kDefaultKnotFrequency = 10;
 
  absl::Status FitSpline(
      const absl::flat_hash_map<double, Pose3d>& poses_world_sensorrig,
      double knot_frequency = kDefaultKnotFrequency,
      int spline_order = kDefaultSplineOrder);
 
  int AddParametersToProblem(ceres::Problem& problem);
 
  void EnablePositionEstimation(bool enable);
  void EnableRotationEstimation(bool enable);
 
  const BSpline<3>& rotation_spline() const { return spline_rotation_world_from_body_; }
  BSpline<3>& rotation_spline() { return spline_rotation_world_from_body_; }
 
  const BSpline<3>& position_spline() const { return spline_position_world_to_body_; }
  BSpline<3>& position_spline() { return spline_position_world_to_body_; }
 
  absl::StatusOr<std::vector<Pose3d>>
  Interpolate(const std::vector<double>& interp_times) const;
 
  template <typename T>
  static Pose3<T> VectorToPose3(const Eigen::Vector<T, 6>& vector) {
    const Eigen::Vector3<T> phi = vector.head(3);
    const Eigen::Vector3<T> pos = vector.tail(3);
    Eigen::Vector4<T> q;
    ceres::AngleAxisToQuaternion(phi.data(), q.data());
    const Eigen::Quaternion<T> rot(q(0), q(1), q(2), q(3));
    return Pose3<T>(rot, pos);
  }
 
  const absl::flat_hash_map<double, Pose3d>& trajectory() const;
  absl::flat_hash_map<double, Pose3d>& trajectory();
 
  TrajectoryEvaluationParams GetEvaluationParamsPosition(double stamp) const;
 
  TrajectoryEvaluationParams GetEvaluationParamsRotation(double stamp) const;
 
 private:
  absl::flat_hash_map<double, Pose3d> pose_id_to_pose_world_body_;
  BSpline<3> spline_rotation_world_from_body_;
  BSpline<3> spline_position_world_to_body_;
  bool position_enabled_;
  bool rotation_enabled_;
 
  // Convenience function for unwrapping discrete axis-angle vectors in order to
  // get a more continuous signal.
  void UnwrapPhaseLogMap(std::vector<Eigen::Vector3d>& phi);
 
};
 
} // namespace calico
 
#endif //CALICO_TRAJECTORY_H_