优化这个函数的效率void common_pack::calculateQuaternionsForEachPoint(std::vector<autoware_msgs::Waypoint> &path) { Eigen::Quaterniond q; for (size_t i = 0; i + 1 < path.size(); i++) { autoware_msgs::Waypoint p_c = path[i]; autoware_msgs::Waypoint p_n = path[i + 1]; double yaw = std::atan2(p_n.pose.pose.position.y - p_c.pose.pose.position.y, p_n.pose.pose.position.x - p_c.pose.pose.position.x); Eigen::AngleAxisd rollangle(0, Eigen::Vector3d::UnitX()); Eigen::AngleAxisd yawangle(yaw, Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchangle(0, Eigen::Vector3d::UnitY()); q = rollangle * yawangle * pitchangle; path.at(i).pose.pose.orientation.x = q.x(); path.at(i).pose.pose.orientation.y = q.y(); path.at(i).pose.pose.orientation.z = q.z(); path.at(i).pose.pose.orientation.w = q.w(); } if (path.size() > 2) { path.at(path.size() - 1).pose.pose.orientation = path[path.size() - 2].pose.pose.orientation; } }
时间: 2024-02-11 16:07:37 浏览: 29
可以考虑以下优化方法:
1. 避免重复计算:可以将重复计算的值存储在变量中,避免重复计算。例如,将 p_n.pose.pose.position.x - p_c.pose.pose.position.x 存储在变量 dx 中,将 p_n.pose.pose.position.y - p_c.pose.pose.position.y 存储在变量 dy 中。
2. 减少函数调用次数:可以避免多次调用 path.at(i).pose.pose.orientation,将其存储在一个变量中,例如,将 path.at(i).pose.pose.orientation 存储在变量 orientation 中,然后直接赋值 orientation.x() = q.x() 等。
3. 减少 vector 的访问:可以将访问 path[i] 和 path[i+1] 的操作提取到循环外,存储在变量 p_c 和 p_n 中。
改进后的代码如下:
```
void common_pack::calculateQuaternionsForEachPoint(std::vector<autoware_msgs::Waypoint> &path) {
Eigen::Quaterniond q;
Eigen::AngleAxisd rollangle(0, Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchangle(0, Eigen::Vector3d::UnitY());
if (path.size() < 2) {
return;
}
autoware_msgs::Waypoint p_c = path[0];
for (size_t i = 0; i + 1 < path.size(); i++) {
autoware_msgs::Waypoint p_n = path[i + 1];
double dx = p_n.pose.pose.position.x - p_c.pose.pose.position.x;
double dy = p_n.pose.pose.position.y - p_c.pose.pose.position.y;
double yaw = std::atan2(dy, dx);
Eigen::AngleAxisd yawangle(yaw, Eigen::Vector3d::UnitZ());
q = rollangle * yawangle * pitchangle;
auto& orientation = path[i].pose.pose.orientation;
orientation.x() = q.x();
orientation.y() = q.y();
orientation.z() = q.z();
orientation.w() = q.w();
p_c = path[i];
}
auto& last_orientation = path.back().pose.pose.orientation;
last_orientation = path[path.size() - 2].pose.pose.orientation;
}
```