ArUco 마커 인식

The Elder Node·6일 전

ROS 2 aruco camera_info intrinsic matrix opencv

로봇팔 픽앤플레이스: ROS 2를 이용한 시뮬레이션

목록 보기

10/14

영상에서 마커를 찾는 노드 만들기

카메라 영상에서 ArUco 마커를 찾아 영상 안의 픽셀 위치를 구하는 노드를 만드는 과정이다.

arm_vision 패키지를 새로 생성하고 영상 구독 작업까지의 소스 내용은 커밋 242fe09, 마커 검출은 999578d 에 있다.

OpenCV 사용

ROS 2에서 OpenCV는 ROS 2 패키지를 통해서 사용하는 것이 아니라 OpenCV 함수를 직접 호출해서 이미지를 처리하는 방식이다. 따라서 OpenCV를 쓰려면 개발용 라이브러리(헤더와 라이브러리 파일)가 시스템에 설치되어 있어야 한다.

그런데 이 시스템 패키지는 OS마다 이름과 설치 방식이 다르다. 이를 도와주는 ROS 도구가 rosdep이다. rosdep은 직접 설치하지 않고, package.xml에 적힌 의존성 항목을 각 OS의 패키지 이름으로 바꿔 apt, dnf 같은 시스템 패키지 관리자로 설치한다. (rosdep ROS 2 documentation).
의존성 패키지 목록은 공식 저장소인 https://github.com/ros/rosdistro 의 rosdep/base.yaml 파일에서 확인할 수 있다.
OpenCV 패키지의 rosdep 의존성 이름은 libopencv-dev이며 Ubuntu와 Debian에서는 libopencv-dev로, Fedora에서는 opencv-devel로, Arch에서는 opencv로 바뀌어 각 OS의 패키지 설치 도구로 설치된다.

CMake에서는 자체적인 의존성 추가 방법이 있는데 이것이 CMakeLists.txt의 find_package이다. find_package는 먼저 Module mode로 Find<이름>.cmake 파일을 찾고(CMAKE_MODULE_PATH, 그다음 CMake 설치본의 Find 모듈 순), 찾지 못하면 Config mode로 <이름>Config.cmake 또는 <소문자 이름>-config.cmake 파일을 찾는다 (CMake 3.28 find_package).
이 프로젝트에는 FindOpenCV.cmake가 없고, OpenCV 패키지가 /usr/lib/x86_64-linux-gnu/cmake/opencv4/OpenCVConfig.cmake를 설치하므로 Config mode에서 OpenCV라는 이름으로 찾는다.

find_package(OpenCV REQUIRED)

find_package로 OpenCV를 찾으면 헤더 경로와 라이브러리 목록이 OpenCV_INCLUDE_DIRS, OpenCV_LIBRARIES 변수로 설정된다. 이 변수로 실행 파일에 헤더 경로와 라이브러리를 연결한다.

target_include_directories(arm_vision PRIVATE ${OpenCV_INCLUDE_DIRS})
target_link_libraries(arm_vision ${OpenCV_LIBRARIES})

참고로 ROS 패키지처럼 직접적으로 헤더와 라이브러리를 연결하지 않고 ament_target_dependencies에 OpenCV를 추가하는 것만으로도 동일한 결과를 얻는다.

다음으로 고려해야 하는 사항은 OpenCV의 버전이다. 현재 사용중인 Podman 컨테이너의 OpenCV 버전은 4.6이므로 공식 문서나 예제를 참고할 때 주의해야 한다.

인식 결과 표시

ArUco 마커는 흑백 픽셀로만 검출 작업을 하기 때문에 영상을 전달 받을 때 mono8로 받는다. 그런데 그 영상에 검출 결과를 표시하기 위해 cv::aruco::drawDetectedMarkers(image, corners, ids)로 결과를 그리려면 흑백이 아닌 다른 색상이 필요하다.

