Srd submodule deployment

.gitmodules

@@ -1,8 +1,11 @@
 [submodule "steam"]
-	path = main/src/deps/steam
-	url = ../steam.git
-	branch = master
+    path = main/src/deps/steam
+    url = ../steam.git
+    branch = master
 [submodule "lgmath"]
-	path = main/src/deps/lgmath
-	url = ../lgmath.git
-	branch = master
+    path = main/src/deps/lgmath
+    url = ../lgmath.git
+    branch = master
+[submodule "spinning-radar-doppler"]
+    path = main/src/deps/spinning-radar-doppler
+    url = ../spinning-radar-doppler.git

main/src/vtr_radar/CMakeLists.txt

@@ -30,6 +30,7 @@ find_package(vtr_tactic REQUIRED)
 find_package(vtr_radar_msgs REQUIRED)
 
 find_package(navtech_msgs REQUIRED)
+find_package(spinning_radar_doppler REQUIRED)
 
 if(DEFINED VTR_ENABLE_RADAR)
 
@@ -49,7 +50,7 @@ ament_target_dependencies(${PROJECT_NAME}_components
   Eigen3 OpenCV
   cv_bridge pcl_conversions pcl_ros
   nav_msgs
-  lgmath steam
+  lgmath steam spinning_radar_doppler
   vtr_common vtr_logging vtr_tactic vtr_radar_msgs
 )
 
@@ -73,7 +74,7 @@ ament_export_dependencies(
   Eigen3 OpenCV
   cv_bridge pcl_conversions pcl_ros
   nav_msgs visualization_msgs
-  lgmath steam
+  lgmath steam spinning_radar_doppler
   vtr_common vtr_logging vtr_tactic vtr_radar_msgs navtech_msgs
 )
 

main/src/vtr_radar/include/vtr_radar/cache.hpp

@@ -26,10 +26,10 @@
 #include "navtech_msgs/msg/radar_b_scan_msg.hpp"
 
 #include "vtr_radar/data_types/point.hpp"
-#include "vtr_radar/data_types/azimuth.hpp"
 #include "vtr_radar/data_types/pointmap.hpp"
 #include "vtr_tactic/cache.hpp"
 #include "vtr_tactic/types.hpp"
+#include "srd/srd.hpp"
 
 #include"vtr_radar/types.hpp"
 
