###目的
チューリッヒ工科大が公開している、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を受信し、動作確認する。
###参考
ethz-asl Github : https://github.com/ethz-asl
ethz-asl HP : http://www.asl.ethz.ch/