void drawDetectedMarkers(InputOutputArray image, InputArrayOfArrays corners,
                         InputArray ids = noArray(),
                         Scalar borderColor = Scalar(0, 255, 0));

기본 테두리 색은 BGR 순서의 초록 Scalar(0, 255, 0)이다. 검출은 흑백 그대로 하고, 표시용 복사본만 컬러로 바꿔 그렸다.

cv::Mat display;
cv::cvtColor(cv_const_ptr->image, display, cv::COLOR_GRAY2BGR);
cv::aruco::drawDetectedMarkers(display, corners, ids);

ArUco 마크 검출 결과

초록 사각형이 마커 테두리, 파란 글자가 마커 번호, 왼쪽 위 빨간 사각형이 첫 번째 꼭짓점이다. 검출기는 마커의 비트를 읽어 마커의 회전을 알아낸 뒤 마커 무늬의 왼쪽 위 모서리를 첫 번째 꼭짓점으로 알려주기 때문에 이를 이용해서 마커의 방향을 알 수 있다.

검출 위치 확인

다수의 마커가 있는 경우 검출됐다는 로그만으로는 그게 우리가 찾으려는 보드의 마커인지 알 수 없다. 이러한 상황을 가정하여 마커 ID와 꼭짓점 픽셀 좌표를 계산값과 대조해 보았다.

camera_info

영상이 투영된 2차원 픽셀 좌표 계산을 위해서는 카메라가 발행하는 camera_info의 K 행렬이 필요하다.

$ ros2 topic echo /overhead_camera/camera_info --once
...
height: 720
width: 1280
distortion_model: plumb_bob
d: [0.0, 0.0, 0.0, 0.0, 0.0]
k: [1758.7457275390625, 0.0, 640.0,
    0.0, 1758.7458229064941, 360.0,
    0.0, 0.0, 1.0]

d는 렌즈 왜곡 계수다. 실제 카메라는 렌즈의 형상 때문에 영상이 휘어지므로 체커보드 같은 보정판으로 캘리브레이션해서 이 계수를 구하고, 왜곡된 카메라 영상을 펴는 데 사용한다. Gazebo 카메라도 SDF의 <distortion> 요소로 왜곡을 흉내 낼 수 있지만, 지금 시뮬레이션 카메라에는 지정하지 않았으므로 계수가 모두 0이다.

카메라 좌표계의 3차원 점을 2차원 이미지 픽셀 좌표로 투영

K는 카메라 내부 행렬(camera intrinsic matrix)이라 부르는데 카메라 좌표계의 3차원 점을 이차원 이미지의 픽셀 좌표로 투영할 때 사용하는 행렬이다. (https://github.com/ros2/common_interfaces/blob/jazzy/sensor_msgs/msg/CameraInfo.msg)

# Intrinsic camera matrix for the raw (distorted) images.
#     [fx  0 cx]
# K = [ 0 fy cy]
#     [ 0  0  1]

OpenCV의 공식 문서에 잘 설명되어 있다(https://docs.opencv.org/4.6.0/d9/d0c/group__calib3d.html#:~:text=Detailed%20Description). 해당 문서에는 camera intrinsic matrix가 A로 표기되어 있다.

Pinhole camera model

왜곡이 없는 핀홀 카메라의 투영 변환은 아래와 같이 표현된다.

$sp=A[R|t]P_w$

여기서 $P_w$ 는 월드 좌표계 관점에서 표현된 3차원상의 한 점이다.
$p$ 는 이미지 평면상의 2차원 픽셀이며, A는 카메라 내부 파라미터 행렬(Intrinsic Matrix), $R$ 과 $t$ 는 월드 좌표계에서 카메라 좌표계(또는 카메라 프레임)로의 변환을 나타내는 회전(Rotation)과 이동(Translation)이다.

카메라 내부 파라미터 행렬 $A$ (또는 $K$ )는 카메라 좌표계의 3차원 점을 2차원 픽셀 좌표로 투영한다. $Pc$ 는 카메라 좌표계로 변환된 3차원 상의 한 점이다( $[R|t]P_w$ 계산 후의 3차원 좌표).

p = A P_c

내부 행렬 $A$ 는 픽셀 단위의 초점 거리 $f_x, f_y$ 와 이미지 중심 부근의 주점(主點, Principal Point) $(c_x, c_y)$ 로 구성된다.

A = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}

따라서 3차원 점 $(X_c, Y_c, Z_c)$ 와 2차원 픽셀 좌표 $(u, v)$ 사이의 투영 관계는 다음과 같은 행렬식으로 표현된다.

s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}

