Depth odometry organized icp (nasa#388)

Added depth odometry sweep tool, optional refinement of the image feature depth odometry estimate, and organized methods taking advantage of the grid like structure of point clouds to speed up computations.

astrobee/config/localization/depth_odometry.config

@@ -18,7 +18,7 @@
 require "context"
 
 -- Max time diff between successive depth measurements 
-max_time_diff = 10
+max_time_diff = 0.7 
 
 -- Max time diff for correlating image and point cloud measurements to create a depth measurement
 -- This ideally have the same timestamp but sometimes vary slightly even though they are from 
@@ -42,8 +42,9 @@ symmetric_objective = true
 enforce_same_direction_normals = true
 
 -- ICP fitness score threshold for rejecting ICP result
+use_fitness_threshold_rejection = false
 fitness_threshold = 1
-max_iterations = 10
+max_iterations = 1
 
 -- Use RANSAC AI algorithm to compute initial estimate
 inital_estimate_with_ransac_ia = false 
@@ -53,9 +54,25 @@ correspondence_rejector_surface_normal = true
 -- The closer to 1, the more aligned the normals are
 correspondence_rejector_surface_normal_threshold = 0.75 
 
+correspondence_rejector_median_distance = false 
+-- Correspondences with distance > median_distance*factor are rejected
+correspondence_rejector_median_distance_factor = 1.0
+
+-- Organized normal estimation
+use_organized_normal_estimation = true 
+-- avg_3d_gradient, covariance, avg_depth_change
+organized_normal_method = "avg_3d_gradient"
+use_depth_dependent_smoothing = false
+max_depth_change_factor = 0.02
+normal_smoothing_size = 10.0
+
+-- Normal space sampling
+use_normal_space_sampling = true
+bins_per_axis = 12 
+num_samples = 3500
 
 -- Downsample options 
-downsample = true 
+downsample = false 
 downsample_leaf_size = 0.02
 
 -- coarse to fine options for ICP
@@ -85,8 +102,7 @@ surf_max_match_distance = 0.25
 -- LK optical flow options
 lk_max_iterations = 10
 lk_termination_epsilon = 0.03
-lk_window_width = 10
-lk_window_height = 10
+lk_window_length = 10
 lk_max_level = 3
 lk_min_eigen_threshold = 0.2
 lk_max_flow_distance = 50
@@ -107,6 +123,8 @@ clahe_clip_limit = 40
 min_x_distance_to_border = 10 
 min_y_distance_to_border = 10
 min_num_inliers = 5
+-- Refine estimate with PointToPlaneICP
+refine_estimate = true
 
 -- Point cloud with known correspondences aligner
 pcwkca_max_num_iterations = 100

astrobee/launch/astrobee.launch

@@ -30,7 +30,6 @@
   <!-- Remaining options -->
   <arg name="output" default="log" />         <!-- Output to screen or log   -->
   <arg name="ground_truth_localizer" default="false" /> <!-- Use Ground Truth Localizer      -->
-  <arg name="depth_odometry" default="false" /> <!-- Run depth odometry node -->
   <arg name="drivers" default="true" />       <!-- Should we launch drivers? -->
   <arg name="sim" default="local" />          <!-- SIM IP address            -->
   <arg name="llp" default="local" />          <!-- LLP IP address            -->
@@ -155,7 +154,6 @@
         <arg name="debug" value="$(arg debug)" />      <!-- Debug node group     -->
         <arg name="output" value="$(arg output)" />
         <arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" />
-        <arg name="depth_odometry" value="$(arg depth_odometry)" />
       </include>
 
       <!-- If we have specified a bag, then play it back into flight software-->

astrobee/launch/robot/MLP.launch

@@ -26,7 +26,6 @@
   <arg name="dds" default="true"/>               <!-- Should DDS be started     -->
   <arg name="output" default="log"/>             <!-- Where nodes should log    -->
   <arg name="ground_truth_localizer" default="false"/>   <!-- Runs ground_truth localizer -->
-  <arg name="depth_odometry" default="false"/>   <!-- Runs depth odometry node -->
 
   <!-- Setup nodelet managers -->
   <group if="$(eval optenv('ASTROBEE_NODEGRAPH','')=='')">
@@ -87,18 +86,16 @@
     <arg name="debug" value="$(arg debug)" />
     <arg name="default" value="true" />
   </include>
-  <group if="$(arg depth_odometry)">
-    <include file="$(find ff_util)/launch/ff_nodelet.launch">
-      <arg name="class" value="depth_odometry/DepthOdometryNodelet" />
-      <arg name="name" value="depth_odometry_nodelet" />
-      <arg name="manager" value="mlp_vision" />
-      <arg name="spurn" value="$(arg spurn)" />
-      <arg name="nodes" value="$(arg nodes)" />
-      <arg name="extra" value="$(arg extra)" />
-      <arg name="debug" value="$(arg debug)" />
-      <arg name="default" value="true" />
-    </include>
-  </group>
+  <include file="$(find ff_util)/launch/ff_nodelet.launch">
+    <arg name="class" value="depth_odometry/DepthOdometryNodelet" />
+    <arg name="name" value="depth_odometry_nodelet" />
+    <arg name="manager" value="mlp_vision" />
+    <arg name="spurn" value="$(arg spurn)" />
+    <arg name="nodes" value="$(arg nodes)" />
+    <arg name="extra" value="$(arg extra)" />
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="default" value="true" />
+  </include>
 
   <!-- Run ground_truth_localizer, otherwise run graph localizer -->
   <group unless="$(arg ground_truth_localizer)">

astrobee/launch/sim.launch

@@ -34,7 +34,6 @@
   <arg name="rec" default="" />                 <!-- Record local data       -->
   <arg name="dds" default="true" />             <!-- Enable DDS              -->
   <arg name="ground_truth_localizer" default="false" />  <!-- Use Ground Truth Localizer      -->
-  <arg name="depth_odometry" default="false" />  <!-- Run depth odometry node-->
 
   <!-- General options -->
   <arg name="gviz" default="false" />           <!-- Start GNC visualizer    -->
@@ -159,7 +158,6 @@
       <arg name="mlp" value="$(arg mlp)" />          <!-- MLP IP address     -->
       <arg name="dds" value="$(arg dds)" />          <!-- Enable DDS         -->
       <arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" /> <!-- Use Ground Truth Localizer    -->
-      <arg name="depth_odometry" value="$(arg depth_odometry)" /> <!-- Run depth odometry node -->
     </include>
   </group>
 

astrobee/launch/spawn.launch

@@ -32,7 +32,6 @@
   <arg name="mlp" default="local" />            <!-- MLP IP address          -->
   <arg name="dds" default="true" />             <!-- Enable DDS              -->
   <arg name="ground_truth_localizer" default="false" /> <!-- Use Ground Truth Localizer      -->
-  <arg name="depth_odometry" default="false" /> <!-- Run depth odometry node -->
 
   <!-- General options -->
   <arg name="gviz" default="false" />           <!-- Start GNC visualizer    -->
@@ -88,7 +87,6 @@
     <arg name="mlp" value="$(arg mlp)" />      <!-- MLP IP address           -->
     <arg name="dds" value="$(arg dds)" />      <!-- Enable DDS               -->
     <arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" />      <!-- Use Ground Truth Localizer -->
-    <arg name="depth_odometry" value="$(arg depth_odometry)" /> <!-- Run depth odometry node -->
   </include>
 
 </launch>

localization/depth_odometry/CMakeLists.txt

@@ -23,6 +23,7 @@ add_compile_options(-std=c++14)
 
 ## Find catkin macros and libraries
 find_package(catkin2 REQUIRED COMPONENTS
+  camera
   ff_util
   localization_common 
   localization_measurements 

localization/depth_odometry/include/depth_odometry/depth_odometry_nodelet.h

@@ -19,6 +19,7 @@
 #define DEPTH_ODOMETRY_DEPTH_ODOMETRY_NODELET_H_
 
 #include <depth_odometry/depth_odometry_wrapper.h>
+#include <ff_msgs/SetBool.h>
 #include <ff_util/ff_nodelet.h>
 
 #include <image_transport/image_transport.h>
@@ -38,11 +39,14 @@ class DepthOdometryNodelet : public ff_util::FreeFlyerNodelet {
   void SubscribeAndAdvertise(ros::NodeHandle* nh);
   void PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg);
   void ImageCallback(const sensor_msgs::ImageConstPtr& image_msg);
+  bool EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res);
 
   DepthOdometryWrapper depth_odometry_wrapper_;
   image_transport::Subscriber image_sub_;
   ros::Subscriber point_cloud_sub_;
   ros::Publisher depth_odometry_pub_;
+  ros::ServiceServer enable_srv_;
+  bool enabled_;
 };
 }  // namespace depth_odometry
 

