感知融合（三）：目标关联之旋转矩形相交检测

一、前言

在自动驾驶场景中，我们常用“矩形框”来描述道路上的车辆、行人等障碍物，并会根据矩形框之间是否有相交，以及相交区域面积占比（IOU）等信息，来进行目标的关联及跟踪，因此矩形的相交判断十分重要。

矩形框有2种表达方式，一种是没有航向角的Axis Aligned Bounding Box(AABB)，从名称中大致能理解其中意思：沿着轴的包围盒子。也就是将物体用方形的盒子包围，这个方形的盒子的四条边都是和坐标轴平行的。

另一种是带航向角信息的Oriented Bounding Box (OBB)，就是找一个最小的包围物体的方形。

下面分别给出相交判断的方法。

二、AABB矩形相交判断

两个不带航向角的矩形判断十分简单，只需要判断：两个矩形的重心距离在x轴y轴上都小于两个矩形长或宽的一半之和。

就是判断：

重心距离在x轴上的投影长度<两个矩形的在x轴的长度之和/2
重心距离在y轴上的投影长度<两个矩形在y轴上的宽度之和/2

代码如下：

bool intersects(Object r1,Object r2){
	if( fabs(r1.x-r2.x)<(r1.length+r2.length)/2 && fabs(r1.y-r2.y)<(r1.width+r2.width)/2 )
		return true;
		
	return false;
}

三、OBB旋转矩形相交判断

带航向角的旋转矩形相交判断较为复杂，这里需要使用一个定理：

分离轴定律SAT(Separating Axis Theorem)：两个凸多边形物体，如果我们能找到一个轴，使得两个在物体在该轴上的投影互不重叠，则这两个物体之间没有碰撞发生，该轴为Separating Axis。也就是说两个多边形在所有轴上的投影都发生重叠，则判定为碰撞；否则，没有发生碰撞。

矩形有4条边，那么就有4条轴，由于矩形的对边是平行的，所以有两条轴是重复的，我们仅需要检查相邻的两个轴，那么两个矩形就需要检查4个轴。

检查投影有两种方法：
第一种，把每个矩形的4个顶点投影到一个轴上，这样算出4个顶点最长的连线距离，以后同样对待第二个矩形，最后判断2个矩形投影距离是否重叠。
第二种，把2个矩形的半径距离投影到轴上，以后把2个矩形的中心点连线投影到轴上，以后判断2个矩形的中心连线投影，和2个矩形的半径投影之和的大小。

代码如下：

#ifndef _OBBRECT_H_
#define _OBBRECT_H_

#include <math.h>

class OBBRect {
public:
    OBBRect(float x, float y, float length, float width, float rotation = 0.0f)
        : _x(x), _y(y), _length(length), _width(width), _rotation(rotation) {
        resetVector();
    }

    bool intersects(OBBRect& other) {
        float distanceVector[2] = {
            other._x - _x,
            other._y - _y
        };

        for (int i = 0; i < 2; ++i) {
            if (getProjectionRadius(_vectors[i]) + other.getProjectionRadius(_vectors[i])
                <= dot(distanceVector, _vectors[i])) {
                return false;
            }
            if (getProjectionRadius(other._vectors[i]) + other.getProjectionRadius(other._vectors[i])
                <= dot(distanceVector, other._vectors[i])) {
                return false;
            }
        }

        return true;
    }

private:
    void resetVector() {
        _vectors[0][0] = cos(_rotation);
        _vectors[0][1] = sin(_rotation);
        _vectors[1][0] = -_vectors[0][1];
        _vectors[1][1] = _vectors[0][0];
    }

    float dot(float a[2], float b[2]) {
        return abs(a[0] * b[0] + a[1] * b[1]);
    }

    float getProjectionRadius(float vector[2]) {
        return (_length * dot(_vectors[0], vector) / 2 
            + _width * dot(_vectors[1], vector) / 2);
    }

    float _x;
    float _y;
    float _length;
    float _width;
    float _rotation;
    float _vectors[2][2];
};

#endif // _OBBRECT_H_