continuous_trajectory.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <vector>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/expressions.h>
 
namespace gtsam {
class LevenbergMarquardtParams;
}
 
namespace gtsam_points {
 
class ContinuousTrajectory {
public:
  ContinuousTrajectory(char symbol, double start_time, double end_time, double knot_interval);
 
  double knot_stamp(int i) const;
 
  int knot_id(double t) const;
 
  int knot_max_id() const;
 
  gtsam::Pose3_ pose(double t, const gtsam::Double_& t_);
 
  gtsam::Pose3 pose(const gtsam::Values& values, double t);
 
  gtsam::Vector6_ imu(double t, const gtsam::Double_& t_, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));
 
  gtsam::Vector6 imu(const gtsam::Values& values, double t, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));
 
  gtsam::Values fit_knots(
    const std::vector<double>& stamps,
    const std::vector<gtsam::Pose3>& poses,
    double smoothness,
    const gtsam::LevenbergMarquardtParams& lm_params) const;
 
  gtsam::Values fit_knots(const std::vector<double>& stamps, const std::vector<gtsam::Pose3>& poses, double smoothness = 1e-3, bool verbose = false)
    const;
 
public:
  const char symbol;
  const double start_time;
  const double end_time;
  const double knot_interval;
};
 
}  // namespace gtsam_points