百度apollo自动驾驶planning代码学习-Apollo\modules\planning\common\util\math_util.cc & math_util.h代码解析

概述

math_util.cc/.h是planning模块下的common/util路径下
根据路径和命名也可以看出
math_util.cc其实就是实现了一些planning中会用到的一些通用的数学辅助工具，util这个文件夹在很多工程中都可以看到，都是存放一些辅助功能的实现代码。

从代码来看math_util.cc主要是实现：
1.将世界坐标系坐标转化到特定参考坐标系下
2.将世界坐标系下的角度转化到参考系下的角度

math_util.h

#pragma once

#include <utility>

namespace apollo {
namespace planning {
namespace util {

// Helper function to convert world coordinates to relative coordinates
// around the obstacle of interest.
//这个函数顾名思义是实现世界系坐标转物体系(车体系)坐标？返回的量是pair结构，一对double
//数,就是转化后的(x,y)
//输入参数：输入待转化的世界坐标input_world_coord，参考系坐标原点obj_world_coord，
//参考系坐标相对世界坐标的旋转角obj_world_angle，只有两维。
std::pair<double, double> WorldCoordToObjCoord(
    std::pair<double, double> input_world_coord,
    std::pair<double, double> obj_world_coord, double obj_world_angle);

//世界系坐标下的与X轴所成角转化成参考系下与x轴所成角
//具体原理可以参见下图的示例
double WorldAngleToObjAngle(double input_world_angle, double obj_world_angle);

}  // namespace util
}  // namespace planning
}  // namespace apollo

math_util.cc

#include "modules/planning/common/util/math_util.h"

#include <utility>

#include "modules/common/math/math_utils.h"

namespace apollo {
namespace planning {
namespace util {

//这个函数顾名思义是实现世界系坐标转物体系(车体系)坐标？返回的量是pair结构，一对double
//数,就是转化后的(x,y)
//输入参数：输入待转化的世界坐标input_world_coord，参考系坐标原点obj_world_coord，
//参考系坐标相对世界坐标的旋转角obj_world_angle，只有两维。
//具体原理可以参见下图的示例
std::pair<double, double> WorldCoordToObjCoord(
    std::pair<double, double> input_world_coord,
    std::pair<double, double> obj_world_coord, double obj_world_angle) {
  //计算待转化的世界坐标相对参考坐标系原点的delta_x,delta_y
  double x_diff = input_world_coord.first - obj_world_coord.first;
  double y_diff = input_world_coord.second - obj_world_coord.second;
  //计算待转化的世界坐标点到相对参考坐标系原点的距离，记为rho
  double rho = std::sqrt(x_diff * x_diff + y_diff * y_diff);
  //计算待转化世界系坐标点与参考系物体系原点连线与参考线横轴所成角
  double theta = std::atan2(y_diff, x_diff) - obj_world_angle;
  //根据rho theta计算世界系坐标转化到参考系坐标下的值
  return std::make_pair(std::cos(theta) * rho, std::sin(theta) * rho);
}
//世界系坐标下的与X轴所成角转化成参考系下与x轴所成角
//具体原理可以参见下图的示例
double WorldAngleToObjAngle(double input_world_angle, double obj_world_angle) {
  return common::math::NormalizeAngle(input_world_angle - obj_world_angle);
}

}  // namespace util
}  // namespace planning
}  // namespace apollo

WorldCoordToObjCoord使用示例

WorldAngleToObjAngle使用示例