working on producing results on boreas.

steam_icp/config/boreas_navtech_steamrio_config.yaml

@@ -15,7 +15,7 @@
     dataset_options:
       all_sequences: false
       root_path: /home/asrl/boreas_docker_local
-      sequence: "boreas-2020-12-04-14-00"
+      sequence: "boreas-2021-01-26-10-59"
       init_frame: 0 # 5675 (highway 7 front image)
       last_frame: 10000 # 5680 (highway 7 front image)
       min_dist_sensor_center: 2.0
@@ -57,9 +57,9 @@
         num_threads: 20
 
       steam:
-        qc_diag: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] #[50.0, 50.0, 50.0, 5.0, 5.0, 5.0]  # [1.0, 0.0001, 0.0001, 0.0001, 0.0001, 0.01]
+        qc_diag: [50.0, 50.0, 50.0, 5.0, 5.0, 5.0] #[50.0, 50.0, 50.0, 5.0, 5.0, 5.0]  # [1.0, 0.0001, 0.0001, 0.0001, 0.0001, 0.01]
         # qc_diag: [10000.0, 10000.0, 10000.0, 10000.0, 10000.0, 10000.0]
-        ad_diag: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+        ad_diag: [0.0, 0.0, 0.0, 100.0, 100.0, 100.0]
         # qc_diag: [50.0, 50.0, 50.0, 5.0, 5.0, 5.0]
         num_threads: 16
         # parameters copied from VTR3:
@@ -74,10 +74,10 @@
         p0_accel: [1.0e-4, 1.0e-4, 1.0e-4, 1.0e-1, 1.0e-1, 1.0e-1]
 
         gravity: -9.8042
-        r_imu_acc: [0.01, 100.0, 1.0]
-        r_imu_ang:  3.34471102e-03
-        q_bias_accel: [0.01, 1.0, 1.0e-1]
-        p0_bias_accel: [1.0e-2, 1.0e-2, 1.0e-2]
+        r_imu_acc: [1.0, 100.0, 1.0]
+        r_imu_ang:  3.34471102e-06
+        q_bias_accel: [0.1, 0.1, 1.0e-1]
+        p0_bias_accel: [1.0e-3, 1.0e-3, 1.0e-3]
         q_bias_gyro: 1.0e-4
         p0_bias_gyro: 1.0e-2
         use_imu: true

steam_icp/config/boreas_navtech_steamro_config.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
-    log_dir: /home/asrl/ASRL/temp/steam_icp/boreas_navtech/steamro
-    output_dir: /home/asrl/ASRL/temp/steam_icp/boreas_navtech/steamro
+    log_dir: /home/asrl/ASRL/temp/steam_icp/boreas_navtech/steamrio
+    output_dir: /home/asrl/ASRL/temp/steam_icp/boreas_navtech/steamrio
     eval_only: false
 
     # VISUALIZATION OPTIONS ----
@@ -16,7 +16,7 @@
     dataset_options:
       all_sequences: true
       root_path: /home/asrl/boreas_docker_local
-      sequence: "boreas-2021-04-22-15-00"
+      sequence: "boreas-2021-01-26-10-59"
       init_frame: 0 # 5675 (highway 7 front image)
       last_frame: 10000 # 5680 (highway 7 front image)
       min_dist_sensor_center: 2.0
@@ -39,7 +39,7 @@
     odometry: STEAMRO
     odometry_options:
       debug_print: true
-      debug_path: /home/krb/ASRL/temp/steam_icp/boreas_navtech/steamro
+      debug_path: /home/krb/ASRL/temp/steam_icp/boreas_navtech/steamrio
       min_number_neighbors: 1
       init_voxel_size: 1.0
       voxel_size: 1.0
@@ -61,7 +61,7 @@
       steam:
         qc_diag: [1.0, 0.004, 0.0001, 0.0001, 0.0001, 0.01] #[50.0, 50.0, 50.0, 5.0, 5.0, 5.0]  # [1.0, 0.0001, 0.0001, 0.0001, 0.0001, 0.01]
         # qc_diag: [50.0, 50.0, 50.0, 5.0, 5.0, 5.0]
