ROSのEthzasl_icp_mappingを動かしてみた。

###目的
チューリッヒ工科大が公開している、ROSの2D/3D-SLAMモジュール「ethzasl_icp_mapping」をKinectV1と繋げて動かしてみた。このノードはやや古くなってきており、最近のROS:kineticでは色々ハマるところがある。
ethzasl_icp_mappingはICP(Iterative Closest Point)アルゴリズムベースのSLAMである。
ethzasl_icp_mappingは、ICPアルゴリズムのライブラリlibpointmatcherのROSのラッパーとなっている。

[WillowGarage Blog: Real-Time Modular 3D Mapping](http://www.willowgarage.com/blog/2013/01/29/real-time-modular-3d-mapping?page=4)
[ethz-asl/ethzasl_icp_mapping Github](https://github.com/ethz-asl/ethzasl_icp_mapping)
[ROS WIKI ethzasl_icp_mapping](http://wiki.ros.org/ethzasl_icp_mapping)
[libpointmatcher](https://libpointmatcher.readthedocs.io/en/latest/)


###環境
Ubuntu 16.04(x64)
ROS:Kinetic
KinectV1(Xbox360)

###libnabo・libpointmatcherのインストール
catkinでlibnabo・libpointmatcherをコンパイルし、インストールする。

```
 mkdir -p ~/catkin_ethzasl_lib/ws1/src
 cd ~/catkin_ethzasl_lib/ws1/src
 catkin_init_workspace
 git clone https://github.com/ethz-asl/libnabo.git
 git clone https://github.com/ethz-asl/libpointmatcher.git
 cd ..
 catkin_make_isolated
 source ~/catkin_ethzasl_lib/ws1/devel_isolated/setup.bash
```

###ethzasl_icp_mappingのコンパイル・インストール

```
 cd 
 mkdir -p ~/catkin_ethzasl_lib/ws2/src
 cd ~/catkin_ethzasl_lib/ws2/src
 catkin_init_workspace
 git clone -b kinetic https://github.com/mgb45/ethzasl_icp_mapping.git
 cd ..
 catkin_make
 source ~/catkin_ethzasl_lib/ws2/devel/setup.bash
```

~/.bashrcにcatkinのリンクを加える。

```
 vi ~/.bashrc
```
```
 source ~/catkin_ethzasl_lib/ws1/devel_isolated/setup.bash
 source ~/catkin_ethzasl_lib/ws2/devel/setup.bash
```


catkin_ethzasl_lib/ws2/src/ethzasl_icp_mapping-kinetic/ethzasl_icp_mapper/launch/2D_scans/map_post_filters.yaml
にprobが定義されていた場合、これを削除する。

> SurfaceNormalDataPointsFilter:
>    knn: 10
    epsilon: 3.16
    keepNormals: 1
    keepDensities: 1
 MaxDensityDataPointsFilter:
    maxDensity: 100000
 MaxPointCountDataPointsFilter:
    maxCount: 600000
>   ~~prob: 0.7~~


###KinectV1のドライバfreenectをインストールする。

```
 sudo apt-get install ros-kinetic-freenect-launch

```

###KinectV1＋ethzasl_icp_mappingでSLAMする

roscore,freenect_launch,kinect_tracker.launch をそれぞれ別のコンソールで立ち上げる。

```
roscore
```

KinectV1(freenect)のノードを起動する。

```
 roslaunch freenect_launch freenect.launch
```

下記のようにLaunchファイルを作成し、実行する。

kinect_tracker.launch 

```
<launch>
  <node name="mapper" type="mapper" pkg="ethzasl_icp_mapper" output="screen"> 
		<remap from="cloud_in" to="/camera/depth_registered/points" />
		<param name="subscribe_scan" value="false"/>
		<param name="icpConfig" value="$(find ethzasl_icp_mapper)/launch/openni/icp.yaml" />
		<param name="inputFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/openni/IROS_2011/input_filters.yaml" />
		<param name="mapPreFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/openni/IROS_2011/map_pre_filters.yaml" />
		<param name="mapPostFiltersConfig" value="$(find ethzasl_icp_mapper)/launch/openni/IROS_2011/map_post_filters.yaml" />
		<param name="odom_frame" value="/kinect" />
		<param name="map_frame" value="/map" />
		<param name="maxOverlapToMerge" value="0.75" />
		<param name="minOverlap" value="0.60" />
		<param name="minMapPointCount" value="0" />
		<param name="useROSLogger" value="false" />
		<param name="inputQueueSize" value="1" />
  </node>
  
	<node name="static_transform_publisher" type="static_transform_publisher" pkg="tf" args="0 0 0 1.57 0 0 /kinect /camera_link 100 "/> 
	
</launch>
```

```
roslaunch kinect_tracker.launch 
```

rvizでTopicを受信し、動作確認する。

![Screenshot from 2017-05-27 17-50-41.png](https://qiita-image-store.s3.amazonaws.com/0/73020/be1d09cb-03b0-6ebe-8b7c-f673e2854162.png)

rqt_graphの図は以下。
![rosgraph.png](https://qiita-image-store.s3.amazonaws.com/0/73020/bf0ab8d8-8650-aa17-5986-6d74b2c8441a.png)


###参考
ethz-asl Github : https://github.com/ethz-asl
ethz-asl HP : http://www.asl.ethz.ch/