@@ -98,7 +98,7 @@ struct RadarQueryCache : virtual public tactic::QueryCache {
 
   // Doppler paper stuff
   tactic::Cache<Eigen::Vector2d> vel_meas;
-  tactic::Cache<DopplerScan> doppler_scan;
+  tactic::Cache<srd::DopplerScan> doppler_scan;
 };
 
 struct RadarOutputCache : virtual public tactic::OutputCache {

main/src/vtr_radar/include/vtr_radar/modules/modules.hpp

@@ -26,6 +26,7 @@
 
 #include "vtr_radar/modules/odometry/odometry_gyro_module.hpp"
 #include "vtr_radar/modules/odometry/odometry_gt_module.hpp"
+#include "vtr_radar/modules/odometry/odometry_doppler_module.hpp"
 
 #include "vtr_radar/modules/odometry/odometry_icp_module.hpp"
 #include "vtr_radar/modules/odometry/odometry_map_maintenance_module.hpp"

main/src/vtr_radar/include/vtr_radar/modules/odometry/odometry_doppler_module.hpp

@@ -0,0 +1,93 @@
+// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/**
+ * \file odometry_doppler_module.hpp
+ * \author Daniil Lisus, Autonomous Space Robotics Lab (ASRL)
+ */
+#pragma once
+
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+#include "steam.hpp"
+
+#include "vtr_radar/cache.hpp"
+#include "vtr_tactic/modules/base_module.hpp"
+#include "vtr_tactic/task_queue.hpp"
+
+namespace vtr {
+
+namespace radar {
+
+/** \brief Direct Doppler-based odometry. */
+class OdometryDopplerModule : public tactic::BaseModule {
+ public:
+  using PointCloudMsg = sensor_msgs::msg::PointCloud2;
+
+  /** \brief Static module identifier. */
+  static constexpr auto static_name = "radar.odometry_doppler";
+
+  /** \brief Config parameters. */
+  struct Config : public tactic::BaseModule::Config {
+    PTR_TYPEDEFS(Config);
+
+    // Undistortion trajectory parameters
+    Eigen::Matrix<double, 3, 1> traj_qc_diag =
+        Eigen::Matrix<double, 3, 1>::Ones();
+    // Number of threads for pointcloud processing
+    int num_threads = 4;
+    // Threshold for zeroing out velocity
+    double zero_velocity_threshold = 0.1;
+    // Whether to optimize the pose estimate using STEAM
+    // If false, direct discrete-time integration is used
+    bool optimize = true;
+    // Doppler bias
+    Eigen::Vector2d doppler_bias = Eigen::Vector2d::Zero();
+
+    int max_iter = 20;
+    bool verbose = false;
+    double gyro_cov = 1e-3;
+    double dopp_cauchy_k = 0.8;
+    double dopp_meas_std = 1.0;
+    int integration_steps = 100;
+    int traj_num_extra_states = 0;
+
+    static ConstPtr fromROS(const rclcpp::Node::SharedPtr &node,
+                            const std::string &param_prefix);
+  };
+
+  OdometryDopplerModule(
+      const Config::ConstPtr &config,
+      const std::shared_ptr<tactic::ModuleFactory> &module_factory = nullptr,
+      const std::string &name = static_name)
+      : tactic::BaseModule(module_factory, name), config_(config) {}
+
+ private:
+  int64_t scan_stamp_prev_ = 0;
+  Eigen::Vector2d v_r_v_in_r_prev_ = Eigen::Vector2d::Zero(); // Extracted velocity from previous frame
+  double yaw_rate_prev_ = 0.0;  // Last gyro yaw rate from previous frame
+  double preint_yaw_ = 0.0;
+  double yaw_rate_avg_prev_ = 0.0;
+  Eigen::Matrix4d T_r_delta_prev_ = Eigen::Matrix4d::Identity();
+  void run_(tactic::QueryCache &qdata, tactic::OutputCache &output,
+            const tactic::Graph::Ptr &graph,
+            const tactic::TaskExecutor::Ptr &executor) override;
+
+  Config::ConstPtr config_;
+
+  VTR_REGISTER_MODULE_DEC_TYPE(OdometryDopplerModule);
+};
+
+}  // namespace radar
+}  // namespace vtr

main/src/vtr_radar/include/vtr_radar/modules/preprocessing/extraction/doppler_extraction_module.hpp

@@ -40,7 +40,7 @@ class DopplerExtractionModule : public tactic::BaseModule {
     PTR_TYPEDEFS(Config);
 
     // General radar params
-    double radar_resolution = 0.0438;
+    double radar_resolution = 0.04381;
     double f_t = 76.04e9;    // Hz, start frequency ramp
     double meas_freq = 1600; // Hz, number of measurements per second
     double del_f = 893.0e6;  // Hz, frequency span during ramp
@@ -50,22 +50,21 @@ class DopplerExtractionModule : public tactic::BaseModule {
     double maxr = 200;
     double beta_corr_fact = 0.944;
     int pad_num = 100;
+    double max_velocity = 50.0;
 
     // Filter params
-    double sigma_gauss = 15;
+    double sigma_gauss = 15.0;
     double z_q = 2.5;
 
     // RANSAC params
-    int vel_dim = 2;
-    int ransac_max_iter = 1000;
+    int ransac_max_iter = 100;
     double ransac_threshold = 0.5;
     double ransac_prior_threshold = 0.5;
 
     // Estimation params
     int opt_max_iter = 100;
     double opt_threshold = 1.0e-6;
     double cauchy_rho = 0.8;
-    double trim_dist = 1000.0;
     double x_bias_slope = 0.0;
     double x_bias_intercept = 0.0;
     double y_bias_slope = 0.0;

main/src/vtr_radar/package.xml

@@ -25,6 +25,7 @@
 
   <depend>lgmath</depend>
   <depend>steam</depend>
+  <depend>spinning_radar_doppler</depend>
 
   <depend>vtr_common</depend>
   <depend>vtr_logging</depend>