localization/depth_odometry/include/depth_odometry/depth_odometry_wrapper.h

@@ -32,12 +32,13 @@
 
 #include <sensor_msgs/PointCloud2.h>
 
+#include <string>
 #include <vector>
 
 namespace depth_odometry {
 class DepthOdometryWrapper {
  public:
-  DepthOdometryWrapper();
+  explicit DepthOdometryWrapper(const std::string& path_prefix = "localization/");
   explicit DepthOdometryWrapper(const DepthOdometryWrapperParams& params);
   std::vector<ff_msgs::DepthOdometry> PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg);
   std::vector<ff_msgs::DepthOdometry> ImageCallback(const sensor_msgs::ImageConstPtr& image_msg);

localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry.h

@@ -21,6 +21,7 @@
 #include <depth_odometry/depth_image_features_and_points.h>
 #include <depth_odometry/depth_odometry.h>
 #include <depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h>
+#include <depth_odometry/point_to_plane_icp_depth_odometry.h>
 #include <depth_odometry/pose_with_covariance_and_correspondences.h>
 #include <point_cloud_common/point_cloud_with_known_correspondences_aligner.h>
 #include <vision_common/feature_detector_and_matcher.h>
@@ -47,6 +48,7 @@ class ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry : public DepthOd
   std::unique_ptr<vision_common::FeatureDetectorAndMatcher> feature_detector_and_matcher_;
   cv::Ptr<cv::CLAHE> clahe_;
   bool normals_required_;
+  boost::optional<PointToPlaneICPDepthOdometry> point_to_plane_icp_depth_odometry_;
 };
 }  // namespace depth_odometry
 

