initial guess based on robot odometry

koide3 · Dec 8, 2020 · c477626 · c477626
1 parent fbfd79e
commit c477626
Show file tree

Hide file tree

Showing 11 changed files with 122 additions and 573 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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" />