###目的
チューリッヒ工科大が公開している、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
ethz-asl/ethzasl_icp_mapping Github
ROS WIKI ethzasl_icp_mapping
libpointmatcher

###環境
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を受信し、動作確認する。

rqt_graphの図は以下。

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