-        num_threads: 1
+        num_threads: 16
         # parameters copied from VTR3:
         p2p_loss_func: CAUCHY  # (L2, DCS, CAUCHY, GM, HUBER)
         p2p_loss_sigma: 1.0

steam_icp/config/boreas_velodyne_steamlio_config.yaml

@@ -5,17 +5,17 @@
 
     # VISUALIZATION OPTIONS ----
     visualization_options:
-      odometry: false
+      odometry: true
       raw_points: false
-      sampled_points: false
+      sampled_points: true
       map_points: false
 
     # DATASET OPTIONS ----
     dataset: BoreasVelodyne
     dataset_options:
-      all_sequences: false
+      all_sequences: true
       root_path: /home/asrl/boreas_docker_local
-      sequence: "boreas-2021-01-26-10-59" #"boreas-2021-09-02-11-42"
+      sequence: "boreas-2020-12-04-14-00" #"boreas-2021-09-02-11-42"
       init_frame: 0 # 5675 (highway 7 front image)
       last_frame: 100000 # 5680 (highway 7 front image)
       min_dist_sensor_center: 5.0
@@ -24,14 +24,13 @@
       lidar_timestamp_round_hz: 400.0
 
     # ODOMETRY OPTIONS ----
-    odometry: STEAMLIO
+    odometry: STEAMLO
     odometry_options:
       debug_print: true
       debug_path: /home/asrl/ASRL/temp/steam_icp/boreas_velodyne/steamlio
       num_iters_icp: 10
       # init_num_frames: 0
       sample_voxel_size: 1.70
-      # min_number_neighbors: 3
       min_distance_points: 0.1
       max_num_points_in_voxel: 18
       min_number_neighbors: 18
@@ -85,13 +84,14 @@
         # r_imu_acc: [0.03418968, 0.01292128, 0.00767178]
         # r_imu_ang: [5.75866815e-06, 2.21540004e-05, 3.48544315e-06]
         r_imu_acc: [1.0, 100.0, 1.0]
-        r_imu_ang: [5.48885234e-06, 2.38789649e-05, 3.34471102e-06]
+        r_imu_ang: [3.34471102e-02, 3.34471102e-02, 3.34471102e-03]
         # q_bias_accel: [1.0e-11, 4.0e-3, 1.0e-3]
-        q_bias_accel: [1.0e-4, 1.0e-1, 1.0e-1]
-        p0_bias_accel: [1.0e-4, 1.0e-4, 1.0e-4]
-        q_bias_gyro: 1.0e-10
+        q_bias_accel: [0.1, 1.0e-1, 1.0e-1]
+        p0_bias_accel: [1.0e-2, 1.0e-2, 1.0e-2]
+        q_bias_gyro: 1.0e-4
         p0_bias_gyro: 1.0e-2
-        use_imu: true
+        use_imu: false
+        use_accel: false
         max_iterations: 5
         verbose: true
         delay_adding_points: 1
@@ -106,7 +106,9 @@
         xi_ig: [0., 0., 0., -1.51869911e-02,  3.07697005e-02, 0.]  # sim: [0., 0., 0., -0.0197052, 0.0285345, 0.] 
         p2p_loss_func: L2
         num_extra_states: 0
-        acc_loss_func: CAUCHY
+        acc_loss_func: L2
         acc_loss_sigma: 1.0
         gyro_loss_func: L2
-        gyro_loss_sigma: 1.0
+        gyro_loss_sigma: 1.0
+        # swf_inside_icp_at_begin: false
+        filter_lifetimes: true

steam_icp/config/boreas_velodyne_steamlo_config.yaml

@@ -57,3 +57,4 @@
         delay_adding_points: 1
         p2p_loss_func: L2
         num_extra_states: 0
+        filter_lifetimes: true

steam_icp/config/ncd_steamlio_config.yaml

@@ -83,4 +83,5 @@
         gyro_loss_func: L2
         gyro_loss_sigma: 1.0
         T_sr_vec: [ 0.01849613,  0.00982219, -0.028535  ,  0.        ,  0.        , 3.14159265]
