마커인식한 후 로봇 위치 인식,조정하기[작성중]

이전 블로그

2021.06.30 - [ROS] - Webcam으로 Aruco 마커 인식하기

 

마커를 인식했으면 카메라로부터 마커의 위치를 알 수 있습니다.

그럼 이를 반대로 하여 카메라의 위치도 알 수 있죠.

제가 해볼 것은 로봇이 마커를 인식하면 마커를 정면으로 바라보도록 위치시키는 것입니다.

 

마커를 정면으로 보도록 하려면 어떤 조건이 필요할까요

먼저 x좌표가 원점과 최대한 가까워야 하고 마커의 x축과 카메라의 x축이 평행해야 한다고 생각합니다.

 

정면을 볼뿐만 아니라 마커와 어느 정도 가까워지도록 해야 하기 때문에 z좌표도 정해주겠습니다.

 

로봇이 마커를 찾으면 마커의 위치를 알 수 있는데 로봇 입장에서는 로봇이 마커를 찾으면 마커로부터 로봇이 어느 정도 위치에 있는지 알 수 있습니다. 그 위치를 어떻게 뽑아낼 수 있을까요

 

로봇의 위치를 알고 조종한다는 것은 x좌표가 0이면 로봇이 정면이다 이게 아니라 x좌표가 0이 될 때까지 로봇이 움직여야 한다입니다.

 

로봇의 rotation vector와 translation vector를 출력하고 translation의 x좌표인 첫 번째 숫자가 0.01m 사이에 카메라가 들어오면 --------------를 출력하도록 하였습니다.

 

 

**x좌표를 출력할 때 발생한 오류**

tvec 행렬을  tvec.shape 하면 1x1x3 행렬이라고 떠서 그냥 tvec [0]을 출력하라고 하니까 출력되지 않았던 것입니다.

tvec [0][0][0]으로 행렬을 맞춰주니까 잘 나오네요.

 

 

이제는 0점 가까이에 위치할 수 있도록 로봇이 움직여야 될 방향을 알려주기 위해 화살표를 그려보도록 하겠습니다.

 

 

빨간색이 x 축인데 x축의 원점이 카메라의 왼쪽으로 치우쳐져 있으면 왼쪽 화살표를 오른쪽으로 치우쳐져 있으면 오른쪽 화살표를 그리도록 하였습니다.

 

위 과정은 ros환경이 아닌 python코드를 사용한 것으로 제대로 ros를 사용하고 있지는 않았어요.

그럼 ros환경에서 마커를 인식해보겠습니다.

과정은 usb camera를 이용해서 ros img를 받아오고 cv.bridge를 통해 opencv이미지로 변환하여 마커를 인식하는 것입니다.

 

먼저 아래 명령어로 usb camera를 깔아야 합니다.

 

sudo apt-get install ros-melodic-usb-cam

 

그다음 아래 명령어를 통해 usb cam을 실행시켜 subscriber에 사용할 토픽 이름을 찾습니다.

 

$ rosrun usb_cam usb_cam_node

 

**usb cam 실행 오류**

 

 

위 오류는 다음 과정으로 해결할 수 있습니다.

 

$ ls -al
$ sudo umount /home/사용자계정/.gvfs
$ sudo chmod 777 .gvfs

 

위 명령어들을 실행시킨 후 다시 rosrun을 실행시키면 실행될 겁니다.

 

 

rqt_graph로 topic을 확인한 결과입니다.

 

rqt_image_view를 이용하여 화면이 제대로 실행이 되는지 확인해보겠습니다.

 

 

상단에 적힌 /usb_cam/image_raw를 subscriber의 topic으로 받아오면 됩니다.

 

먼저 usb cam으로 rospy를 써서  image를 받아오는 거 먼저 해보겠습니다.

아래 코드로 usb cam을 설치합니다.

 

$ sudo apt-get install ros-melodic-usb-cam

 

다음으로 할 거는 이미지를 불러와서 현재 코드에서 videocapture와 while문을 사용하지 않고 cvbridge로 opencv를 작동시키는 것입니다. cvbridge는 아래 그림과 같은 역할을 합니다.

 

 

ros image를 불러와서 opencv와 연결할 수 있도록 도와줍니다.

 

먼저 cvbridge를 이해하기 위해서 roswiki에 있는 예제를 실행시켜보려고 합니다.

roswiki 사이트 : http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython

catkin_make에서 opencv2 오류가 발생해서 cvbridge 예제를 못하고 있는 중입니다...

 

 