localization/depth_odometry/include/depth_odometry/image_features_with_known_correspondences_aligner_depth_odometry_params.h

@@ -19,6 +19,7 @@
 #define DEPTH_ODOMETRY_IMAGE_FEATURES_WITH_KNOWN_CORRESPONDENCES_ALIGNER_DEPTH_ODOMETRY_PARAMS_H_
 
 #include <depth_odometry/depth_odometry_params.h>
+#include <depth_odometry/point_to_plane_icp_depth_odometry_params.h>
 #include <point_cloud_common/point_cloud_with_known_correspondences_aligner_params.h>
 #include <vision_common/brisk_feature_detector_and_matcher_params.h>
 #include <vision_common/lk_optical_flow_feature_detector_and_matcher_params.h>
@@ -41,6 +42,8 @@ struct ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometryParams : public
   double min_x_distance_to_border;
   double min_y_distance_to_border;
   int min_num_inliers;
+  bool refine_estimate;
+  PointToPlaneICPDepthOdometryParams point_to_plane_icp;
 };
 }  // namespace depth_odometry
 

localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry.h

@@ -28,7 +28,10 @@ class PointToPlaneICPDepthOdometry : public DepthOdometry {
  public:
   explicit PointToPlaneICPDepthOdometry(const PointToPlaneICPDepthOdometryParams& params);
   boost::optional<PoseWithCovarianceAndCorrespondences> DepthImageCallback(
-    const localization_measurements::DepthImageMeasurement& depth_image) final;
+    const localization_measurements::DepthImageMeasurement& depth_image_measurement) final;
+  boost::optional<PoseWithCovarianceAndCorrespondences> DepthImageCallbackWithEstimate(
+    const localization_measurements::DepthImageMeasurement& depth_image_measurement,
+    const boost::optional<Eigen::Isometry3d&> target_T_source_initial_estimate = boost::none);
   const PointToPlaneICPDepthOdometryParams& params() const { return params_; }
 
  private:

