This article summarizes the problems related to coordinate system conversion encountered by bloggers in their work, as well as the implementation of C language programming.
Article directory
1 Problem Scenario
For ADAS algorithm development, I have encountered many scenarios that require coordinate system conversion in my work. For example, a car has many sensors, including cameras, millimeter-wave radar, and lidar. After sensing the external environment information at the same time, since different sensors are based on their own coordinate system, the target information needs to be converted to the same coordinate system (usually the center point of the rear axle of the car) before data fusion.
For example, the XOY coordinate system in the figure below is based on the center point of the rear axle of the vehicle as the origin, the direction of the front of the vehicle is the positive direction of the X axis, and the vertical direction of the vehicle body to the left is the positive direction of the Y axis. X'O'Y' is the front-view camera coordinate system, with the camera position as the coordinate origin, and the coordinate axis direction is the same as XY.
The X'O'Y' coordinate system translates a distance forward relative to the XOY coordinate system, and there is a corresponding distance on the left and right. Knowing that the coordinates of a point P in the X'O'Y' coordinate system are (x', y'), and the coordinates of the point O' in the XOY coordinate system (xo', yo'), you can pass the coordinates Translate to calculate the coordinates of point P in the XOY coordinate system. This is an example of coordinate system translation.
For another example, when the vehicle is in motion, if the steering wheel is turned at an angle, the car will make a circular motion. At a certain moment t0, after Δt time, the vehicle not only has a positional translation, but also has a rotation itself.
Combined with the above figure, for example, at time t0, the coordinates of a target point in the XOY coordinate system are (x, y), and at time t1, the vehicle moves to the position of the previous point, and the front of the vehicle is facing an angle to the left. At this time, the relative position between the target point and the car has changed, and its coordinates in the X'O'Y' coordinate system need to be recalculated. Using the distance and angle of the X'O'Y' coordinate system relative to the XOY coordinate system, the coordinates (x', y') of the target point in the X'O'Y' coordinate system can be obtained. This example includes translation and rotation of the coordinate system.
2 Formula derivation
The problem of the above scenario is summarized as follows: The coordinates of the known point P in the XOY coordinate system are (px,py), and the coordinates of the origin O' of the X'O'Y' coordinate system in the XOY coordinate system are (ox',oy '), find the coordinates of point P in the X'O'Y' coordinate system. Here the coordinate system adopts the right-handed system, that is, X goes to the right and Y goes up. The rotation angle is defined as a counterclockwise positive angle for later derivation.
The two coordinate systems are transformed through translation and rotation. To simplify the derivation, first make an intermediate coordinate system X''O'Y'' based on the O' point, and complete the rotation first.
The problem here is converted to first deriving the relationship between (px', py') and (px'', py''), and then deriving the relationship between (px'', py'') and (px, py).
2.1 Derivation of rotating coordinate system
First, make vertical lines PA⊥O'Y', PB⊥O'X', PC⊥O'Y'', PD⊥O'X'' through point P, then it is easy to know ∠BPD = θ, as shown in the figure below Show.
Then do DE⊥O'X' through point D, as follows:
In this way, px' can be deduced:
px ′ = ∣ O ′ E ∣ + ∣ EB ∣ = ∣ O ′ D ∣ ⋅ cos θ + ∣ PD ∣ ⋅ sin θ = px ′ ′ ⋅ cos θ + py ′ ′ ⋅ sin θ px'\;=\;\vert O'E\vert\;+\vert\;EB\vert\;=\;\vert O'D\vert\cdot\cos\theta\;+\;\vert PD\vert\cdot\sin\theta\;=\;px''\cdot\cos\theta\;\;+\;py' '\cdot\sin\theta\;px′=∣O′E∣+∣EB∣=∣O′D∣⋅cosi+∣PD∣⋅sini=px′′⋅cosi+py′′⋅sini
Then deduce py', do DF⊥PF through point D (PF is the extension line of PB), then you can deduce py':
py ′ = ∣ PF ∣ − ∣ BF ∣ = ∣ PD ∣ ⋅ cos θ − ∣ OD ∣ ⋅ sin θ = py ′ ′ ⋅ cos θ − px ′ ′ ⋅ sin θ py'\;=\;\vert PF\vert\;-\;\vert\;BF\vert\;=\;\ vert PD\vert\cdot\cos\theta\;-\;\vert OD\vert\cdot\sin\theta\;=\;py''\cdot\cos\theta\;\;-\;px'' \cdot\sin\theta\;py′=∣PF∣−∣BF∣=∣PD∣⋅cosi−∣OD∣⋅sini=py′′⋅cosi−px′′⋅sini
Write the results of the above two derivations together, which will be used in the following chapters:
px ′ = px ′ ′ ⋅ cos θ + py ′ ′ ⋅ sin θ py ′ = − px ′ ′ ⋅ sin θ + py ′ ′ ⋅ cos θ px'\;=\;px''\cdot\cos\theta\;\;+\;py''\cdot\sin\theta\;\\\; py'\;=-\ ;px''\cdot\sin\theta\;+\;py''\cdot\cos\theta\;\;px′=px′′⋅cosi+py′′⋅sinipy′=−px′′⋅sini+py′′⋅cosi
Here, matrix transformation can also be used, and this conclusion can be drawn in one step. Point P is equivalent to turning the angle θ clockwise around the origin, so the transformation relationship can be obtained through the rotation matrix formula.
[ px ′ py ′ ] = [ cos θ sin θ − sin θ cos θ ] [ px ′ ′ py ′ ′ ] \begin{bmatrix}px'\\py'\end{bmatrix}\;=\ ;\begin{bmatrix}\cos\theta&\sin\theta\\-\sin\theta&\cos\theta\end{bmatrix}\begin{bmatrix}px''\\py''\end{bmatrix}[px′py′]=[cosi−sinisinicosi][px′′py′′]
2.2 Derivation of translational coordinate system
The derivation process of translation is much simpler, as shown in the figure below,
it is simple addition and subtraction:
p x ′ ′ = p x − o ′ x p y ′ ′ = p y − o ′ y px''\;=\;px\;-\;o'x\\py''\;=\;py\;-\;o'y px′′=px−o′xpy′′=py−o′y
2.3 Complete formula
Substituting the formulas in the above two sections into the complete formula:
px ′ = ( px − o ′ x ) ⋅ cos θ + ( py − o ′ y ) ⋅ sin θ py ′ = − ( px − o ′ x ) ⋅ sin θ + ( py − o ′ y ) ⋅ cos θ px'\;=\;(px\;-\;o'x)\cdot\cos\theta\;\;+\;(py\ ;-\;o'y)\cdot\sin\theta\;\\\; py'\;=-\;(px\;-\;o'x)\cdot\sin\theta\;+\; (py\;-\;o'y)\cdot\cos\theta\;\;px′=(px−o′x)⋅cosi+(py−o′y)⋅sinipy′=−(px−o′x)⋅sini+(py−o′y)⋅cosi
Write a C language program based on this formula later
3 C language programming
First, analyze the requirements of this program. The input is the coordinates of a point in the old coordinate system, and the position and rotation angle of the new coordinate system relative to the old coordinate system. The output is the coordinates of the point in the new coordinate system.
Here, the blogger designs a simple function to implement this algorithm.
#include <stdio.h>
#include <math.h>
#define PI 3.1415926F
typedef struct Point_Tag
{
float X;
float Y;
} Point_Type;//点结构体
typedef struct CoordinatePosition_Tag
{
float deltaX;
float deltaY;
float theta;
} CoordinatePosition_Type;//新坐标系相对旧坐标系位置结构体
//坐标系变换函数
void coordinate_transformation(const Point_Type* const point_old_system_sp,
const CoordinatePosition_Type* const coordinate_position_sp,
Point_Type* const point_new_system_sp)
{
point_new_system_sp->X = (point_old_system_sp->X - coordinate_position_sp->deltaX) * cosf(coordinate_position_sp->theta) +
(point_old_system_sp->Y - coordinate_position_sp->deltaY) * sinf(coordinate_position_sp->theta);
point_new_system_sp->Y = -(point_old_system_sp->X - coordinate_position_sp->deltaX) * sinf(coordinate_position_sp->theta) +
(point_old_system_sp->Y - coordinate_position_sp->deltaY) * cosf(coordinate_position_sp->theta);
}
//main函数中简单测试
int main()
{
Point_Type point_old_system;
CoordinatePosition_Type coordinate_position;
Point_Type point_new_system;
point_old_system.X = 2.0F;
point_old_system.Y = 2.0F;
coordinate_position.deltaX = 0.0F;
coordinate_position.deltaY = 0.0F;
coordinate_position.theta = (45.0F/180.0F*PI);
coordinate_transformation(&point_old_system,&coordinate_position,&point_new_system);
printf("NEW_X = %f,NEW_Y = %f", point_new_system.X,point_new_system.Y);
}
The code implementation is relatively simple, that is, the formula is translated into C code. Three parameters are passed in the function function, the first two pointers are used to input the position of the point in the old coordinate system, and the position and angle of the new coordinate system in the old coordinate system. The third pointer is used to get the output result.