opencv2의 path를 찾지 못해서 나는 오류라고 하는데 버전은 있으면서 package는 왜 없는지 모르겠네요..

먼저 제가 가지고 있는 opencv버전이 무엇인지 확인해보았습니다.

 

 

예제에서 패키지를 만들 때 의존성으로 opencv2를 사용하라고 했는데 제가 가지고 있는 것은 그냥 opencv였습니다.

 

 

제 노트북에 있던 opencv폴더의 이름이 OpenCV였기 때문에 CMakeLists.txt 파일에 의존성 이름을 아래와 같이 바꾸니까 catkin_make오류는 이제 뜨지 않네요. opencv2 오류가 완벽하게 해결된 건지는 모르겠습니다.

 

 

**또 다른 오류 해결법**

알고 보니 wiki사이트 맨 아래에 kinetic 버전 이후로는 의존성 패키지에 opencv를 빼도 된다고 적혀있더라고요.

안 적어도 실행이 됩니다!  저는 나중에 발견했네요ㅠㅠ

 

 

코드를 작성할 때는 publisher로 rospy를 import 하여 이미지를 받아오고 subscriber의 callback 함수에서 원래 있던 코드의 while 문을 실행시키면 됩니다.

 

 

cv_example.py 코드를 실행시켰을 때 이렇게 나왔는데 제대로 나온 건지 잘 모르겠네요.

위 rqt_graph는 카메라 노드를 실행시키지 않았을 때입니다.

 

 

실행시키니까 위처럼 print 하라고 한 거만 뜨고 실행이 되지 않길래 뭔가 싶었는데 topic을 잘못 적었더라고요.

바꾸고 나니까 node가 하나로 나오는데 이걸 publisher랑 subscriber각각 다른 이름으로 나오게 하고 싶은데 그리고 converter는 중간에 있어야 하는 게 아닌가 싶은데 어떻게 하는 걸까요

 

 

코드에서 rospy.init_node()는 노드의 이름을 한 번만 지정할 수 있게 해 주더라고요. 만약 여러 노드를 쓰려면 코드를 나눠서 실행을 해야 할 거 같아요. 그럼 주는 거 이름을 publisher로 하고 받는 거 이름을 subscriber로 하면 converter는 어디에 들어가야 하는 걸까요.. 

 

제가 아예 publisher와 subscriber 개념을 이해를 잘못하고 있었던 것 같아요.

굳이 코드를 나눌필요도 없고 한 코드에서 실행하면 되는데 주는 노드가 usb cam이니까 publisher는 이름이 usb cam으로 정해져 있고 받는 노드의 이름만 지정해주면 되는데 그것이 image_converter였습니다.

 

cv_example 코드를 실행시켜보려고 하는데 왜 카메라 창이 뜨지 않는 걸까요.

cv2.imshow 에서 잘못 불러온 건가

 

위 오류는 callback 함수 자체를 못 들어가는 거였습니다. 실수로 camera node를 안 키고 실행시켰더라고요ㅋㅋㅋㅋ

그런데 callback함수에 들어가고 나서 또 오류가 발생했는데 이는 CompressedImage에서 Image로 import를 바꿔주니까 해결이 됐습니다.

 

 

잘못된 영상

 

ros 이미지를 opencv이미지로 전환하여 영상을 내보내도록 하였는데 왜 이렇게 영상이 끊기는 걸까요

토픽을 받아오는데 시간이 걸려서 그런가

아래 그림은 위 영상처럼 나타날 때의 rqt_graph입니다.

 

 

해결 완료!

 

오류 해결한 영상

 

publisher 랑 subscriber에 토픽 이름이 같아서 그런 거였습니다.

받아오는 이미지가 같으니까 보여주는 이미지가 시간차로 계속 끊겨서 보이는 거죠

publisher 이름은 그냥 내 마음대로 지으면 되는 거였습니다. 받아오는 subscriber 이름만 카메라 토픽으로 써주면 되는 거였어요

 

 

이거였습니다! 이미지 변환 성공!!

 

그럼 이제 실시간 영상에서 마커 인식을 해보겠습니다.

 

 

마커 인식하는 코드와 이미지 변환해주는 코드를 합쳐서 ros 환경에서 마커 인식을 하였습니다.

 

 

그런데 로봇이 정면을 보고 있을 때 화살표를 표시하고 싶은데 위 영상을 보면 정면을 보지 않아도 화살표 방향을 표시하는 것을 볼 수 있습니다. 정면을 보도록 하고 싶으면 어떻게 해야 할까요... 축을 평행하게 만드는 방법이 뭘까요