localization/depth_odometry/include/depth_odometry/point_to_plane_icp_depth_odometry_params.h

@@ -21,11 +21,23 @@
 #include <depth_odometry/depth_odometry_params.h>
 #include <point_cloud_common/point_to_plane_icp_params.h>
 
+#include <pcl/features/integral_image_normal.h>
+
 namespace depth_odometry {
 struct PointToPlaneICPDepthOdometryParams : public DepthOdometryParams {
   point_cloud_common::PointToPlaneICPParams icp;
   bool downsample;
   double downsample_leaf_size;
+  // Organized normal estimation
+  bool use_organized_normal_estimation;
+  pcl::IntegralImageNormalEstimation<pcl::PointXYZI, pcl::Normal>::NormalEstimationMethod normal_estimation_method;
+  bool use_depth_dependent_smoothing;
+  double max_depth_change_factor;
+  double normal_smoothing_size;
+  // Normal space sampling
+  bool use_normal_space_sampling;
+  int bins_per_axis;
+  int num_samples;
 };
 }  // namespace depth_odometry
 

localization/depth_odometry/package.xml

@@ -14,6 +14,7 @@
     Astrobee Flight Software
   </maintainer>
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>camera</build_depend>
   <build_depend>ff_msgs</build_depend>
   <build_depend>ff_util</build_depend>
   <build_depend>localization_common</build_depend>
@@ -26,6 +27,7 @@
   <build_depend>point_cloud_common</build_depend>
   <build_depend>vision_common</build_depend>
   <build_depend>roscpp</build_depend>
+  <run_depend>camera</run_depend>
   <run_depend>ff_msgs</run_depend>
   <run_depend>ff_util</run_depend>
   <run_depend>localization_common</run_depend>

localization/depth_odometry/src/depth_odometry_nodelet.cc

@@ -26,7 +26,7 @@ namespace depth_odometry {
 namespace lc = localization_common;
 namespace mc = msg_conversions;
 
-DepthOdometryNodelet::DepthOdometryNodelet() : ff_util::FreeFlyerNodelet(NODE_DEPTH_ODOM, true) {}
+DepthOdometryNodelet::DepthOdometryNodelet() : ff_util::FreeFlyerNodelet(NODE_DEPTH_ODOM, true), enabled_(false) {}
 
 void DepthOdometryNodelet::Initialize(ros::NodeHandle* nh) { SubscribeAndAdvertise(nh); }
 
@@ -44,21 +44,30 @@ void DepthOdometryNodelet::SubscribeAndAdvertise(ros::NodeHandle* nh) {
                                   static_cast<std::string>(TOPIC_HARDWARE_PICOFLEXX_SUFFIX_AMPLITUDE_IMAGE);
   image_sub_ = image_transport.subscribe(image_topic, 1, &DepthOdometryNodelet::ImageCallback, this);
   depth_odometry_pub_ = nh->advertise<ff_msgs::DepthOdometry>(TOPIC_LOCALIZATION_DEPTH_ODOM, 10);
+  enable_srv_ = nh->advertiseService(SERVICE_LOCALIZATION_DO_ENABLE, &DepthOdometryNodelet::EnableService, this);
 }
 
 void DepthOdometryNodelet::PointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg) {
+  if (!enabled_) return;
   const auto depth_odometry_msgs = depth_odometry_wrapper_.PointCloudCallback(point_cloud_msg);
   for (const auto& depth_odometry_msg : depth_odometry_msgs) {
     depth_odometry_pub_.publish(depth_odometry_msg);
   }
 }
 
 void DepthOdometryNodelet::ImageCallback(const sensor_msgs::ImageConstPtr& image_msg) {
+  if (!enabled_) return;
   const auto depth_odometry_msgs = depth_odometry_wrapper_.ImageCallback(image_msg);
   for (const auto& depth_odometry_msg : depth_odometry_msgs) {
     depth_odometry_pub_.publish(depth_odometry_msg);
   }
 }
