|
Eigen::Matrix4d | ov_core::exp_se3 (Eigen::Matrix< double, 6, 1 > vec) |
| SE(3) matrix exponential function. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::exp_so3 (const Eigen::Matrix< double, 3, 1 > &w) |
| SO(3) matrix exponential. More...
|
|
Eigen::Matrix4d | ov_core::hat_se3 (const Eigen::Matrix< double, 6, 1 > &vec) |
| Hat operator for R^6 -> Lie Algebra se(3) More...
|
|
Eigen::Matrix< double, 4, 1 > | ov_core::Inv (Eigen::Matrix< double, 4, 1 > q) |
| JPL Quaternion inverse. More...
|
|
Eigen::Matrix4d | ov_core::Inv_se3 (const Eigen::Matrix4d &T) |
| SE(3) matrix analytical inverse. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::Jl_so3 (const Eigen::Matrix< double, 3, 1 > &w) |
| Computes left Jacobian of SO(3) More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::Jr_so3 (const Eigen::Matrix< double, 3, 1 > &w) |
| Computes right Jacobian of SO(3) More...
|
|
Eigen::Matrix< double, 6, 1 > | ov_core::log_se3 (Eigen::Matrix4d mat) |
| SE(3) matrix logarithm. More...
|
|
Eigen::Matrix< double, 3, 1 > | ov_core::log_so3 (const Eigen::Matrix< double, 3, 3 > &R) |
| SO(3) matrix logarithm. More...
|
|
Eigen::Matrix< double, 4, 4 > | ov_core::Omega (Eigen::Matrix< double, 3, 1 > w) |
| Integrated quaternion from angular velocity. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::quat_2_Rot (const Eigen::Matrix< double, 4, 1 > &q) |
| Converts JPL quaterion to SO(3) rotation matrix. More...
|
|
Eigen::Matrix< double, 4, 1 > | ov_core::quat_multiply (const Eigen::Matrix< double, 4, 1 > &q, const Eigen::Matrix< double, 4, 1 > &p) |
| Multiply two JPL quaternions. More...
|
|
Eigen::Matrix< double, 4, 1 > | ov_core::quatnorm (Eigen::Matrix< double, 4, 1 > q_t) |
| Normalizes a quaternion to make sure it is unit norm. More...
|
|
Eigen::Matrix< double, 3, 1 > | ov_core::rot2rpy (const Eigen::Matrix< double, 3, 3 > &rot) |
| Gets roll, pitch, yaw of argument rotation (in that order). More...
|
|
Eigen::Matrix< double, 4, 1 > | ov_core::rot_2_quat (const Eigen::Matrix< double, 3, 3 > &rot) |
| Returns a JPL quaternion from a rotation matrix. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::rot_x (double t) |
| Construct rotation matrix from given roll. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::rot_y (double t) |
| Construct rotation matrix from given pitch. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::rot_z (double t) |
| Construct rotation matrix from given yaw. More...
|
|
Eigen::Matrix< double, 3, 3 > | ov_core::skew_x (const Eigen::Matrix< double, 3, 1 > &w) |
| Skew-symmetric matrix from a given 3x1 vector. More...
|
|
Eigen::Matrix< double, 3, 1 > | ov_core::vee (const Eigen::Matrix< double, 3, 3 > &w_x) |
| Returns vector portion of skew-symmetric. More...
|
|