泊车路线规划中的坐标转换介绍

背景：

之前在泊车路径规划过程中，碰到关于坐标系转换的问题，因为涉及到几个坐标系，不同的转换方式，在此希望通过文章的方式记录自己知识的过程。

目的：

掌握泊车路径规划过程中，坐标系转换的规律

前提：

用三个坐标系之间的转换举例，分别是baselink坐标系，原点在后轴中心，x轴指向车头方向；camera坐标系，原点在车头，y坐标系指向车头方向；slot坐标系，原点在车位2号角点，y轴是2->3方向，所有坐标系是右手坐标系

公式：

假设有两个坐标系，A and B，我们希望将坐标系A中的坐标转换到坐标系B中

A是原坐标系，B是目标坐标系

其中代表坐标系B中的坐标， 代表坐标系A中的坐标， 代表坐标系A到B的旋转， 代表坐标系A到B的平移

内容：

内容主要分两部分，概要：

1. 知道原始坐标系A的原点以及x轴分别在目标坐标系B中的位置(x, y)和角度theta;
2. 知道目标坐标系B的原点以及x轴的航向角在原始坐标系A的位置(x, y)和角度theta.

详细介绍方法：

一. 知道原始坐标系A的原点以及x轴分别在目标坐标系B中的位置(x, y)和角度theta我们取这个原点坐标作为平移量translation，角度作为旋转量rotation

translation = 坐标系A的原点目标坐标系B中的坐标(x, y)
rotation = 坐标系A的x轴在目标坐标系B中的角度theta

bool ConvertPointFromSourceToDestinationCoordinateBaseDestination(
         POINT &point, const POINT &translation, const float rotation) {
    POINT p;
    const float r_cos = cosf(rotation);
    const float r_sin = sinf(rotation);

    p.x = point.x * r_cos - point.y * r_sin;
    p.y = point.x * r_sin + point.y * r_cos;

    point.x = p.x + translation.x;
    point.y = p.y + translation.y;

    return true;
}

第一步，旋转，这个可以这样理解，B坐标系不动，但是B坐标系有一个拷贝坐标系，刚开始时，两者重合，然后拷贝坐标系旋转theta角度，方向和坐标系A重合；
第二步，平移，可以这样理解，将这个已经旋转的B的拷贝坐标系再平移到A的原点。

此时就完成了将坐标系A中的点point通过先旋转再平移，得到在B坐标系中的位置。

举个栗子：baselink->camera

将baselink下面的点转换到camera坐标系

因为知道轴距，所以知道baselink的原点在camera坐标系中的坐标为(0, -(后轴中心到车头距离))，同时，baselink坐标系的x轴在camera坐标系中的角度theta = 90度，所以通过带入上面公式即可得。

二. 知道目标坐标系B的原点以及x轴的航向角在原始坐标系A的位置(x, y)和角度theta

我们取这个原点坐标作为平移量translation，角度作为旋转量rotation

translation = 坐标系B的原点目标坐标系A中的坐标(x, y)
rotation = 坐标系B的x轴在目标坐标系A中的角度theta

bool ConvertPointFromSourceToDestinationCoordinateBaseSource(
         POINT &point, const POINT &translation, const float rotation) {
    FPOINT p;
    const float r_cos = cosf(rotation);
    const float r_sin = sinf(rotation);

    p.x = point.x - translation.x;
    p.y = point.y - translation.y;

    point.x = p.x * r_cos + p.y * r_sin;
    point.y = p.y * r_cos - p.x * r_sin;

    return true;
}

这一部分可以看做和第一部分相反的操作

第一步平移，这个时候可以这样理解，此时有个原坐标系A的拷贝，A首先平移到B坐标系的位置，此时的平移和方法一正好相反，直观公式是用的“-”；
第二步旋转，这个时候请看旋转矩阵，也和方法一旋转方向相反，因为此时是原坐标系旋转theta角度到目标坐标；

举个栗子：

现在有一个环视系统，会输出环视检测的车位信息结果，一般输出检测到的车位点坐标，这些原始坐标通过在camera坐标系中输出，假设，现在得到四个车位在camera坐标系下的角点坐标，然后路径规划过程中，我们希望在一个固定的车位坐标系中进行路径规划，所以希望建立一个以2号角点为原点，2->3为y方向的车位坐标系

于是，因为我们知道(目标坐标系)车位坐标系原点(2号角点)在原坐标系(camera坐标系)中的坐标，同时通过2， 1点之间的关系，通过tan得到车位坐标系x轴在原坐标系中的theta,就满足了方法二所必需的的条件，使用方法二，即可得。

总结

1. 如果想知道现在需要什么样的转换，首先应该明白，现在自己手里有什么东西，比如，我们“ 知道原始坐标系A的原点以及x轴分别在目标坐标系B中的位置(x, y)和角度theta”，那么就用方法一，如果“ 知道目标坐标系B的原点以及x轴的航向角在原始坐标系A的位置(x, y)和角度theta”，就用方法二；
2. 两种方法的旋转和平移的次序是相反的，同时旋转和平移的量也是相反的；