diff --git a/apps/scan_matching_odometry_nodelet.cpp b/apps/scan_matching_odometry_nodelet.cpp
index fb69a7e1..0bb42d5c 100644
--- a/apps/scan_matching_odometry_nodelet.cpp
+++ b/apps/scan_matching_odometry_nodelet.cpp
@@ -8,6 +8,7 @@
 #include <ros/duration.h>
 #include <pcl_ros/point_cloud.h>
 #include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
 #include <tf/transform_broadcaster.h>
 
 #include <std_msgs/Time.h>
@@ -65,6 +66,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
     auto& pnh = private_nh;
     points_topic = pnh.param<std::string>("points_topic", "/velodyne_points");
     odom_frame_id = pnh.param<std::string>("odom_frame_id", "odom");
+    robot_odom_frame_id = pnh.param<std::string>("robot_odom_frame_id", "robot_odom");
 
     // The minimum tranlational distance and rotation angle between keyframes.
     // If this value is zero, frames are always compared with the previous frame
@@ -161,6 +163,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
    */
   Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {
     if(!keyframe) {
+      prev_time = ros::Time();
       prev_trans.setIdentity();
       keyframe_pose.setIdentity();
       keyframe_stamp = stamp;
@@ -172,6 +175,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
     auto filtered = downsample(cloud);
     registration->setInputSource(filtered);
 
+    std::string msf_source;
     Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();
 
     if(private_nh.param<bool>("enable_imu_frontend", false)) {
@@ -180,16 +184,31 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
         Eigen::Isometry3d pose1 = pose2isometry(msf_pose->pose.pose);
         Eigen::Isometry3d delta = pose0.inverse() * pose1;
 
+        msf_source = "imu";
         msf_delta = delta.cast<float>();
       } else {
         std::cerr << "msf data is too old" << std::endl;
       }
+    } else if(private_nh.param<bool>("enable_robot_odometry_init_guess", false) && !prev_time.isZero()) {
+      tf::StampedTransform transform;
+      if(tf_listener.waitForTransform(cloud->header.frame_id, stamp, cloud->header.frame_id, prev_time, robot_odom_frame_id, ros::Duration(0))) {
+        tf_listener.lookupTransform(cloud->header.frame_id, stamp, cloud->header.frame_id, prev_time, robot_odom_frame_id, transform);
+      } else if(tf_listener.waitForTransform(cloud->header.frame_id, ros::Time(0), cloud->header.frame_id, prev_time, robot_odom_frame_id, ros::Duration(0))) {
+        tf_listener.lookupTransform(cloud->header.frame_id, ros::Time(0), cloud->header.frame_id, prev_time, robot_odom_frame_id, transform);
+      }
+
+      if(transform.stamp_.isZero()) {
+        NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id);
+      } else {
+        msf_source = "odometry";
+        msf_delta = tf2isometry(transform).cast<float>();
+      }
     }
 
     pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
     registration->align(*aligned, prev_trans * msf_delta.matrix());
 
-    publish_scan_matching_status(stamp, cloud->header.frame_id, *registration);
+    publish_scan_matching_status(stamp, cloud->header.frame_id, msf_source, msf_delta);
 
     if(!registration->hasConverged()) {
       NODELET_INFO_STREAM("scan matching has not converged!!");
@@ -212,6 +231,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
       }
     }
 
+    prev_time = stamp;
     prev_trans = trans;
 
     auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "keyframe");
@@ -226,6 +246,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
 
       keyframe_pose = odom;
       keyframe_stamp = stamp;
+      prev_time = stamp;
       prev_trans.setIdentity();
     }
 
@@ -273,7 +294,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
   /**
    * @brief publish scan matching status
    */