방법은 마커의 xy평면과 카메라의 xy평면을 평행하도록 하는 것입니다.

블로그 맨 아래 질문란에 자세히 적혀있습니다.

 

 

rvec 행렬에 값 순서대로 x, y, z 축으로 얼마만큼 회전했는지 라디안 각도를 의미합니다.

마커의 x축이 180도 돌면 마커의 xy평면과 카메라의 xy평면이 평행하게 됩니다.  이를 조건으로 사용하여 rvec의 0번 값은 3~3.2 사이로 오고 1번 값은 0~0.02 사이로 오도록 하고 tvec에 x, y 값이 0에 가까우면 정면을 보는 것으로 인식하고 'front'라고 출력하도록 하였습니다.

카메라가 로봇에 부착될 것이기 때문에고 rvec에 z는 0도와 가까울 것으로 생각하여 조건을 넣지 않았습니다.

 

다음으로 할 것은 마커의 위치로부터 카메라의 위치를 구해야 합니다.

-회전구하기

  1. 구한 rvec(회전벡터)를 cv2.Rodrigues 함수를 이용하여 회전행렬로 만들기 

  2. 그 행렬의 역행렬 구하기(전치)

  3. 마커에 대한 카메라의 회전 구하기 완료

 

-위치구하기

  1. 마커의 회전행렬과 위치벡터를 이용하여 4x4 동차변환행렬을 만들기

  2. 역동차변환행렬 구하기

  3. 그 중에 위치벡터만 뽑아오기

 

다음은 마커인식에 성공한 영상입니다.

 

https://youtu.be/D2ihFEbgVBo

 

이것은 제가 사용한 소스코드입니다.

https://github.com/yehjin00/ArUco-marker-detection

yehjin00/ArUco-marker-detection

 

주로 공장에서 로봇을 사용할 때 마커 인식을 통해 로봇의 위치를 인식하도록 하는데요. 그럼 공장 내부 곳곳에 마커가 부착되어있어야 하는데 마커의 크기가 크면 보기 싫지 않을까요? 저는 여 러크기의 마커를 부착한 후 카메라와의 거리에 따라 변화하는 인식 정도를 알아보았습니다.

 

마커의 크기를 줄이거나 카메라와의 거리를 멀게 하여 언제 마커가 가장 잘 인식이 되는가를 보았습니다.

이때까지 위에서 사용한 마커의 크기는 100mm였습니다.

 

아래 측정에 사용된 마커 정보입니다.

 

마커 번호 : 100

마커 크기(mm) : 10, 30, 50

 

마커 크기가 100일 때는 상당히 먼 거리에서도 인식이 가능한 것을 확인하였습니다.

10일 때는 25cm 정도까지, 30일 때는 60cm 정도까지, 50일 때는 100cm 정도까지 인식은 가능한데 좌표축이 엄청 작게 나왔습니다.

측정 결과 거의 마커 크기에 두배 정도 거리까지 인식이 가능한 것 같습니다.

크기 10mm 거리 25cm                                               크기 30mm 거리 60cm                                        크기 50mm 거리 100cm   

 

거리가 멀어졌을 때 인식률을 확인해 봤으니까 다음으로 거리가 어느 정도 이상 가까워지면 인식이 되지 않는지 확인해 보겠습니다.

10일 때는 4cm 30일 때는 4cm로 거의 동일하게 나왔는데요.

가까운 정도는 창안에 마커가 다 들어오면 되는 거라서 로봇이 마커에 아예 밀착하지 않는 이상 웬만하면 마커를 인식할 수 있을 것 같습니다.

 

마커의 크기는 카메라를 로봇에 부착해본 후 마커를 발견하고 로봇이 마커를 정면으로 바라보기까지 움직일 수 있는 반경을 고려해서 정해야 할 것 같습니다.

 

마커에 대해 더 찾아보고 현재 사용되는 마커는 어떤 것 이 있는지 왜 그 마커가 사용되는지 등등 여러 장단점을 비교해 본 후 어떤 마커를 쓰는 것이 가장 좋을지 조사해보겠습니다.

 

또한 카메라의 위치도 고민을 해봐야 합니다.

로봇의 전면부가 어떤 축인 지 확인한 후 그 축에 카메라를 설치할 경우 오랫동안 마커를 볼 수 있습니다.

그 위치가 가장 좋은 위치 아닌가 생각합니다.

 

마커를 인식한 후 로봇이 움직이도록 해보겠습니다.

먼저 웹캠에서 마커의 위치를 뽑아내서 토픽으로 내보내보겠습니다.

