From b6fca43a42dd8d900cdb378da30f2d258a569c97 Mon Sep 17 00:00:00 2001 From: lxowalle Date: Mon, 16 Dec 2024 18:33:47 +0800 Subject: [PATCH] * update caculate distance with apriltag --- docs/doc/en/vision/apriltag.md | 139 +++++++++++++++++++++++++++++++++ docs/doc/zh/vision/apriltag.md | 134 ++++++++++++++++++++++++++++++- 2 files changed, 272 insertions(+), 1 deletion(-) diff --git a/docs/doc/en/vision/apriltag.md b/docs/doc/en/vision/apriltag.md index 62e91ec1..a8fa7cb9 100644 --- a/docs/doc/en/vision/apriltag.md +++ b/docs/doc/en/vision/apriltag.md @@ -125,3 +125,142 @@ Here are explanations for common parameters. If you can't find parameters to imp | families | Apriltag label family type | Scan for labels from the TAG36H11 family:
```img.find_apriltags(families = image.ApriltagFamilies.TAG36H11)``` | This article introduces common methods. For more API information, please refer to the [image](../../../api/maix/image.md) section of the API documentation. + +### Measuring Distance Between Camera and Object + +This section describes a method to estimate the distance using the formula `distance = k / width`, where: +`distance`: Distance between the camera and the object in millimeters (mm). +`k`: A constant. +`width`: Width of the object in the image, measured in pixels. + +The process consists of two steps: + +1. Measure the constant coefficient `k`. +2. Calculate the distance between the camera and the object using the constant and the object's `width`. + +#### Preparation + +1. AprilTag paper for calibration. +2. A ruler (or other measuring tool). + +#### Measuring the Constant Coefficient `k` + +- Fix the AprilTag in place and set the camera (maixcam) at a distance of 20 cm from the tag. + +- Use `maixcam` to detect the `AprilTag` and calculate the tag's `width`. Refer to the following code: + + ```python + from maix import camera, display + import math + + ''' + x1, y1, x2, y2: Coordinates of two points defining the tag's width, typically obtained using the corners() method. + Returns the width of the tag in pixels. + ''' + def caculate_width(x1, y1, x2, y2): + return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) + + cam = camera.Camera(160, 120) + disp = display.Display() + + while 1: + img = cam.read() + + apriltags = img.find_apriltags() + for a in apriltags: + corners = a.corners() + + # Calculate width using two horizontal corner points + width = caculate_width(corners[0][0], corners[0][1], corners[1][0], corners[1][1]) + # Print the detected width of the AprilTag + print(f'apriltag width:{width}') + disp.show(img) + ``` + +- Calculate the constant `k`: + + ```python + ''' + width: Width of the AprilTag detected at a known distance. + distance: The actual distance to the AprilTag during detection, in mm. + Returns the constant coefficient. + ''' + def caculate_k(width, distance): + return width * distance + + # Example: At a distance of 200 mm, the tag width is detected as 43 pixels + k = caculate_k(43, 200) + ``` + +#### Calculate Distance Between Camera and Object Using `k` + +```python +''' +width: Width of the AprilTag in pixels. +k: Constant coefficient. +Returns the distance between the camera and the object in mm. +''' +def caculate_distance(width, k): + return k / width + +distance = caculate_distance(55, 8600) +``` + +#### 完整的代码参考: + +```python +from maix import camera, display, image +import math + +''' +x1, y1, x2, y2: Coordinates of two points defining the tag's width, typically obtained using the corners() method. +Returns the width of the tag in pixels. +''' +def caculate_width(x1, y1, x2, y2): + return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) + +''' +width: Width of the AprilTag detected at a known distance. +distance: The actual distance to the AprilTag during detection, in mm. +Returns the constant coefficient. +''' +def caculate_k(width, distance): + return width * distance + +''' +width: Width of the AprilTag in pixels. +k: Constant coefficient. +Returns the distance between the camera and the object in mm. +''' +def caculate_distance(width, k): + return k / width + + +cam = camera.Camera(192, 108) +disp = display.Display() + +# Example: At a distance of 200 mm, the tag width is detected as 43 pixels +k = caculate_k(43, 200) + +while 1: + img = cam.read() + + apriltags = img.find_apriltags() + for a in apriltags: + corners = a.corners() + for i in range(4): + img.draw_line(corners[i][0], corners[i][1], corners[(i + 1) % 4][0], corners[(i + 1) % 4][1], image.COLOR_GREEN) + + # Calculate width using two horizontal corner points + width = caculate_width(corners[0][0], corners[0][1], corners[1][0], corners[1][1]) + + # Calculate distance + distance = caculate_distance(width, k) + + print(f'apriltag width:{width} distance:{distance} mm') + + disp.show(img) + +``` + +This method uses the width of the `AprilTag` to calculate the distance. It can also be extended to use height for distance calculation. However, note that this approach provides an estimate of the distance, and slight inaccuracies may occur due to real-world factors. \ No newline at end of file diff --git a/docs/doc/zh/vision/apriltag.md b/docs/doc/zh/vision/apriltag.md index cad56b7a..6faaa2f5 100644 --- a/docs/doc/zh/vision/apriltag.md +++ b/docs/doc/zh/vision/apriltag.md @@ -101,7 +101,7 @@ while 1: for i in range(4): corners[i][0] = int(corners[i][0] * x_scale) corners[i][1] = int(corners[i][1] * y_scale) - + # 显示 for i in range(4): img.draw_line(corners[i][0], corners[i][1], corners[(i + 1) % 4][0], corners[(i + 1) % 4][1], image.COLOR_RED) @@ -129,3 +129,135 @@ while 1: +### 测量摄像头与物体的距离 + +这里提供一种使用`distance=k/width`的公式来测距, 其中`distance`是摄像头和物体的距离,单位`mm`, `k`是一个常量, `width`是物体在画面中的宽度,单位是像素点. + +测量方法分两步: 1. 测量常量系数k; 2. 通过常量系数和标签宽度来计算物体与摄像头的距离 + +#### 前期准备 + +1. `apriltag`标签纸 +2. 尺子(或其他测距工具) + +#### 测量常量系数k + +- 将`apriltag`标签纸固定,并在距离`apriltag`标签20cm处固定`maixcam` + +- 使用`maixcam`检测`apriltag`标签并计算标签的宽度, 参考代码: + + ```python + from maix import camera, display + import math + + ''' + x1,y1,x2,y2: apriltag宽度的两点坐标, 一般通过corners()方法获取 + 返回标签的宽度,单位为像素点 + ''' + def caculate_width(x1, y1, x2, y2): + return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) + + cam = camera.Camera(160, 120) + disp = display.Display() + + while 1: + img = cam.read() + + apriltags = img.find_apriltags() + for a in apriltags: + corners = a.corners() + + # 通过水平方向的两个坐标点计算宽度 + width = caculate_width(corners[0][0], corners[0][1], corners[1][0], corners[1][1]) + # 打印apriltag标签的实际宽度 + print(f'apriltag width:{width}') + disp.show(img) + ``` + +- 计算常量系数k + + ```python + ''' + width: 当距离为distance时,检测到apriltag标签的宽度 + distance: 检测apriltag标签时距离apriltag标签的实际距离, 单位mm + 返回常量系数 + ''' + def caculate_k(width, distance): + return width * distance + + # 距离200mm时检测到标签宽度为43个像素 + k = caculate_k(43, 200) + ``` + +#### 通过常量系数计算摄像头和物体间的距离 + +```python +''' +width: apriltag标签的宽度 +k: 常量系数 +返回摄像头与物体的距离,单位mm +''' +def caculate_distance(width, k): + return k / width + +distance = caculate_distance(55, 8600) +``` + +#### 完整的代码参考: + +```python +from maix import camera, display, image +import math + +''' +x1,y1,x2,y2: apriltag宽度的两点坐标, 一般通过corners()方法获取 +返回标签的宽度,单位为像素点 +''' +def caculate_width(x1, y1, x2, y2): + return math.sqrt((x2 - x1)**2 + (y2 - y1)**2) + +''' +width: 当距离为distance时,检测到apriltag标签的宽度 +distance: 检测apriltag标签时距离apriltag标签的实际距离, 单位mm +返回常量系数 +''' +def caculate_k(width, distance): + return width * distance + +''' +width: apriltag标签的宽度 +k: 常量系数 +返回摄像头与物体的距离,单位mm +''' +def caculate_distance(width, k): + return k / width + + +cam = camera.Camera(192, 108) +disp = display.Display() + +# 距离200mm时检测到标签宽度为43个像素 +k = caculate_k(43, 200) + +while 1: + img = cam.read() + + apriltags = img.find_apriltags() + for a in apriltags: + corners = a.corners() + for i in range(4): + img.draw_line(corners[i][0], corners[i][1], corners[(i + 1) % 4][0], corners[(i + 1) % 4][1], image.COLOR_GREEN) + + # 通过水平方向的两个坐标点计算宽度 + width = caculate_width(corners[0][0], corners[0][1], corners[1][0], corners[1][1]) + + # 计算距离 + distance = caculate_distance(width, k) + + print(f'apriltag width:{width} distance:{distance} mm') + + disp.show(img) + +``` + +上面的方法是通过`apriltag`的宽度计算距离, 同样也可以扩展为使用高度来计算距离. 但需要注意该方法是在对距离估测, 实际应用中会有些许误差存在.