|
Eigen::Matrix4d | exp_se3 (Eigen::Matrix< double, 6, 1 > vec) |
|
Eigen::Matrix< double, 3, 3 > | exp_so3 (const Eigen::Matrix< double, 3, 1 > &w) |
|
Eigen::Matrix4d | hat_se3 (const Eigen::Matrix< double, 6, 1 > &vec) |
|
Eigen::Matrix< double, 4, 1 > | Inv (Eigen::Matrix< double, 4, 1 > q) |
|
Eigen::Matrix4d | Inv_se3 (const Eigen::Matrix4d &T) |
|
Eigen::Matrix< double, 3, 3 > | Jl_so3 (const Eigen::Matrix< double, 3, 1 > &w) |
|
Eigen::Matrix< double, 3, 3 > | Jr_so3 (const Eigen::Matrix< double, 3, 1 > &w) |
|
Eigen::Matrix< double, 6, 1 > | log_se3 (Eigen::Matrix4d mat) |
|
Eigen::Matrix< double, 3, 1 > | log_so3 (const Eigen::Matrix< double, 3, 3 > &R) |
|
Eigen::Matrix< double, 4, 4 > | Omega (Eigen::Matrix< double, 3, 1 > w) |
|
Eigen::Matrix< double, 3, 3 > | quat_2_Rot (const Eigen::Matrix< double, 4, 1 > &q) |
|
Eigen::Matrix< double, 4, 1 > | quat_multiply (const Eigen::Matrix< double, 4, 1 > &q, const Eigen::Matrix< double, 4, 1 > &p) |
|
Eigen::Matrix< double, 4, 1 > | quatnorm (Eigen::Matrix< double, 4, 1 > q_t) |
|
Eigen::Matrix< double, 3, 1 > | rot2rpy (const Eigen::Matrix< double, 3, 3 > &rot) |
|
Eigen::Matrix< double, 4, 1 > | rot_2_quat (const Eigen::Matrix< double, 3, 3 > &rot) |
|
Eigen::Matrix< double, 3, 3 > | rot_x (double t) |
|
Eigen::Matrix< double, 3, 3 > | rot_y (double t) |
|
Eigen::Matrix< double, 3, 3 > | rot_z (double t) |
|
Eigen::Matrix< double, 3, 3 > | skew_x (const Eigen::Matrix< double, 3, 1 > &w) |
|
Eigen::Matrix< double, 3, 1 > | vee (const Eigen::Matrix< double, 3, 3 > &w_x) |
|