orientation : 방향

 

 

 

아래 영상은 뽑아낸 위치를 이용해 마커와 카메라사이 거리와 축의 각도를 토픽으로 알려주는 것 입니다.

 

 

 

마커를 인식했으니 그 위치를 이용해 속도를 publish해서 로봇을 움직여 보겠습니다.

로봇은 실행시키지 않고 realsense로 마커를 인식하라고 했을 때 입니다.

 

 

 

로봇을 실행시켜본 결과 아래 두 영상과 같이 나왔는데 각도값을 변경하거나 축이 맞는지 다시 봐야할 것 같습니다.

5x5 사이즈의 마커를 인식하고 마커와 로봇이 정면을 보도록 cmd_vel를 publish한 결과입니다.

 

 

 

 

4x4 사이즈의 마커를 인식했을때 입니다.

 

 

 

위 launch를 실행했을때 rqt_grpah입니다.

 

 

카메라가 마커를 인식했을 때 반대로 움직이는 것을 확인한 후 로봇의 z축값에 마커로부터 구한 angle그대로의 값을 넣은 것이 아니라 -angle로 넣어봤습니다.

또한 저는 로봇과 마커사이를 본 것이 아니라 카메라에서 본 마커의 위치를 이용해 거리와 각도를 계산했습니다.

그랬더니 위 오류를 해결할 수 있었습니다. 이와 관련된 영상은 아래에 있습니다.

 

마커를 문에 붙이지 않고 마커가 움직이면 로봇이 따라가도록 해보았습니다.

 

 

 

 

마커가 멀리있을때, 각이 클때 로봇이 조금더 빨리 움직이도록 하였고 마커와 카메라사이 거리가 10cm정도 떨어지게되면 더이상 가까이 오지않도록 하였습니다.

 

줄자로 재봤을때 실제로 10cm 떨어져있는지 확인한 후 다시 글을 쓰도록 하겠습니다....

 

영상이 어두워지면 마커를 인식하지못해서 이를 HE(histogram equalization)을 통해 이미지대조비를 높혀서 인식률을 높여보았습니다.

제가 사용한 것은 CLAHE (Contrast Limited Adaptive Histogram Equalization)입니다.

HE를 사용하면 이미지전체가 밝아져서 영상이 잘 안보이는 부분이 발생할수있기때문에 CLAHE를 사용하여 clip limit(histogram의 높이를 제한하는 값)값이상이 되는 부분만 밝게 하는 방법을 사용했습니다.

 

이는 계산시간이 길다는 단점이 있는데 얼마나 느린지, 인식속도에 문제가 없는지 확인해보겠습니다.

 

 

질문

Q1. import image 랑 compressedimage 랑 차이점?

A1. compressed 가 압축한다는 뜻인데 받아오는 이미지를 압축해서 쓸 것인지 그대로 쓸 것인지 묻는 것입니다. 카메라가 여러 대여서 받아와야 하는 이미지가 많고 용량이 크다면 압축을 해서 풀어서 사용하는 경우가 있는데 저는 카메라가 한대라 image를 셔요 하였습니다. compressed 이미지를 토픽으로 받아왔는데 엔코딩이 잘못됐다는 오류가 떴었는데 해결하는 방법은 더 찾아봐야 할 것 같습니다.

 

Q2. CmakeList에 의존성으로 opencv2를 사용하라고 했는데 가지고 있는 것이 opencv라서 그냥 opencv를 사용하였는데 같은 건가요?

A2. 예전 버전이 아마 opencv2일 텐데 그냥 opencv를 써도 상관없습니다.

 

Q3. 지금은 x축 원점에 가까워졌을 때만 방향을 표시하였는데 정면을 보지는 않습니다. 카메라를 정면으로 보게 만들고 싶으면 어떤 조건이 들어가야 할까요?

A3. 정면을 보려면 마커의 xy평면과 카메라의 xy평면이 평행하다는 조건을 넣어줘야 하는데 이는 rvec 값을 조정하면 됩니다. 정면을 보고 싶을 때 마커가 카메라의 중앙에 왔으면 할 때 rvec를 (3.14 , 0, 0)로 만들어 주고 tvec를 (0,0, z)로 만들어 주면 됩니다. rvec는 라디안 값입니다. (좌표의 부호는 상관없습니다.)

 

 

참고 코드

https://deepdeepit.tistory.com/118

http://wiki.ros.org/rospy_tutorials/Tutorials/WritingImagePublisherSubscriber

http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython

https://blog.daum.net/pg365/295