| 75 | } |
| 76 | |
| 77 | void PerceptionUtils::setPose(const Vector3d& pos, const double& yaw) |
| 78 | { |
| 79 | pos_ = pos; |
| 80 | yaw_ = yaw; |
| 81 | |
| 82 | // Transform the normals of camera FOV |
| 83 | Eigen::Matrix3d R_wb; |
| 84 | R_wb << cos(yaw_), -sin(yaw_), 0.0, sin(yaw_), cos(yaw_), 0.0, 0.0, 0.0, 1.0; |
| 85 | Vector3d pc = pos_; |
| 86 | |
| 87 | Eigen::Matrix4d T_wb = Eigen::Matrix4d::Identity(); |
| 88 | T_wb.block<3, 3>(0, 0) = R_wb; |
| 89 | T_wb.block<3, 1>(0, 3) = pc; |
| 90 | Eigen::Matrix4d T_wc = T_wb * T_bc_; |
| 91 | Eigen::Matrix3d R_wc = T_wc.block<3, 3>(0, 0); |
| 92 | // Vector3d t_wc = T_wc.block<3, 1>(0, 3); |
| 93 | normals_ = { n_top_, n_bottom_, n_left_, n_right_ }; |
| 94 | for (auto& n : normals_) n = R_wc * n; |
| 95 | } |
| 96 | |
| 97 | void PerceptionUtils::setPose_PY(const Vector3d& pos, const double& pitch, const double& yaw) |
| 98 | { |
no outgoing calls
no test coverage detected