|
Calico
A visual-inertial calibration library designed for rapid problem construction and debugging.
|
#include <trajectory.h>
Public Member Functions | |
| 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 |
| Accessors for the rotation spline. | |
| BSpline< 3 > & | rotation_spline () |
| const BSpline< 3 > & | position_spline () const |
| Accesors for the position spline. | |
| BSpline< 3 > & | position_spline () |
| absl::StatusOr< std::vector< Pose3d > > | Interpolate (const std::vector< double > &interp_times) const |
| const absl::flat_hash_map< double, Pose3d > & | trajectory () const |
| Setter/getter for the internal map between timestamp and pose. | |
| absl::flat_hash_map< double, Pose3d > & | trajectory () |
| TrajectoryEvaluationParams | GetEvaluationParamsPosition (double stamp) const |
| Get the parameters needed to evaluate the position spline for a given timestamp. | |
| TrajectoryEvaluationParams | GetEvaluationParamsRotation (double stamp) const |
| Get the parameters needed to evaluate the rotation spline for a given timestamp. | |
Static Public Member Functions | |
| template<typename T > | |
| static Pose3< T > | VectorToPose3 (const Eigen::Vector< T, 6 > &vector) |
Static Public Attributes | |
| static constexpr int | kDefaultSplineOrder = 6 |
| Default spline order of 6. | |
| static constexpr double | kDefaultKnotFrequency = 10 |
| Default knot frequency at 10Hz. | |
Trajectory class. Takes in discrete world-from-sensorrig poses and encodes them as a 6-DOF B-Spline.
| int calico::Trajectory::AddParametersToProblem | ( | ceres::Problem & | problem | ) |
Add internal parameters to a ceres problem. Any internal parameters set to constant are marked as such in the problem. Returns the total number of parameters added to the problem.
| absl::Status calico::Trajectory::FitSpline | ( | const absl::flat_hash_map< double, Pose3d > & | poses_world_sensorrig, |
| double | knot_frequency = kDefaultKnotFrequency, |
||
| int | spline_order = kDefaultSplineOrder |
||
| ) |
Add discrete poses representing a time-parameterized trajectory. A new spline will be fit to these poses with a specified knot frequency and spline order. Returns absl::OkStatus() if successful, an error otherwise.
poses_world_body is a map between a double timestamp in seconds and Pose3d world-from-sensorrig 6-DOF pose:
\[ \mathbf{T}^w_r(t) = \left[\begin{matrix}\mathbf{R}^w_r&\mathbf{t}^w_{wr}\\ \mathbf{0}&1\end{matrix}\right] \]
knot_frequency is the knot rate of the spline. If poses_world_body contains timestamps spanning from \(t_0\) to \(t_f\) seconds, this function will yield a spline with \(n_{knots} = \left(t_f - t_0\right) \left(\text{knot_frequency}\right)\)
spline_order is the order of the spline. A spline order of \(n\) implies that the pose at any given time is:
\[ \mathbf{T}^w_r(t) = \left[\begin{matrix}\boldsymbol{\Phi}^w_r(t)\\\ \mathbf{t}^w_{wr}(t)\end{matrix}\right] = \mathbf{c}_{i,0} + \mathbf{c}_{i,1}t + ... + \mathbf{c}_{i,n}t^n \]
where \(\mathbf{c}_{i,j}\) is the j'th control point for the i'th spline segment.
| absl::StatusOr< std::vector< Pose3d > > calico::Trajectory::Interpolate | ( | const std::vector< double > & | interp_times | ) | const |
Interpolate the trajectory at given timestamps. Returns a vector of world-from-sensorrig poses evaluated in the order of the timestamps. Returns an error if something went wrong during interpolation. For example, attempting to interpolate on out-of-bounds timestmaps.
interp_times is the timestamps to query the spline. No assumptions are made about order.
|
inlinestatic |
Convert a 6-DOF vector to Pose3 type. First three elements of the vector are expected to be an \(SO(3)\) log map vector, and the last three are the position values.
\[ \text{vector}=\left[\begin{matrix}\boldsymbol{\Phi}\\\mathbf{t}\end{matrix}\right]\\ \boldsymbol{\Phi} = \left[\begin{matrix}\Phi_x\\\Phi_y\\\Phi_z\end{matrix}\right]\\ \mathbf{R} = \exp\left([\boldsymbol{\Phi}]_\times\right)\\ \left[\boldsymbol{\Phi}\right]_\times = \left[\begin{matrix}0 &-\Phi_z&\Phi_y\\ \Phi_z&0&-\Phi_x\\ -\Phi_y&\Phi_x&0\end{matrix}\right] \]
1.9.1