-        # T_sr_vec: [ 0.,  0., 0.  ,  0.        ,  0.        , 0.]
+        # T_sr_vec: [ 0.,  0., 0.  ,  0.        ,  0.        , 0.]
+        filter_lifetimes: false

steam_icp/config/ncd_steamlo_config.yaml

@@ -96,4 +96,5 @@
         T_mi_prior_cov: [1.0e-2, 1.0e-2, 1.0e-3, 1.0, 1.0, 1.0e-4]
         acc_loss_func: L2
         acc_loss_sigma: 1.0
-        gravity: -9.81599  #-9.8042
+        gravity: -9.81599  #-9.8042
+        filter_lifetimes: false

steam_icp/include/steam_icp/datasets/boreas_navtech.hpp

@@ -61,9 +61,8 @@ class BoreasNavtechDataset : public Dataset {
       // "boreas-2022-05-13-10-30",  // marc santi
       // "boreas-2022-05-13-11-47",  // glen shields
       // "boreas-2022-05-18-17-23",  // cocksfield
-      // "boreas-2020-12-04-14-00", "boreas-2021-01-26-10-59", "boreas-2021-02-09-12-55", "boreas-2021-03-09-14-23",
-      // "boreas-2021-04-22-15-00", 
-      "boreas-2021-06-29-18-53", "boreas-2021-06-29-20-43", "boreas-2021-09-08-21-00",
+      "boreas-2020-12-04-14-00", "boreas-2021-01-26-10-59", "boreas-2021-02-09-12-55", "boreas-2021-03-09-14-23",
+      "boreas-2021-04-22-15-00", "boreas-2021-06-29-18-53", "boreas-2021-06-29-20-43", "boreas-2021-09-08-21-00",
       "boreas-2021-09-09-15-28", "boreas-2021-10-05-15-35", "boreas-2021-10-26-12-35", "boreas-2021-11-06-18-55",
       "boreas-2021-11-28-09-18",
   };

steam_icp/include/steam_icp/datasets/boreas_velodyne.hpp

@@ -60,10 +60,10 @@ class BoreasVelodyneDataset : public Dataset {
       // "boreas-2022-05-13-10-30",  // marc santi
       // "boreas-2022-05-13-11-47",  // glen shields
       // "boreas-2022-05-18-17-23",  // cocksfield
-      "boreas-2020-12-04-14-00", "boreas-2021-01-26-10-59", "boreas-2021-02-09-12-55", "boreas-2021-03-09-14-23",
-      "boreas-2021-04-22-15-00", "boreas-2021-06-29-18-53", "boreas-2021-06-29-20-43", "boreas-2021-09-08-21-00",
-      "boreas-2021-09-09-15-28", "boreas-2021-10-05-15-35", "boreas-2021-10-26-12-35", "boreas-2021-11-06-18-55",
-      "boreas-2021-11-28-09-18",
+      // "boreas-2020-12-04-14-00",
+      "boreas-2021-01-26-10-59", "boreas-2021-02-09-12-55", "boreas-2021-03-09-14-23", "boreas-2021-04-22-15-00",
+      "boreas-2021-06-29-18-53", "boreas-2021-06-29-20-43", "boreas-2021-09-08-21-00", "boreas-2021-09-09-15-28",
+      "boreas-2021-10-05-15-35", "boreas-2021-10-26-12-35", "boreas-2021-11-06-18-55", "boreas-2021-11-28-09-18",
   };
 
   STEAM_ICP_REGISTER_DATASET("BoreasVelodyne", BoreasVelodyneDataset);

steam_icp/include/steam_icp/odometry/steam_lio.hpp

@@ -70,6 +70,8 @@ class SteamLioOdometry : public Odometry {
     double acc_loss_sigma = 1.0;
     std::string gyro_loss_func = "L2";
     double gyro_loss_sigma = 1.0;
+    //
+    bool filter_lifetimes = false;
   };
 
   SteamLioOdometry(const Options &options);

