[Monocular Ranging] How to measure distance when the slope of the ground is known

Article directory

I. Introduction
2. Ranging code
- 2.1. The ground has a slope
- 2.2. python code
- - 2.2.1. Rotation matrix rotation angle
  - 2.2.2. Angle to rotation matrix
  - 2.2.3. Three-dimensional rotation principle (Rotation principle)
  - 2.2.4. Complete code
- 2.3, c++ code

1. Preface

The previous blog [Monocular Ranging] How to measure distance when the camera angle is known is mentioned when the camera is not in an ideal state. In actual situations, such as when the camera is installed and there is an angle deviation, the camera needs to be calibrated. Perform calibration. At the same time, the multiple factors that affect the ranging error and the weight of each factor are also analyzed.

The above are all based on the fact that the ground is parallel to itself, when there is a slope on the ground, especially when there is an up and down slope. At this time, the ranging error will be very large. If there is a 1° slope, then the target ranged at 10 m has an error of 20 cm.

If we know the slope sigma of the ground in advance, we can correct the camera external parameters in real time to compensate for the error caused by this angle.

Below I will provide the code and rotation principle for real-time correction of camera external parameters. Let me take you through the mystery of the rotation matrix.

2. Ranging code

Let’s first review the previous ranging code

Input camera internal parameters, external parameters, and camera height to complete the calibration in advance.

The target pixel is found by the target detection bbox.

import numpy as np

h = 1.5 # The camera is 1.5m high from the ground
pitch = -0.023797440420123328 # radians
pixe_x, pixel_y = 888, 700 # Image pixel point, ground point
CameraMat = np.array([[1008, 0, 945],
                      [0, 1009, 537],
                      [0, 0, 1]]) # Camera internal parameters

R = np.array([[-0.0330564609, 0.0238237337, 0.999169505],
              [0.999452124, -0.000862625046, 0.0330863791, ],
              [0.00165014972, 0.999715802, -0.0237821659]]) # Rotation matrix
T = np.array([0, 0, -1.5])

sigma = np.arctan((pixe_y - CameraMat[1][2]) / CameraMat[1][1])
z = h * np.cos(sigma) / np.sin(sigma + pitch) # Depth
x_pixe, y_pixe = 2 * CameraMat[0][2] - pixe_x, 2 * CameraMat[1][2] - pixe_y # Select whether to perform central symmetry transformation according to the custom coordinate system
camera_x = z * (x_pixel / CameraMat[0][0] - CameraMat[0][2] / CameraMat[0][0])
camera_y = z * (y_pixel / CameraMat[1][1] - CameraMat[1][2] / CameraMat[1][1])
camera_z = z
distance_machine_direction = R[0][0] * camera_x + R[0][1] * camera_y + R[0][2] * camera_z + T[0] # Vertical distance
distance_transverse_direction = R[1][0] * camera_x + R[1][1] * camera_y + R[1][2] * camera_z + T[1] # Transverse distance
print(distance_machine_direction, distance_transverse_direction)

2.1. The ground has a slope

According to the previous analysis, if the ground has a slope, we should correct the camera external parameters in real time. How to do it is also very simple. That is to update our pitch angle and the camera’s external parameters in real time.

Our premise is that we need to know what the ground slope is. We will talk about how to obtain the ground slope later when we have the opportunity.

2.2, python code

Python converts from rotation matrix to angle, and from angle to transformation matrix, mainly using Rotation in the scipy library.

2.2.1. Rotation matrix rotation angle

import numpy as np
from scipy.spatial.transform import Rotation

r = np.array([-0.0517, -0.0611, 0.9968, 0.9987, 0.0011, 0.0519, -0.0042, 0.9981, 0.0609]).reshape(3, 3)
euler_r = Rotation.from_matrix(r).as_euler('zxy', degrees=False) # zxy is the external rotation order. degrees False displays radians, True displays angles
print(euler_r)

# [1.56967277 -0.0518037 1.50976086]

2.2.2, Angle to rotation matrix

from scipy.spatial.transform import Rotation