여기서 s는 사영 변환의 스케일 팩터(scale factor)이다. 행렬곱을 전개하면 세 번째 행에서 s = Z_c가 되며, 이는 카메라 광학 축(Z축) 방향으로 내려다 봤을 때 대상 점이 있는 평면까지의 거리이다.

3차원 점이 픽셀이 되는 과정은 ROS image_pipeline 문서의 Derivation Diagram에 그림으로 나와 있다. 네모 친 K, D, R, P가 camera_info에 들어 있는 값이다.

Derivation Diagram

윗줄을 오른쪽에서 왼쪽으로 따라가면 원본 영상으로 투영하는 과정이다.
(아랫줄은 왜곡을 펴거나 스테레오 영상을 맞춘 보정 영상을 만드는 경로)
광학 좌표계의 점 X'를 깊이 Z로 나눠, 카메라 앞 거리 1인 정규화 평면에 찍힌 위치(X/Z, Y/Z)를 구한다. 그런 다음 왜곡 계수 D로 점을 휘어진 위치로 옮긴다. K로 초점거리를 곱하고 주점을 더해 픽셀 좌표를 얻는다.

OpenCV의 행렬식에 적용해 보면 다음과 같은 계산식이 나온다.

s·u = fx·X + cx·Z
s·v = fy·Y + cy·Z
s   = Z

행렬식의 세 번째 줄에서 s = Z이므로, 위 두 줄을 Z로 나누면 다음과 같이 픽셀 좌표를 얻을 수 있다.

u = cx + fx · X / Z
v = cy + fy · Y / Z

계산과 관측

월드 좌표계에서 카메라는 (0.5, 0.5, 1.0), 보드는 (0.4, 0.6)에 있고 마커 윗면은 카메라에서 0.995m 떨어져 있다. 보드는 카메라 기준으로 base_link의 x 방향으로 −0.1, y 방향으로 +0.1 떨어져 있다. 카메라 TF로 확인한 축 방향은 영상 오른쪽(광학 x)이 base_link의 −y, 아래쪽(광학 y)이 −x다. 그래서 광학 좌표계에서 보드는 X = −0.1, Y = +0.1, Z = 0.995에 있다.

핀홀 카메라에서 광축에 수직인 평면 위의 점들은 모두 같은 Z를 가진다. 이때 Z는 카메라에서 점까지의 직선 거리가 아니라 광학 좌표계의 원점(핀홀 위치)에서 광축 방향으로 영상이 맺히는 평면까지의 거리이다.

현재 시뮬레이션 상황은 카메라가 바로 위에서 내려다보고 있고 보드가 작업대 위에 평평하게 놓여 있어서 마커 면이 광축에 수직이다. 따라서 윗변의 양 끝은 깊이 Z가 같고 X만 마커 한 변 길이인 0.027m만큼 차이가 나므로, 두 점의 u 차이는 fx × 0.027 / Z다.

검출 노드가 출력한 로그는 이렇다. 마커 번호, 첫 번째와 두 번째 꼭짓점이다. 보드를 회전시키지 않았으므로 두 꼭짓점은 마커 윗변의 양 끝이다.

0 [440, 513] [486, 513]