steam_icp/include/steam_icp/odometry/steam_lo.hpp

@@ -56,6 +56,8 @@ class SteamLoOdometry : public Odometry {
     Eigen::Matrix<double, 6, 1> T_mi_init_cov = Eigen::Matrix<double, 6, 1>::Ones();
     std::string acc_loss_func = "L2";
     double acc_loss_sigma = 1.0;
+    //
+    bool filter_lifetimes = false;
   };
 
   SteamLoOdometry(const Options &options);

steam_icp/include/steam_icp/radar/detector.hpp

@@ -21,20 +21,21 @@
 #pragma once
 
 #include "opencv2/opencv.hpp"
+#include "steam_icp/point.hpp"
 
 namespace steam_icp {
 
-template <class PointT>
+// template <class PointT>
 class Detector {
  public:
   virtual ~Detector() = default;
 
-  virtual std::vector<PointT> run(const cv::Mat &raw_scan, const float &res, const std::vector<int64_t> &azimuth_times,
-                                  const std::vector<double> &azimuth_angles) = 0;
+  virtual std::vector<Point3D> run(const cv::Mat &raw_scan, const float &res, const std::vector<int64_t> &azimuth_times,
+                                   const std::vector<double> &azimuth_angles) = 0;
 };
 
-template <class PointT>
-class ModifiedCACFAR : public Detector<PointT> {
+// template <class PointT>
+class ModifiedCACFAR : public Detector /*<PointT>*/ {
  public:
   ModifiedCACFAR() = default;
   ModifiedCACFAR(int width, int guard, double threshold, double threshold2, double threshold3, int num_threads,
@@ -50,8 +51,8 @@ class ModifiedCACFAR : public Detector<PointT> {
         range_offset_(range_offset),
         initial_timestamp_(initial_timestamp_micro) {}
 
-  std::vector<PointT> run(const cv::Mat &raw_scan, const float &res, const std::vector<int64_t> &azimuth_times,
-                          const std::vector<double> &azimuth_angles) override;
+  std::vector<Point3D> run(const cv::Mat &raw_scan, const float &res, const std::vector<int64_t> &azimuth_times,
+                           const std::vector<double> &azimuth_angles) override;
 
  private:
   int width_ = 41;  // window = width + 2 * guard

steam_icp/include/steam_icp/radar/detector.inl

@@ -23,27 +23,28 @@
 
 namespace steam_icp {
 
-template <class PointT>
-std::vector<PointT> ModifiedCACFAR<PointT>::run(const cv::Mat &raw_scan, const float &res,
-                                                const std::vector<int64_t> &azimuth_times,
-                                                const std::vector<double> &azimuth_angles) {
+// template <class PointT>
+std::vector<Point3D> ModifiedCACFAR::run(const cv::Mat &raw_scan, const float &res,
+                                         const std::vector<int64_t> &azimuth_times,
+                                         const std::vector<double> &azimuth_angles) {
   const int rows = raw_scan.rows;
   const int cols = raw_scan.cols;
   if (width_ % 2 == 0) width_ += 1;
   const int w2 = std::floor(width_ / 2);
-  auto mincol = minr_ / res + w2 + guard_ + 1;
+  int mincol = minr_ / res + w2 + guard_ + 1;
   if (mincol > cols || mincol < 0) mincol = 0;
-  auto maxcol = maxr_ / res - w2 - guard_;
+  int maxcol = maxr_ / res - w2 - guard_;
   if (maxcol > cols || maxcol < 0) maxcol = cols;
   const int N = maxcol - mincol;
 
-  std::vector<PointT> raw_points;
+  std::vector<Point3D> raw_points;
+  raw_points.clear();
   raw_points.reserve(2000);
 
   const double time_delta = azimuth_times.back() - azimuth_times.front();
 
-#pragma omp declare reduction( \
-        merge_points : std::vector<PointT> : omp_out.insert(omp_out.end(), omp_in.begin(), omp_in.end()))
+#pragma omp declare reduction(merge_points : std::vector<Point3D> : omp_out.insert( \
+        omp_out.end(), omp_in.begin(), omp_in.end())) initializer(omp_priv = decltype(omp_orig)(omp_orig.size()))
 #pragma omp parallel for num_threads(num_threads_) reduction(merge_points : raw_points)
   for (int i = 0; i < rows; ++i) {
     const double azimuth = azimuth_angles[i];
@@ -71,21 +72,30 @@ std::vector<PointT> ModifiedCACFAR<PointT>::run(const cv::Mat &raw_scan, const f
         peak_points += j;
         num_peak_points += 1;
       } else if (num_peak_points > 0) {
-        PointT p;
+        Point3D p;
         const double rho = res * peak_points / num_peak_points + range_offset_;
         p.raw_pt[0] = rho * std::cos(-azimuth);
         p.raw_pt[1] = rho * std::sin(-azimuth);
         p.raw_pt[2] = 0.0;
         p.pt = p.raw_pt;
         p.timestamp = time;
         p.alpha_timestamp = alpha_time;
-        raw_points.emplace_back(p);
+        p.radial_velocity = rho;
+        raw_points.push_back(p);
         peak_points = 0;
         num_peak_points = 0;
       }
     }
   }
+
   raw_points.shrink_to_fit();
+  // sort points into a canonical order
+  std::sort(raw_points.begin(), raw_points.end(), [](Point3D a, Point3D b) {
+    if (a.timestamp == b.timestamp)
+      return a.radial_velocity < b.radial_velocity;
+    else
+      return a.timestamp < b.timestamp;
+  });
   return raw_points;
 }
 

steam_icp/src/datasets/boreas_navtech.cpp

@@ -213,19 +213,20 @@ std::vector<Point3D> BoreasNavtechSequence::readPointCloud(const std::string &pa
   //                                  options_.min_dist_sensor_center, options_.max_dist_sensor_center,
   //                                  options_.radar_range_offset, initial_timestamp_);
 
-  auto detector = [&]() -> ModifiedCACFAR<Point3D> {
+  auto detector = [&]() -> ModifiedCACFAR /*<Point3D>*/ {
     if (radar_resolution > 0.05) {
-      return ModifiedCACFAR<Point3D>(options_.modified_cacfar_width, options_.modified_cacfar_guard,
-                                     options_.modified_cacfar_threshold, options_.modified_cacfar_threshold2,
-                                     options_.modified_cacfar_threshold3, options_.modified_cacfar_num_threads,
-                                     options_.min_dist_sensor_center, options_.max_dist_sensor_center,
-                                     options_.radar_range_offset, initial_timestamp_);
+      return ModifiedCACFAR /*<Point3D>*/ (options_.modified_cacfar_width, options_.modified_cacfar_guard,
+                                           options_.modified_cacfar_threshold, options_.modified_cacfar_threshold2,
+                                           options_.modified_cacfar_threshold3, options_.modified_cacfar_num_threads,
+                                           options_.min_dist_sensor_center, options_.max_dist_sensor_center,
+                                           options_.radar_range_offset, initial_timestamp_);
     } else {
-      return ModifiedCACFAR<Point3D>(options_.modified_cacfar_width_0438, options_.modified_cacfar_guard_0438,
-                                     options_.modified_cacfar_threshold_0438, options_.modified_cacfar_threshold2_0438,
-                                     options_.modified_cacfar_threshold3_0438, options_.modified_cacfar_num_threads,
-                                     options_.min_dist_sensor_center, options_.max_dist_sensor_center,
-                                     options_.radar_range_offset, initial_timestamp_);
+      return ModifiedCACFAR /*<Point3D>*/ (
+          options_.modified_cacfar_width_0438, options_.modified_cacfar_guard_0438,
+          options_.modified_cacfar_threshold_0438, options_.modified_cacfar_threshold2_0438,
+          options_.modified_cacfar_threshold3_0438, options_.modified_cacfar_num_threads,
+          options_.min_dist_sensor_center, options_.max_dist_sensor_center, options_.radar_range_offset,
+          initial_timestamp_);
     }
   }();