+
+bool DepthOdometryNodelet::EnableService(ff_msgs::SetBool::Request& req, ff_msgs::SetBool::Response& res) {
+  enabled_ = req.enable;
+  res.success = true;
+  return true;
+}
 }  // namespace depth_odometry
 
 PLUGINLIB_EXPORT_CLASS(depth_odometry::DepthOdometryNodelet, nodelet::Nodelet);

localization/depth_odometry/src/depth_odometry_wrapper.cc

@@ -33,12 +33,12 @@ namespace lc = localization_common;
 namespace lm = localization_measurements;
 namespace mc = msg_conversions;
 
-DepthOdometryWrapper::DepthOdometryWrapper() {
+DepthOdometryWrapper::DepthOdometryWrapper(const std::string& config_prefix) {
   config_reader::ConfigReader config;
   config.AddFile("cameras.config");
   config.AddFile("transforms.config");
   config.AddFile("geometry.config");
-  config.AddFile("localization/depth_odometry.config");
+  config.AddFile((config_prefix + "depth_odometry.config").c_str());
   if (!config.ReadFiles()) {
     LogFatal("Failed to read config files.");
   }

localization/depth_odometry/src/image_features_with_known_correspondences_aligner_depth_odometry.cc

@@ -50,6 +50,10 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::ImageFeaturesWithKnow
     clahe_ = cv::createCLAHE(params_.clahe_clip_limit, cv::Size(params_.clahe_grid_length, params_.clahe_grid_length));
 
   normals_required_ = params_.aligner.use_point_to_plane_cost || params_.aligner.use_symmetric_point_to_plane_cost;
+
+  if (params_.refine_estimate) {
+    point_to_plane_icp_depth_odometry_ = PointToPlaneICPDepthOdometry(params_.point_to_plane_icp);
+  }
 }
 
 boost::optional<PoseWithCovarianceAndCorrespondences>
@@ -59,6 +63,7 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback(
     latest_depth_image_features_and_points_.reset(new DepthImageFeaturesAndPoints(
       depth_image_measurement.depth_image, *(feature_detector_and_matcher_->detector()), clahe_, normals_required_));
     latest_timestamp_ = depth_image_measurement.timestamp;
+    if (params_.refine_estimate) point_to_plane_icp_depth_odometry_->DepthImageCallback(depth_image_measurement);
     return boost::none;
   }
   const lc::Time timestamp = depth_image_measurement.timestamp;
@@ -134,13 +139,18 @@ ImageFeaturesWithKnownCorrespondencesAlignerDepthOdometry::DepthImageCallback(
   boost::optional<const std::vector<Eigen::Vector3d>&> target_normals_ref =
     normals_required_ ? boost::optional<const std::vector<Eigen::Vector3d>&>(target_normals) : boost::none;
 
-  const auto target_T_source =
+  auto target_T_source =
     aligner_.ComputeRelativeTransform(source_landmarks, target_landmarks, source_normals_ref, target_normals_ref);
   if (!target_T_source) {
     LogWarning("DepthImageCallback: Failed to get relative transform.");
     return boost::none;
   }
 
+  if (params_.refine_estimate) {
+    return point_to_plane_icp_depth_odometry_->DepthImageCallbackWithEstimate(depth_image_measurement,
+                                                                              target_T_source->pose);
+  }
+
   const auto source_T_target = lc::InvertPoseWithCovariance(*target_T_source);
 
   if (!lc::PoseCovarianceSane(source_T_target.covariance, params_.position_covariance_threshold,