| 116 | } |
| 117 | |
| 118 | void PerceptionUtils::getFOV(vector<Vector3d>& list1, vector<Vector3d>& list2) |
| 119 | { |
| 120 | list1.clear(); |
| 121 | list2.clear(); |
| 122 | |
| 123 | // Get info for visualizing FOV at (pos, yaw) |
| 124 | Eigen::Matrix3d Rwb; |
| 125 | Rwb << cos(yaw_), -sin(yaw_), 0, sin(yaw_), cos(yaw_), 0, 0, 0, 1; |
| 126 | for (size_t i = 0; i < cam_vertices1_.size(); ++i) { |
| 127 | auto p1 = Rwb * cam_vertices1_[i] + pos_; |
| 128 | auto p2 = Rwb * cam_vertices2_[i] + pos_; |
| 129 | list1.push_back(p1); |
| 130 | list2.push_back(p2); |
| 131 | } |
| 132 | } |
| 133 | |
| 134 | void PerceptionUtils::getFOV_PY(vector<Vector3d>& list1, vector<Vector3d>& list2) |
| 135 | { |