euler_r = [1.56967277, -0.0518037, 1.50976086]
new_r = Rotation.from_euler("zxy", [euler_r[0], euler_r[1], euler_r[2]], degrees=False).as_matrix()

2.2.3. Three-dimensional rotation principle (Rotation principle)

import numpy as np
from scipy.spatial.transform import Rotation


def get_r_matrix(str, alpha):
    sin = -np.sin(alpha)
    cos = np.cos(alpha)
    res = np.eye(3)
    if str == "z":
        res = np.array([[cos, sin, 0],
                        [-sin, cos, 0],
                        [0, 0, 1]])
    elif str == "y":
        res = np.array([[cos, 0, -sin],
                        [0, 1, 0],
                        [sin, 0, cos]])
    elif str == "x":
        res = np.array([[1, 0, 0],
                        [0, cos, sin],
                        [0, -sin, cos]])
    return res


euler_r = [1.56967277, -0.0518037, 1.50976086]
a, b, c = euler_r[0], euler_r[1], euler_r[2]

z = get_r_matrix("z", a)
x = get_r_matrix("x", b)
y = get_r_matrix("y", c)
mtx = y@x@z
mtx_1 = Rotation.from_euler("zxy", [a, b, c], degrees=False).as_matrix()
print(mtx, mtx_1) #The results are exactly the same

2.2.4, complete code

To sum up, it can be obtained

import numpy as np
from scipy.spatial.transform import Rotation

diff_pitch = -0.01 # Assume that the current ground slope is -0.01 radians
h = 1.5 # The camera is 1.5m high from the ground
pitch = -0.023797440420123328 # radians
pitch = pitch + diff_pitch
pixe_x, pixel_y = 888, 700 # Image pixel point, ground point
CameraMat = np.array([[1008, 0, 945],
                      [0, 1009, 537],
                      [0, 0, 1]]) # Camera internal parameters

original_r = np.array([[-0.0330564609, 0.0238237337, 0.999169505],
                       [0.999452124, -0.000862625046, 0.0330863791],
                       [0.00165014972, 0.999715802, -0.0237821659]]) # Rotation matrix
euler_r = Rotation.from_matrix(original_r).as_euler('zxy', degrees=False)
R = Rotation.from_euler("zxy", [euler_r[0], euler_r[1], euler_r[2] + diff_pitch], degrees=False).as_matrix()

T = np.array([0, 0, -1.5]) # Translation matrix

sigma = np.arctan((pixe_y - CameraMat[1][2]) / CameraMat[1][1])
z = h * np.cos(sigma) / np.sin(sigma + pitch) # Depth
x_pixe, y_pixe = 2 * CameraMat[0][2] - pixe_x, 2 * CameraMat[1][2] - pixe_y # Select whether to perform central symmetry transformation according to the custom coordinate system
camera_x = z * (x_pixel / CameraMat[0][0] - CameraMat[0][2] / CameraMat[0][0])
camera_y = z * (y_pixel / CameraMat[1][1] - CameraMat[1][2] / CameraMat[1][1])
camera_z = z
distance_machine_direction = R[0][0] * camera_x + R[0][1] * camera_y + R[0][2] * camera_z + T[0] # Vertical distance
distance_transverse_direction = R[1][0] * camera_x + R[1][1] * camera_y + R[1][2] * camera_z + T[1] # Transverse distance
print(distance_machine_direction, distance_transverse_direction)

2.3, c++ code

Knowing the three-dimensional rotation principle in 2.2.3, we can easily obtain new external parameters by using matrix multiplication.

 double pitchDiff = -0.01;
  cv::Mat initR = (cv::Mat_<double>(3,3) << -0.0330564609, 0.0238237337, 0.999169505,
                                             0.999452124, -0.000862625046, 0.0330863791,
                                             0.00165014972, 0.999715802, -0.0237821659); // Camera initial external parameters
  
  cv::Mat pitchR = (cv::Mat_<double>(3, 3) << cos(pitchDiff), 0, sin(pitchDiff), 0, 1, 0, -sin(pitchDiff), 0, cos(pitchDiff ));

  cv::Mat curR = pitchR * initR;