항목	계산	관측
마커 중심 x	640 − 0.1 × 1758.75 ÷ 0.995 = 463.2	(440 + 486) ÷ 2 = 463
마커 중심 y	360 + 0.1 × 1758.75 ÷ 0.995 = 536.8	513 + 46 ÷ 2 = 536
마커 한 변	0.027 × 1758.75 ÷ 0.995 = 47.7 px	486 − 440 = 46 px

관측 중심 y는 마커가 회전하지 않은 정사각형이라고 보고 윗변에 한 변의 절반을 더한 값이다. 중심이 맞으므로 카메라 위치와 자세, 초점거리, 보드 배치가 일치한다.

하지만, 계산상 한 변은 47.7픽셀이 나와야 하는데 46픽셀이 나왔다. 위치를 구하는 데는 문제가 없지만, 마커가 영상에서 차지하는 크기로 거리를 계산하는 자세 추정에서는 이 차이만큼 거리가 길게 나온다(마커 한 변의 픽셀 수 = fx × 0.027 / Z에서 픽셀 수가 작으면 Z가 커진다).

원인으로 생각할 수 있는 부분은 OpenCV의 검출 파라미터의 꼭짓점 계산 방식 설정이다. 기본값이 CORNER_REFINE_NONE이라 꼭짓점 좌표가 정수값으로 계산되므로 양쪽 끝에서 1픽셀 안팎으로 어긋날 수 있어서
CORNER_REFINE_SUBPIX로 변경하고 다시 측정해 봤다.

0 [439.285, 513.004] [486.003, 512.989]

축 길이를 픽셀로 계산하면 486.003 − 439.285 = 46.718px이고 이를 광축 깊이로 변환하면 약 1.016이 나온다.
노드가 실제 계산한 위치의 Z값은 아래 출력과 같이 1.017이 나오므로 계산 결과와 마커 인식 결과가 거의 일치한다.

Marker ID: 0, Position: [-0.103, 0.102, 1.017]

결과값으로 봤을 때 보드 높이 오차(0.022m)는 여전히 크다.

오차 보정

카메라 좌표 [-0.103, 0.102, 1.017]을 base_link 기준으로 바꾸면 (0.398, 0.603)이다. 광학 좌표계의 x가 base_link의 -y, y가 -x이므로 x = 0.5 − 0.102, y = 0.5 + 0.103이다. 보드가 (0.4, 0.6)에 있으니 옆방향 오차는 x가 2.0mm, y가 2.5mm, 대각선 3.2mm다. 마커 인식 코드를 작성하고 있던 시점에서 시뮬레이션의 그리퍼는 최대 너비가 50mm였다. 보드 짧은 변이 40mm라 중심을 정확히 측정했다 하더라도 좌우로 각각 5mm의 여유밖에 남지 않는다.

지금 당장 오차를 더 줄이는 것은 힘들 것으로 생각되어 개방폭을 70mm로 넓혔다.

<origin xyz="0 0.04 0.05" rpy="0 0 0"/>
<limit lower="0.0" upper="0.035" effort="20" velocity="0.3"/>

두 면이 맞닿는 지점인 upper도 0.025에서 0.035로 맞췄다. 접촉까지 움직이는 거리가 5mm에서 15mm로 늘어서 velocity도 0.1에서 0.3으로 올렸다.(어떤 영향이 있는지는 확인하지 않았음)

높이를 이미 아는 값으로 고정

높이 방향 오차 22mm는 보상할 방법이 마땅치 않아서 고정값을 쓰는 것으로 결정하였다. 시뮬레이션 환경을 고정된 평판 위에서 작업하는 것으로 가정하였기 때문에 높이가 바뀔 이유가 없다. 높이까지 카메라로 얻어 내려면 stereo camera나 depth camera로 바꿔야 한다. 그렇게 되면 센서 설정과 검출 코드를 모두 변경해야 하기 때문에 시간이 더 걸리게 되고 시뮬레이션을 통한 개념 학습 목적에는 부합하지 않는다고 생각해서 진행하지 않았다.