-  void publish_scan_matching_status(const ros::Time& stamp, const std::string& frame_id, const pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>& reg) {
+  void publish_scan_matching_status(const ros::Time& stamp, const std::string& frame_id, const std::string& msf_source, const Eigen::Isometry3f& msf_delta) {
     if(!status_pub.getNumSubscribers()) {
       return;
     }
@@ -289,24 +310,24 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
     int num_inliers = 0;
     std::vector<int> k_indices;
     std::vector<float> k_sq_dists;
-    for(const auto& pt: *reg.getInputCloud()) {
-      reg.getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
+    for(const auto& pt: *registration->getInputCloud()) {
+      registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
       if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
         num_inliers++;
       }
     }
-    status.inlier_fraction = static_cast<float>(num_inliers) / reg.getInputCloud()->size();
+    status.inlier_fraction = static_cast<float>(num_inliers) / registration->getInputCloud()->size();
+
+    status.relative_pose = isometry2pose(Eigen::Isometry3f(registration->getFinalTransformation()).cast<double>());
 
-    Eigen::Quaternionf quat(registration->getFinalTransformation().block<3, 3>(0, 0));
-    Eigen::Vector3f trans = registration->getFinalTransformation().block<3, 1>(0, 3);
+    if(!msf_source.empty()) {
+      status.prediction_labels.resize(1);
+      status.prediction_labels[0].data = msf_source;
 
-    status.relative_pose.position.x = trans.x();
-    status.relative_pose.position.y = trans.y();
-    status.relative_pose.position.z = trans.z();
-    status.relative_pose.orientation.x = quat.x();
-    status.relative_pose.orientation.y = quat.y();
-    status.relative_pose.orientation.z = quat.z();
-    status.relative_pose.orientation.w = quat.w();
+      status.prediction_errors.resize(1);
+      Eigen::Isometry3f error = Eigen::Isometry3f(registration->getFinalTransformation()).inverse() * msf_delta;
+      status.prediction_errors[0] = isometry2pose(error.cast<double>());
+    }
 
     status_pub.publish(status);
   }
@@ -324,11 +345,13 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
   ros::Publisher trans_pub;
   ros::Publisher aligned_points_pub;
   ros::Publisher status_pub;
+  tf::TransformListener tf_listener;
   tf::TransformBroadcaster odom_broadcaster;
   tf::TransformBroadcaster keyframe_broadcaster;
 
   std::string points_topic;
   std::string odom_frame_id;
+  std::string robot_odom_frame_id;
   ros::Publisher read_until_pub;
 
   // keyframe parameters
@@ -345,6 +368,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
   geometry_msgs::PoseWithCovarianceStampedConstPtr msf_pose;
   geometry_msgs::PoseWithCovarianceStampedConstPtr msf_pose_after_update;
 
+  ros::Time prev_time;
   Eigen::Matrix4f prev_trans;                  // previous estimated transform from keyframe
   Eigen::Matrix4f keyframe_pose;               // keyframe pose
   ros::Time keyframe_stamp;                    // keyframe time
diff --git a/include/hdl_graph_slam/ros_utils.hpp b/include/hdl_graph_slam/ros_utils.hpp
index b7da6f49..0a53617f 100644
--- a/include/hdl_graph_slam/ros_utils.hpp
+++ b/include/hdl_graph_slam/ros_utils.hpp
@@ -49,6 +49,29 @@ static Eigen::Isometry3d pose2isometry(const geometry_msgs::Pose& pose) {
   return mat;
 }
 
+static Eigen::Isometry3d tf2isometry(const tf::StampedTransform& trans) {
+  Eigen::Isometry3d mat = Eigen::Isometry3d::Identity();
+  mat.translation() = Eigen::Vector3d(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z());
+  mat.linear() = Eigen::Quaterniond(trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()).toRotationMatrix();
+  return mat;
+}
+
+static geometry_msgs::Pose isometry2pose(const Eigen::Isometry3d& mat) {
+  Eigen::Quaterniond quat(mat.linear());
+  Eigen::Vector3d trans = mat.translation();
+
+  geometry_msgs::Pose pose;
+  pose.position.x = trans.x();
+  pose.position.y = trans.y();
+  pose.position.z = trans.z();
+  pose.orientation.w = quat.w();
+  pose.orientation.x = quat.x();
+  pose.orientation.y = quat.y();
+  pose.orientation.z = quat.z();
+
+  return pose;
+}
+
 static Eigen::Isometry3d odom2isometry(const nav_msgs::OdometryConstPtr& odom_msg) {
   const auto& orientation = odom_msg->pose.pose.orientation;
   const auto& position = odom_msg->pose.pose.position;
diff --git a/launch/hdl_graph_slam.launch b/launch/hdl_graph_slam.launch
index 80591ad4..b5e7868b 100644
--- a/launch/hdl_graph_slam.launch
+++ b/launch/hdl_graph_slam.launch
@@ -6,7 +6,14 @@
   <arg name="enable_gps" default="false" />
   <arg name="enable_imu_acc" default="false" />
   <arg name="enable_imu_ori" default="false" />
+
   <arg name="points_topic" default="/velodyne_points" />
+  <arg name="map_frame_id" default="map" />
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
 
   <!-- transformation between lidar and base_link -->
   <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
@@ -36,27 +43,29 @@
 
   <!-- scan_matching_odometry_nodelet -->
   <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
-      <param name="points_topic" value="$(arg points_topic)" />
-      <param name="odom_frame_id" value="odom" />
-      <param name="keyframe_delta_trans" value="1.0" />
-      <param name="keyframe_delta_angle" value="1.0" />
-      <param name="keyframe_delta_time" value="10000.0" />
-      <param name="transform_thresholding" value="false" />
-      <param name="max_acceptable_trans" value="1.0" />
-      <param name="max_acceptable_angle" value="1.0" />
-      <param name="downsample_method" value="NONE" />
-      <param name="downsample_resolution" value="0.1" />
-      <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
-      <param name="registration_method" value="FAST_GICP" />
-      <param name="reg_num_threads" value="0" />
-      <param name="reg_transformation_epsilon" value="0.01"/>
-      <param name="reg_maximum_iterations" value="64"/>
-      <param name="reg_max_correspondence_distance" value="2.5"/>
-      <param name="reg_max_optimizer_iterations" value="20"/>
-      <param name="reg_use_reciprocal_correspondences" value="false"/>
-      <param name="reg_correspondence_randomness" value="20"/>
-      <param name="reg_resolution" value="1.0" />
-      <param name="reg_nn_search_method" value="DIRECT7" />
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
+    <param name="keyframe_delta_trans" value="1.0" />
+    <param name="keyframe_delta_angle" value="1.0" />
+    <param name="keyframe_delta_time" value="10000.0" />
+    <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
+    <param name="max_acceptable_trans" value="1.0" />
+    <param name="max_acceptable_angle" value="1.0" />
+    <param name="downsample_method" value="NONE" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.01"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.5"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
   </node>
 
   <!-- floor_detection_nodelet -->
@@ -74,8 +83,8 @@
   <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
     <param name="points_topic" value="$(arg points_topic)" />
     <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
     <!-- optimization params -->
     <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
     <param name="g2o_solver_type" value="lm_var_cholmod" />
@@ -143,5 +152,5 @@
     <param name="map_cloud_resolution" value="0.05" />
   </node>
 
-  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"/>
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
 </launch>
diff --git a/launch/hdl_graph_slam_400.launch b/launch/hdl_graph_slam_400.launch
index ef047822..1b4b4f27 100644
--- a/launch/hdl_graph_slam_400.launch
+++ b/launch/hdl_graph_slam_400.launch
@@ -6,7 +6,14 @@
   <arg name="enable_gps" default="false" />
   <arg name="enable_imu_acc" default="false" />
   <arg name="enable_imu_ori" default="false" />
+
   <arg name="points_topic" default="/velodyne_points" />
+  <arg name="map_frame_id" default="map" />
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
 
   <!-- transformation between lidar and base_link -->
   <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
@@ -37,11 +44,13 @@
   <!-- scan_matching_odometry_nodelet -->
   <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
     <param name="points_topic" value="$(arg points_topic)" />
-    <param name="odom_frame_id" value="odom" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
     <param name="keyframe_delta_trans" value="1.0" />
     <param name="keyframe_delta_angle" value="1.0" />
     <param name="keyframe_delta_time" value="10000.0" />
     <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
     <param name="max_acceptable_trans" value="1.0" />
     <param name="max_acceptable_angle" value="1.0" />
     <param name="downsample_method" value="NONE" />
@@ -74,8 +83,8 @@
   <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
     <param name="points_topic" value="$(arg points_topic)" />
     <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
     <!-- optimization params -->
     <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
     <param name="g2o_solver_type" value="lm_var_cholmod" />
diff --git a/launch/hdl_graph_slam_501.launch b/launch/hdl_graph_slam_501.launch
index 0ac1f5b9..8f009c06 100644
--- a/launch/hdl_graph_slam_501.launch
+++ b/launch/hdl_graph_slam_501.launch
@@ -6,7 +6,14 @@
   <arg name="enable_gps" default="false" />
   <arg name="enable_imu_acc" default="false" />
   <arg name="enable_imu_ori" default="false" />
+
   <arg name="points_topic" default="/velodyne_points" />
+  <arg name="map_frame_id" default="map" />
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
 
   <!-- transformation between lidar and base_link -->
   <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
@@ -37,11 +44,13 @@
   <!-- scan_matching_odometry_nodelet -->
   <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
     <param name="points_topic" value="$(arg points_topic)" />
-    <param name="odom_frame_id" value="odom" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
     <param name="keyframe_delta_trans" value="0.25" />
     <param name="keyframe_delta_angle" value="1.0" />
     <param name="keyframe_delta_time" value="10000.0" />
     <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
     <param name="max_acceptable_trans" value="1.0" />
     <param name="max_acceptable_angle" value="1.0" />
     <param name="downsample_method" value="NONE" />
@@ -74,8 +83,8 @@
   <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
     <param name="points_topic" value="$(arg points_topic)" />
     <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
     <!-- optimization params -->
     <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
     <param name="g2o_solver_type" value="lm_var_cholmod" />
diff --git a/launch/hdl_graph_slam_centrair.launch b/launch/hdl_graph_slam_centrair.launch
deleted file mode 100644
index 477bc819..00000000
--- a/launch/hdl_graph_slam_centrair.launch
+++ /dev/null
@@ -1,132 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- arguments -->
-  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
-  <arg name="enable_floor_detection" default="true" />
-  <arg name="enable_gps" default="false" />
-  <arg name="enable_imu_acc" default="false" />
-  <arg name="enable_imu_ori" default="false" />
-  <arg name="points_topic" default="/velodyne_points" />
-
-  <!-- transformation between lidar and base_link -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0.1 0 0.2 base_link velodyne 10" />
-
-  <!-- in case you use velodyne_driver, comment out the following line -->
-  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
-
-  <!-- prefiltering_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
-    <remap from="/velodyne_points" to="$(arg points_topic)" />
-    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
-    <param name="base_link_frame" value="base_link" />
-    <!-- distance filter -->
-    <param name="use_distance_filter" value="true" />
-    <param name="distance_near_thresh" value="0.1" />
-    <param name="distance_far_thresh" value="100.0" />
-    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
-    <param name="downsample_method" value="VOXELGRID" />
-    <param name="downsample_resolution" value="0.1" />
-    <!-- NONE, RADIUS, or STATISTICAL -->
-    <param name="outlier_removal_method" value="RADIUS" />
-    <param name="statistical_mean_k" value="30" />
-    <param name="statistical_stddev" value="1.2" />
-    <param name="radius_radius" value="0.5" />
-    <param name="radius_min_neighbors" value="2" />
-  </node>
-
-  <!-- scan_matching_odometry_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
-      <param name="points_topic" value="$(arg points_topic)" />
-      <param name="odom_frame_id" value="odom" />
-      <param name="keyframe_delta_trans" value="1.0" />
-      <param name="keyframe_delta_angle" value="1.0" />
-      <param name="keyframe_delta_time" value="1.0" />
-      <param name="transform_thresholding" value="false" />
-      <param name="max_acceptable_trans" value="1.0" />
-      <param name="max_acceptable_angle" value="1.0" />
-      <param name="downsample_method" value="NONE" />
-      <param name="downsample_resolution" value="0.1" />
-      <!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) -->
-      <param name="registration_method" value="NDT_OMP" />
-      <param name="ndt_resolution" value="2.0" />
-      <param name="ndt_num_threads" value="0" />
-      <param name="ndt_nn_search_method" value="DIRECT7" />
-  </node>
-
-  <!-- floor_detection_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <param name="tilt_deg" value="0.0" />
-    <param name="sensor_height" value="2.0" />
-    <param name="height_clip_range" value="1.0" />
-    <param name="floor_pts_thresh" value="128" />
-    <param name="use_normal_filtering" value="true" />
-    <param name="normal_filter_thresh" value="40.0" />
-  </node>
-
-  <!-- hdl_graph_slam_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
-    <!-- optimization params -->
-    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
-    <param name="g2o_solver_type" value="gn_var_cholmod" />
-    <param name="g2o_solver_num_iterations" value="128" />
-    <!-- keyframe registration params -->
-    <param name="enable_gps" value="$(arg enable_gps)" />
-    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
-    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
-    <param name="max_keyframes_per_update" value="10" />
-    <param name="keyframe_delta_trans" value="2.0" />
-    <param name="keyframe_delta_angle" value="2.0" />
-    <!-- loop closure params -->
-    <param name="distance_thresh" value="0.0" />
-    <param name="accum_distance_thresh" value="15.0" />
-    <param name="min_edge_interval" value="5.0" />
-    <param name="fitness_score_max_range" value="2.0" />
-    <param name="fitness_score_thresh" value="0.4" />
-    <!-- scan matching params -->
-    <param name="registration_method" value="NDT_OMP" />
-    <param name="ndt_resolution" value="2.0" />
-    <param name="ndt_num_threads" value="0" />
-    <param name="ndt_nn_search_method" value="DIRECT7" />
-    <!-- edge params -->
-    <!-- GPS -->
-    <param name="gps_edge_robust_kernel" value="NONE" />
-    <param name="gps_edge_robust_kernel_size" value="1.0" />
-    <param name="gps_edge_stddev_xy" value="20.0" />
-    <param name="gps_edge_stddev_z" value="5.0" />
-    <!-- IMU orientation -->
-    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
-    <param name="imu_orientation_edge_stddev" value="1.0" />
-    <!-- IMU acceleration (gravity vector) -->
-    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
-    <param name="imu_acceleration_edge_stddev" value="1.0" />
-    <!-- ground plane -->
-    <param name="floor_edge_robust_kernel" value="NONE" />
-    <param name="floor_edge_stddev" value="10.0" />
-    <!-- scan matching -->
-    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
-    <param name="fix_first_node" value="true" />
-    <param name="odometry_edge_robust_kernel" value="NONE" />
-    <param name="odometry_edge_robust_kernel_size" value="1.0" />
-    <param name="loop_closure_edge_robust_kernel" value="Huber" />
-    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
-    <param name="use_const_inf_matrix" value="false" />
-    <param name="const_stddev_x" value="0.5" />
-    <param name="const_stddev_q" value="0.1" />
-    <param name="var_gain_a" value="20.0" />
-    <param name="min_stddev_x" value="0.1" />
-    <param name="max_stddev_x" value="5.0" />
-    <param name="min_stddev_q" value="0.05" />
-    <param name="max_stddev_q" value="0.2" />
-    <!-- update params -->
-    <param name="graph_update_interval" value="3.0" />
-    <param name="map_cloud_update_interval" value="15.0" />
-    <param name="map_cloud_resolution" value="0.2" />
-  </node>
-
-  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
-</launch>
diff --git a/launch/hdl_graph_slam_ford.launch b/launch/hdl_graph_slam_ford.launch
deleted file mode 100644
index da5740c1..00000000
--- a/launch/hdl_graph_slam_ford.launch
+++ /dev/null
@@ -1,130 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- arguments -->
-  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
-  <arg name="enable_floor_detection" default="false" />
-  <arg name="enable_gps" default="true" />
-  <arg name="enable_imu_acc" default="false" />
-  <arg name="enable_imu_ori" default="false" />
-  <arg name="points_topic" default="/velodyne_points" />
-
-  <!-- transformation between lidar and base_link -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
-
-  <!-- in case you use velodyne_driver, comment out the following line -->
-  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
-
-  <!-- prefiltering_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
-    <remap from="/velodyne_points" to="$(arg points_topic)" />
-    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
-    <param name="base_link_frame" value="base_link" />
-    <!-- distance filter -->
-    <param name="use_distance_filter" value="true" />
-    <param name="distance_near_thresh" value="1.0" />
-    <param name="distance_far_thresh" value="100.0" />
-    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
-    <param name="downsample_method" value="VOXELGRID" />
-    <param name="downsample_resolution" value="0.2" />
-    <!-- NONE, RADIUS, or STATISTICAL -->
-    <param name="outlier_removal_method" value="RADIUS" />
-    <param name="statistical_mean_k" value="30" />
-    <param name="statistical_stddev" value="1.2" />
-    <param name="radius_radius" value="0.5" />
-    <param name="radius_min_neighbors" value="5" />
-  </node>
-
-  <!-- scan_matching_odometry_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
-      <param name="points_topic" value="$(arg points_topic)" />
-      <param name="odom_frame_id" value="odom" />
-      <param name="keyframe_delta_trans" value="2.5" />
-      <param name="keyframe_delta_angle" value="1.0" />
-      <param name="keyframe_delta_time" value="5.0" />
-      <param name="transform_thresholding" value="false" />
-      <param name="max_acceptable_trans" value="2.0" />
-      <param name="max_acceptable_angle" value="1.0" />
-      <param name="downsample_method" value="NONE" />
-      <param name="downsample_resolution" value="0.1" />
-      <!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) -->
-      <param name="registration_method" value="NDT_OMP" />
-      <param name="ndt_resolution" value="2.0" />
-      <param name="ndt_num_threads" value="0" />
-      <param name="ndt_nn_search_method" value="DIRECT7" />
-  </node>
-
-  <!-- floor_detection_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <param name="tilt_deg" value="0.0" />
-    <param name="sensor_height" value="2.0" />
-    <param name="height_clip_range" value="1.0" />
-    <param name="floor_pts_thresh" value="512" />
-    <param name="use_normal_filtering" value="true" />
-    <param name="normal_filter_thresh" value="20.0" />
-  </node>
-
-  <!-- hdl_graph_slam_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
-    <!-- optimization params -->
-    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
-    <param name="g2o_solver_type" value="lm_var_cholmod" />
-    <param name="g2o_solver_num_iterations" value="512" />
-    <!-- keyframe registration params -->
-    <param name="enable_gps" value="$(arg enable_gps)" />
-    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
-    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
-    <param name="max_keyframes_per_update" value="10" />
-    <param name="keyframe_delta_trans" value="10.0" />
-    <param name="keyframe_delta_angle" value="3.0" />
-    <!-- loop closure params -->
-    <param name="distance_thresh" value="30.0" />
-    <param name="accum_distance_thresh" value="60.0" />
-    <param name="min_edge_interval" value="10.0" />
-    <param name="fitness_score_thresh" value="2.0" />
-    <!-- scan matching params -->
-    <param name="registration_method" value="NDT_OMP" />
-    <param name="ndt_resolution" value="2.0" />
-    <param name="ndt_num_threads" value="0" />
-    <param name="ndt_nn_search_method" value="DIRECT7" />
-    <!-- edge params -->
-    <!-- GPS -->
-    <param name="gps_edge_robust_kernel" value="NONE" />
-    <param name="gps_edge_robust_kernel_size" value="1.0" />
-    <param name="gps_edge_stddev_xy" value="20.0" />
-    <param name="gps_edge_stddev_z" value="5.0" />
-    <!-- IMU orientation -->
-    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
-    <param name="imu_orientation_edge_stddev" value="1.0" />
-    <!-- IMU acceleration (gravity vector) -->
-    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
-    <param name="imu_acceleration_edge_stddev" value="1.0" />
-    <!-- ground plane -->
-    <param name="floor_edge_robust_kernel" value="NONE" />
-    <param name="floor_edge_stddev" value="10.0" />
-    <!-- scan matching -->
-    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
-    <param name="odometry_edge_robust_kernel" value="NONE" />
-    <param name="odometry_edge_robust_kernel_size" value="1.0" />
-    <param name="loop_closure_edge_robust_kernel" value="Huber" />
-    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
-    <param name="use_const_inf_matrix" value="false" />
-    <param name="const_stddev_x" value="0.5" />
-    <param name="const_stddev_q" value="0.1" />
-    <param name="var_gain_a" value="20.0" />
-    <param name="min_stddev_x" value="0.1" />
-    <param name="max_stddev_x" value="5.0" />
-    <param name="min_stddev_q" value="0.05" />
-    <param name="max_stddev_q" value="0.2" />
-    <!-- update params -->
-    <param name="graph_update_interval" value="3.0" />
-    <param name="map_cloud_update_interval" value="10.0" />
-    <param name="map_cloud_resolution" value="0.05" />
-  </node>
-
-  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
-</launch>
diff --git a/launch/hdl_graph_slam_imu.launch b/launch/hdl_graph_slam_imu.launch
index 57b2caca..b7c41192 100644
--- a/launch/hdl_graph_slam_imu.launch
+++ b/launch/hdl_graph_slam_imu.launch
@@ -33,7 +33,7 @@
   <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)" output="screen">
     <remap from="/velodyne_points" to="$(arg points_topic)" />
     <remap from="/imu/data" to="$(arg imu_topic)" />
