角度归一化实现

Posted 穿越临界点

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了角度归一化实现相关的知识,希望对你有一定的参考价值。

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

文章目录

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;

该代码对应的公式如下:
ϕ = θ + 2 k × π , k ∈ Z ,   θ ∈ ( − 2 π , 2 π ) (1) \\mathbf\\phi = \\theta+2k \\times \\pi,\\qquad k \\in Z,\\ \\theta \\in (-2\\pi,2\\pi) \\tag1 ϕ=θ+2k×π,kZ, θ(2π,2π)(1)

θ = ϕ m o d    ( 2 π ) (2) \\theta = \\phi \\mod (2\\pi) \\tag2 θ=ϕmod(2π)(2)

α = θ + 2 π , − 2 π < θ < − π θ − 2 π , π ≤ θ < 2 π θ , − π ≤ θ < π (3) \\alpha = \\begincases \\theta + 2\\pi, &-2\\pi<\\theta < -\\pi\\\\[3ex] \\theta - 2\\pi, &\\pi \\le \\theta < 2\\pi\\\\[3ex] \\theta, &-\\pi \\le \\theta < \\pi \\endcases \\tag3 α=θ+2π,θ2π,θ,2π<θ<ππθ<2ππθ<π(3)

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

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

3.3 实现优化

对上一节的公式进行变形,如下:
l e t θ ^ = θ + π (4) let\\quad\\hat\\theta = \\theta + \\pi \\tag4 letθ^=θ+π(4)

θ ^ = ( ϕ + π ) m o d    ( 2 π ) (5) \\hat\\theta = (\\phi+\\pi) \\mod (2\\pi) \\tag5 θ^=(ϕ+π)mod(2π)(5)

α = θ ^ + π , θ ^ < 0 θ ^ − π , θ ^ ≥ 0 (6) \\alpha = \\begincases \\hat\\theta + \\pi, \\quad \\hat\\theta < 0 \\\\[3ex] \\hat\\theta - \\pi, \\quad \\hat\\theta \\ge 0 \\endcases \\tag6 α=θ^+π,θ^<0θ^π,θ^0(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 总结

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

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

以上是关于角度归一化实现的主要内容,如果未能解决你的问题,请参考以下文章

角度归一化实现

抽象类与归一化

R语言使用pheatmap绘制热力图(数据归一化行列聚类注释文字角度字体)

为什么要进行归一化处理?(从寻找最优解这个角度讨论)

Tensorflow2实现像素归一化与频谱归一化

TensorFlow实现条件批归一化(Conditional Batch Normalization)