角度归一化实现

Posted 2022-11-29 穿越临界点

在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。

文章目录

• • 0 前言
  • 1 什么是角度归一化
  • 2 为什么做角度归一化
  • 3 代码实现
  • • 3.1 Apollo中的实现
    • 3.2 常规思路实现
    • 3.3 实现优化
  • 4 总结

0 前言

从嵌入式系统转到自动驾驶算法岗一段时间了，发现思考问题的思路或者说套路还是很不一样的：系统更依赖于逻辑思维（分析和推理），算法更依赖于数学思维（每一行代码都能看懂，但是背后的逻辑是不能单靠代码看懂的，需要推一推数学公式，数学公式推明白了，再看代码，发现其实也很简单）。

这篇博客的内容实际上非常简单，之所以写它，一个原因是自己好长时间不写博客了，拿它热热身；还有就是作为自己转型之后的处女作调子不想起的太高，而且难的事情或者难的技术未必很难，只是没有被分解为多个简单的事情而已，不好高骛远，小步快跑 才是进步最快的方法；最后一个原因是看到Apollo里角度归一化的代码写的对新手并不“友好”，简单问题搞复杂了，借此契机作一个说明，希望对新手有一点点帮助。

1 什么是角度归一化

归一化就是将函数的幅值映射到[0,1]的范围内。

角度归一化只是这么叫，其实并不是将角度映射到[0,1]，而是将其映射到一个周期内，[0,2pi)或[-pi, pi)或者其他的区间。

2 为什么做角度归一化

三角函数为周期函数，反三角函数的结果按道理说有无数个，为了方便将其映射到一个周期内。

而且C++中的反三角函数给出的结果映射到了[-pi, pi]，我们计算角度时为了和C++标准库保持一致，也将角度值映射到这个区间。

至于这个区间为什么是全闭区间，查看C++标准库（以std::arctan2为例）说明如下：

If no errors occur, the arc tangent of y/x (arctan(yx)) in the range [-π , +π] radians, is returned.

• If x is -∞ and y is finite and positive, +π is returned
• If x is -∞ and y is finite and negative, -π is returned

3 代码实现

3.1 Apollo中的实现

double NormalizeAngle(const double angle) 
    double a = std::fmod(angle + M_PI, 2.0 * M_PI); 
    if (a < 0.0) 
        a += (2.0 * M_PI);
    
    return a - M_PI;

上述代码是对正常的思路（公式）做了一个“偷懒”的变化之后的，让人看着很难受。如果你是第一次看到这个代码，先跳过往下看吧，看完下面的代码再回过头来看，就可以一眼发现到底是哪里“怪怪的”了。

这里需要说明一点，Apollo的代码将角度值映射到了 [-pi, pi) 这样一个左闭右开的区间，和标准库的映射区间并不相同。

3.2 常规思路实现

正常人第一个想到的代码实现是这个样子的：

double NormalizeAngle(const double angle) 
    double a = std::fmod(angle, 2.0 * M_PI); 
    if (a < -M_PI) 
        a += (2.0 * M_PI);
     else if (a >= M_PI)  // 这里一般不加等号，为了和Apollo代码保持完全一致（映射到[-pi,pi))才加了等号。
    	a -= (2.0 * M_PI);
    
    return a;

该代码对应的公式如下：
$$\varphi =\theta +2k\times \pi ,k\in Z,\theta \in (-2\pi ,2\pi )\text{\hspace{1em}(1)}$$

$$\theta =\varphi \mathrm{m}\mathrm{o}\mathrm{d}(2\pi )\text{\hspace{1em}(2)}$$

$$\alpha =\{\begin{array}{ll}\theta +2\pi ,& -2\pi <\theta <-\pi \\ \theta -2\pi ,& \pi \le \theta <2\pi \\ \theta ,& -\pi \le \theta <\pi \end{array}\text{\hspace{1em}(3)}$$

我们发现这个代码的实现思路就非常符合正常思维（未对公式进行变形）了。但是我们发现一个代码效率的问题，就是标准实现思路比Apollo的代码多了一个分支判断。

怎么样来简化掉这个分支判断呢？答案是对公式进行变形。

3.3 实现优化

对上一节的公式进行变形，如下：
$$let\hat{\theta }=(\theta +\pi )\mathrm{m}\mathrm{o}\mathrm{d}(2\pi ),\hat{\theta }\in (-\pi ,\pi )\text{\hspace{1em}(4)}$$

$$\begin{array}{rl}\hat{\theta }& =((\varphi \mathrm{m}\mathrm{o}\mathrm{d}(2\pi ))+\pi )\mathrm{m}\mathrm{o}\mathrm{d}(2\pi )\\ & =(\varphi +\pi )\mathrm{m}\mathrm{o}\mathrm{d}(2\pi )\end{array}\text{\hspace{1em}(5)}$$

$$\alpha =\{\begin{array}{l}\hat{\theta }+\pi ,-\pi <\hat{\theta }<0\\ \hat{\theta }-\pi ,0\le \hat{\theta }<\pi \end{array}\text{\hspace{1em}(6)}$$

根据该公式进行代码实现如下：

double NormalizeAngle(const double angle) 
    double a = std::fmod(angle + M_PI, 2.0 * M_PI); 
    if (a < 0) 
        a += M_PI;
     else  
    	a -= M_PI;
    
    return a;

其实到这一步，优化已经做完了，仅仅进行了一次判断。对于Apollo去掉else分支的做法并不能提升代码效率，反而会降低代码的可读性，个人不推荐这种做法。

4 总结

• 看算法代码和普通的逻辑代码不同，要先了解背后的公式，否则效率很低。
• 如果对代码效率有提升再做公式的变换或者代码行数的减少，否则不要画蛇添足。

恭喜你又坚持看完了一篇博客，又进步了一点点！如果感觉还不错就点个赞再走吧，你的点赞和关注将是我持续输出的哒哒哒动力~~