-    <param name="deskewing" value="true" />
+    <param name="deskewing" value="$(arg enable_imu_frontend)" />
     <param name="scan_period" value="0.1" />
     <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
     <param name="base_link_frame" value="base_link" />
diff --git a/launch/hdl_graph_slam_kit.launch b/launch/hdl_graph_slam_kit.launch
deleted file mode 100644
index 152df526..00000000
--- a/launch/hdl_graph_slam_kit.launch
+++ /dev/null
@@ -1,135 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- arguments -->
-  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
-  <arg name="enable_floor_detection" default="false" />
-  <arg name="enable_gps" default="false" />
-  <arg name="enable_imu_acc" default="false" />
-  <arg name="enable_imu_ori" default="false" />
-  <arg name="points_topic" default="/points" />
-
-  <!-- transformation between lidar and base_link -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link lidar 10" />
-  <node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="-0.79 0 -1.73 0 0 0 base_link imu 10" />
-
-  <!-- in case you use velodyne_driver, comment out the following line -->
-  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
-
-  <!-- prefiltering_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
-    <remap from="/velodyne_points" to="$(arg points_topic)" />
-    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
-    <param name="base_link_frame" value="base_link" />
-    <!-- distance filter -->
-    <param name="use_distance_filter" value="true" />
-    <param name="distance_near_thresh" value="1.0" />
-    <param name="distance_far_thresh" value="100.0" />
-    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
-    <param name="downsample_method" value="VOXELGRID" />
-    <param name="downsample_resolution" value="0.2" />
-    <!-- NONE, RADIUS, or STATISTICAL -->
-    <param name="outlier_removal_method" value="RADIUS" />
-    <param name="statistical_mean_k" value="30" />
-    <param name="statistical_stddev" value="1.2" />
-    <param name="radius_radius" value="0.5" />
-    <param name="radius_min_neighbors" value="5" />
-  </node>
-
-  <!-- scan_matching_odometry_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
-      <param name="points_topic" value="$(arg points_topic)" />
-      <param name="odom_frame_id" value="odom" />
-      <param name="keyframe_delta_trans" value="2.5" />
-      <param name="keyframe_delta_angle" value="1.0" />
-      <param name="keyframe_delta_time" value="5.0" />
-      <param name="transform_thresholding" value="false" />
-      <param name="max_acceptable_trans" value="2.0" />
-      <param name="max_acceptable_angle" value="1.0" />
-      <param name="downsample_method" value="NONE" />
-      <param name="downsample_resolution" value="0.1" />
-      <!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) -->
-      <param name="registration_method" value="NDT_OMP" />
-      <param name="ndt_resolution" value="2.0" />
-      <param name="ndt_num_threads" value="0" />
-      <param name="ndt_nn_search_method" value="DIRECT1" />
-  </node>
-
-  <!-- floor_detection_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <param name="tilt_deg" value="0.0" />
-    <param name="sensor_height" value="2.0" />
-    <param name="height_clip_range" value="1.0" />
-    <param name="floor_pts_thresh" value="512" />
-    <param name="use_normal_filtering" value="true" />
-    <param name="normal_filter_thresh" value="20.0" />
-  </node>
-
-  <!-- hdl_graph_slam_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
-    <remap from="/gps/geopoint" to="/gps" />
-    <remap from="/gpsimu_driver/imu_data" to="/imu" />
-    <param name="points_topic" value="$(arg points_topic)" />
-    <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
-    <!-- optimization params -->
-    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
-    <param name="g2o_solver_type" value="lm_var_cholmod" />
-    <param name="g2o_solver_num_iterations" value="512" />
-    <!-- keyframe registration params -->
-    <param name="enable_gps" value="$(arg enable_gps)" />
-    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
-    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
-    <param name="max_keyframes_per_update" value="10" />
-    <param name="keyframe_delta_trans" value="10.0" />
-    <param name="keyframe_delta_angle" value="3.0" />
-    <!-- loop closure params -->
-    <param name="distance_thresh" value="30.0" />
-    <param name="accum_distance_thresh" value="60.0" />
-    <param name="min_edge_interval" value="10.0" />
-    <param name="fitness_score_thresh" value="2.0" />
-    <!-- scan matching params -->
-    <param name="registration_method" value="NDT_OMP" />
-    <param name="ndt_resolution" value="2.0" />
-    <param name="ndt_num_threads" value="0" />
-    <param name="ndt_nn_search_method" value="DIRECT7" />
-    <!-- edge params -->
-    <!-- GPS -->
-    <param name="gps_time_offset" value="0.0" />
-    <param name="gps_edge_robust_kernel" value="Huber" />
-    <param name="gps_edge_robust_kernel_size" value="0.1" />
-    <param name="gps_edge_stddev_xy" value="500.0" />
-    <param name="gps_edge_stddev_z" value="50.0" />
-    <!-- IMU orientation -->
-    <param name="imu_time_offset" value="-1.0" />
-    <param name="imu_orientation_edge_robust_kernel" value="Huber" />
-    <param name="imu_orientation_edge_stddev" value="0.1" />
-    <!-- IMU acceleration (gravity vector) -->
-    <param name="imu_acceleration_edge_robust_kernel" value="Huber" />
-    <param name="imu_acceleration_edge_stddev" value="1.0" />
-    <!-- ground plane -->
-    <param name="floor_edge_robust_kernel" value="Huber" />
-    <param name="floor_edge_stddev" value="10.0" />
-    <!-- scan matching -->
-    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
-    <param name="odometry_edge_robust_kernel" value="NONE" />
-    <param name="odometry_edge_robust_kernel_size" value="1.0" />
-    <param name="loop_closure_edge_robust_kernel" value="Huber" />
-    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
-    <param name="use_const_inf_matrix" value="false" />
-    <param name="const_stddev_x" value="0.5" />
-    <param name="const_stddev_q" value="0.1" />
-    <param name="var_gain_a" value="20.0" />
-    <param name="min_stddev_x" value="0.1" />
-    <param name="max_stddev_x" value="5.0" />
-    <param name="min_stddev_q" value="0.05" />
-    <param name="max_stddev_q" value="0.2" />
-    <!-- update params -->
-    <param name="graph_update_interval" value="3.0" />
-    <param name="map_cloud_update_interval" value="10.0" />
-    <param name="map_cloud_resolution" value="0.05" />
-  </node>
-
-  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
-</launch>
diff --git a/launch/hdl_graph_slam_pasco.launch b/launch/hdl_graph_slam_pasco.launch
deleted file mode 100644
index 6575f6ef..00000000
--- a/launch/hdl_graph_slam_pasco.launch
+++ /dev/null
@@ -1,131 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <!-- arguments -->
-  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
-  <arg name="enable_floor_detection" default="false" />
-  <arg name="enable_gps" default="false" />
-  <arg name="enable_imu_acc" default="false" />
-  <arg name="enable_imu_ori" default="false" />
-  <arg name="points_topic" default="/velodyne_points" />
-
-  <!-- transformation between lidar and base_link -->
-  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
-
-  <!-- in case you use velodyne_driver, comment out the following line -->
-  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
-
-  <!-- prefiltering_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
-    <remap from="/velodyne_points" to="$(arg points_topic)" />
-    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
-    <param name="base_link_frame" value="base_link" />
-    <!-- distance filter -->
-    <param name="use_distance_filter" value="false" />
-    <param name="distance_near_thresh" value="0.1" />
-    <param name="distance_far_thresh" value="100.0" />
-    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
-    <param name="downsample_method" value="APPROX_VOXELGRID" />
-    <param name="downsample_resolution" value="0.1" />
-    <!-- NONE, RADIUS, or STATISTICAL -->
-    <param name="outlier_removal_method" value="RADIUS" />
-    <param name="statistical_mean_k" value="30" />
-    <param name="statistical_stddev" value="1.2" />
-    <param name="radius_radius" value="0.5" />
-    <param name="radius_min_neighbors" value="2" />
-  </node>
-
-  <!-- scan_matching_odometry_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
-      <param name="points_topic" value="$(arg points_topic)" />
-      <param name="odom_frame_id" value="odom" />
-      <param name="keyframe_delta_trans" value="10.0" />
-      <param name="keyframe_delta_angle" value="10.0" />
-      <param name="keyframe_delta_time" value="10.0" />
-      <param name="transform_thresholding" value="false" />
-      <param name="max_acceptable_trans" value="1.0" />
-      <param name="max_acceptable_angle" value="1.0" />
-      <param name="downsample_method" value="NONE" />
-      <param name="downsample_resolution" value="0.1" />
-      <!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) -->
-      <param name="registration_method" value="GICP" />
-      <param name="ndt_resolution" value="2.0" />
-      <param name="ndt_num_threads" value="0" />
-      <param name="ndt_nn_search_method" value="DIRECT7" />
-  </node>
-
-  <!-- floor_detection_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <param name="tilt_deg" value="0.0" />
-    <param name="sensor_height" value="2.0" />
-    <param name="height_clip_range" value="1.0" />
-    <param name="floor_pts_thresh" value="512" />
-    <param name="use_normal_filtering" value="true" />
-    <param name="normal_filter_thresh" value="20.0" />
-  </node>
-
-  <!-- hdl_graph_slam_nodelet -->
-  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
-    <param name="points_topic" value="$(arg points_topic)" />
-    <!-- frame settings -->
-    <param name="map_frame_id" value="map" />
-    <param name="odom_frame_id" value="odom" />
-    <!-- optimization params -->
-    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
-    <param name="g2o_solver_type" value="lm_var_cholmod" />
-    <param name="g2o_solver_num_iterations" value="512" />
-    <!-- keyframe registration params -->
-    <param name="enable_gps" value="$(arg enable_gps)" />
-    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
-    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
-    <param name="max_keyframes_per_update" value="10" />
-    <param name="keyframe_delta_trans" value="5.0" />
-    <param name="keyframe_delta_angle" value="4.0" />
-    <!-- loop closure params -->
-    <param name="distance_thresh" value="15.0" />
-    <param name="accum_distance_thresh" value="25.0" />
-    <param name="min_edge_interval" value="15.0" />
-    <param name="fitness_score_thresh" value="2.5" />
-    <!-- scan matching params -->
-    <param name="registration_method" value="GICP" />
-    <param name="ndt_resolution" value="1.0" />
-    <param name="ndt_num_threads" value="0" />
-    <param name="ndt_nn_search_method" value="DIRECT7" />
-    <!-- edge params -->
-    <!-- GPS -->
-    <param name="gps_edge_robust_kernel" value="NONE" />
-    <param name="gps_edge_robust_kernel_size" value="1.0" />
-    <param name="gps_edge_stddev_xy" value="20.0" />
-    <param name="gps_edge_stddev_z" value="5.0" />
-    <!-- IMU orientation -->
-    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
-    <param name="imu_orientation_edge_stddev" value="1.0" />
-    <!-- IMU acceleration (gravity vector) -->
-    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
-    <param name="imu_acceleration_edge_stddev" value="1.0" />
-    <!-- ground plane -->
-    <param name="floor_edge_robust_kernel" value="NONE" />
-    <param name="floor_edge_stddev" value="10.0" />
-    <!-- scan matching -->
-    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
-    <param name="fix_first_node" value="true" />
-    <param name="odometry_edge_robust_kernel" value="NONE" />
-    <param name="odometry_edge_robust_kernel_size" value="1.0" />
-    <param name="loop_closure_edge_robust_kernel" value="Huber" />
-    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
-    <param name="use_const_inf_matrix" value="false" />
-    <param name="const_stddev_x" value="0.5" />
-    <param name="const_stddev_q" value="0.1" />
-    <param name="var_gain_a" value="20.0" />
-    <param name="min_stddev_x" value="0.1" />
-    <param name="max_stddev_x" value="5.0" />
-    <param name="min_stddev_q" value="0.05" />
-    <param name="max_stddev_q" value="0.2" />
-    <!-- update params -->
-    <param name="graph_update_interval" value="3.0" />
-    <param name="map_cloud_update_interval" value="10.0" />
-    <param name="map_cloud_resolution" value="0.05" />
-  </node>
-
-  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
-</launch>
diff --git a/msg/ScanMatchingStatus.msg b/msg/ScanMatchingStatus.msg
index 8dde80f1..58d8b525 100644
--- a/msg/ScanMatchingStatus.msg
+++ b/msg/ScanMatchingStatus.msg
@@ -4,3 +4,6 @@ bool has_converged
 float32 matching_error
 float32 inlier_fraction
 geometry_msgs/Pose relative_pose
+
+std_msgs/String[] prediction_labels
+geometry_msgs/Pose[] prediction_errors
\ No newline at end of file