ROS-noetic ARマーカの位置検出

ros noetic環境で,ARマーカーの位置検出をしたいと思い,ARマーカを認識するROSパッケージをいくつか試した際のメモ.
最終的にうまく行ったのは`パッケージ その3`の`試した方法 2`に記載した本家のar_track_alvarにプルリクエストで上がっていたものでした.

https://github.com/pmusau17/ar_track_alvar/tree/noetic-devel

# パッケージ その１ `aruco_ros`
https://github.com/pal-robotics/aruco_ros

- install
```
sudo apt install ros-noetic-aruco-ros
```

- demo実行

arマーカ認識プログラムの実行
```
roslaunch aruco_ros single.launch
```

トピック一覧

```
/aruco_single/debug
/aruco_single/debug/compressed
/aruco_single/debug/compressed/parameter_descriptions
/aruco_single/debug/compressed/parameter_updates
/aruco_single/debug/compressedDepth
/aruco_single/debug/compressedDepth/parameter_descriptions
/aruco_single/debug/compressedDepth/parameter_updates
/aruco_single/debug/theora
/aruco_single/debug/theora/parameter_descriptions
/aruco_single/debug/theora/parameter_updates
/aruco_single/marker
/aruco_single/parameter_descriptions
/aruco_single/parameter_updates
/aruco_single/pixel
/aruco_single/pose
/aruco_single/position
/aruco_single/result
/aruco_single/result/compressed
/aruco_single/result/compressed/parameter_descriptions
/aruco_single/result/compressed/parameter_updates
/aruco_single/result/compressedDepth
/aruco_single/result/compressedDepth/parameter_descriptions
/aruco_single/result/compressedDepth/parameter_updates
/aruco_single/result/theora
/aruco_single/result/theora/parameter_descriptions
/aruco_single/result/theora/parameter_updates
/aruco_single/transform
/rosout
/rosout_agg
/stereo/left/camera_info
/stereo/left/image_rect_color
/tf
/tf_static
```

入力はこれみたい

```
/stereo/left/camera_info
/stereo/left/image_rect_color
```

画像トピックのtype確認

```
$ rostopic type /stereo/left/image_rect_color
sensor_msgs/Image
```

入力画像の生成 ( usb_camera または realsense 起動 + リレーでトピック名合わせ )

```
# usb_camの場合
rosrun usb_cam usb_cam_node 
rosrun topic_tools relay /usb_cam/image_raw /stereo/left/image_rect_color
rosrun topic_tools relay  /usb_cam/camera_info /stereo/left/camera_info

# realsenseの場合
roslaunch realsense2_camera demo_pointcloud.launch
rosrun topic_tools relay /camera/color/image_raw /stereo/left/image_rect_color
rosrun topic_tools relay /camera/color/camera_info /stereo/left/camera_info
```

marker id と marker sizeの指定した実行方法

```
roslaunch aruco_ros single.launch markerId:=0 markerSize:=0.08
```

結果 : 全然認識しない

![Screenshot from 2022-11-30 02-04-04.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/bc6b3060-0150-0997-845c-41d9410aa455.png)

原因これか？

http://mechatoyo.blogspot.com/2018/12/aruco-marker.html

キャリブレーションをして`.ros/camera_info/*.yaml`を生成する必要があるみたい

`realsenseでarマーカーの位置検出ができない`という内容のissue

https://github.com/pal-robotics/aruco_ros/issues/107

aruco_rosはオリジナルの辞書を使用しているためだったと結論づけてissueを閉じている

オリジナルの辞書を生成できるのか?

もう一つissueあった内容: `オリジナルのマーカが使えるか?`
`ここで4x4の辞書使い方`って行ってる人もいる.デフォルトは5x5とかなのかな?

https://github.com/pal-robotics/aruco_ros/issues/57

プログラムみると5x5がデフォルトで使われてるっていっている人がいる

```
I think the dictionary used is 5x5. This function is pretty much hard coding it. I haven't tried changing it but that might be a good place to start. 
```

確かにREADMEで出てくるarマーカは5x5ぽい

![Screenshot from 2022-11-30 03-12-46.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/7a4dd8e7-89df-9c73-e95a-f5c09f5e99f4.png)

5x5にしたら認識した、しかし３次元の位置が出てこない(poseトピックやtfなど)

関連issue 
https://github.com/pal-robotics/aruco_ros/issues/45

- キャリブレーション

参考サイト1
https://qiita.com/proton_lattice/items/aa805b28700575ba5ed3

キャリブレーション準備
```
ネットにチェッカーボードの画像はあるので、印刷して、平らな板に貼り付ける

白黒の正方形の大きさを計測（～mm）
縦・横の白黒の”交点”を数える（横長で、9x7列なら、8x6個）
```

参考サイト2 (ros wikiにもチェッカーボードがあるみたい ここから : http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFile&do=get&target=check-108.pdf)
https://qiita.com/srs/items/416aa78f2c679ddb7c52

チェッカーボード準備できたらcalibrationを試して今度試す

# パッケージ その2

https://www.techlife-hacking.com/?p=1291


```
roslaunch aruco_detect aruco_detect.launch camera:=/camera/color image:=/camera/color/image_raw transport:=compressed
```

# パッケージ その3 `ar_track_alvar`


ar_track_alvar 

ros wiki 
http://wiki.ros.org/ar_track_alvar

github
https://github.com/ros-perception/ar_track_alvar

## 試した方法 1 本家パッケージでブランチを`noetic-devel`に
noeticではビルドエラー
`noetic-devel`ブランチでもだめだった.CMakeLits.txtみるとg++バージョンが11とかにしか対応してなさそう&ビルドのエラーにもg++のバージョンに関するエラーを吐いている感じがした.
g++ 11を入れてみたらどうなるか試してみる.

## 試した方法 2 pullrequestから

https://github.com/ros-perception/ar_track_alvar/pull/89

https://github.com/pmusau17/ar_track_alvar/tree/noetic-devel

これはビルド通った.

下記サイトのlaunchファイルをもとにrealsenseようにlaunchを修正
※ARマーカのサイズも正しいものに書き換えるように注意
http://ishi.main.jp/ros/ros_ar_indiv.html

```
<launch>
	<arg name="marker_size" default="4.5" />
	<arg name="max_new_marker_error" default="0.08" />
	<arg name="max_track_error" default="0.2" />
	<arg name="cam_image_topic" default="/camera/color/image_raw" />
	<arg name="cam_info_topic" default="/camera/color/camera_info" />	
	<arg name="output_frame" default="/camera_link" />

	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen"
	 args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
</launch>
		
```

結果

すごすぎる,点群のarマーカの位置とほぼ一致している
カメラだけでこんなに精度がでるのか

![Screenshot from 2022-12-06 14-24-54.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/589906/c2b9517d-fa47-2931-7863-77cdfe59beba.png)

ar marker ここから: http://wiki.ros.org/ar_track_alvar

# 関連

aruco用のARマーカの生成

https://qiita.com/hoshianaaa/items/a008ffb4ee518dbbbd75