| 112 | } |
| 113 | |
| 114 | double ViewNode::computeCostPY(const Eigen::VectorXd& pose1, const Eigen::VectorXd& pose2) |
| 115 | { |
| 116 | // Cost of position change |
| 117 | Vector3d pos1 = pose1.head(3); |
| 118 | Vector3d pos2 = pose2.head(3); |
| 119 | vector<Vector3d> path; |
| 120 | double pos_cost = ViewNode::searchPath(pos1, pos2, path) / vm_; |
| 121 | |
| 122 | // Cost of pitch change |
| 123 | double pitch1 = pose1(3); |
| 124 | double pitch2 = pose2(3); |
| 125 | double diff_pitch = fabs(pitch1 - pitch2); |
| 126 | double pitch_cost = diff_pitch / yd_; |
| 127 | |
| 128 | // Cost of yaw change |
| 129 | double yaw1 = pose1(4); |
| 130 | double yaw2 = pose2(4); |
| 131 | double diff_yaw = fabs(yaw1 - yaw2); |
| 132 | diff_yaw = min(diff_yaw, 2 * M_PI - diff_yaw); |
| 133 | double yaw_cost = diff_yaw / yd_; |
| 134 | return max(pos_cost, max(pitch_cost, yaw_cost)); |
| 135 | } |
| 136 | } // namespace hetero_planner |