Merge pull request #25 from KOKIAOKI/feature/fix_discrete_transformation

Feature/fix discrete transformation

Author: KOKIAOKI

CMakeLists.txt

@@ -26,7 +26,6 @@ include_directories(
 add_library(cpu_bbs3d
   SHARED
   bbs3d/src/cpu_bbs3d/bbs3d.cpp
-  bbs3d/src/cpu_bbs3d/discreate_transformation.cpp
   bbs3d/src/cpu_bbs3d/voxelmaps.cpp
   bbs3d/src/cpu_bbs3d/voxelmaps_io.cpp
 )
@@ -38,7 +37,6 @@ if(BUILD_CUDA)
     SHARED 
     bbs3d/src/gpu_bbs3d/bbs3d.cu
     bbs3d/src/gpu_bbs3d/calc_score.cu
-    bbs3d/src/gpu_bbs3d/discreate_transformation.cu
     bbs3d/src/gpu_bbs3d/voxelmaps.cu
     bbs3d/src/gpu_bbs3d/stream_manager/check_error.cu
     bbs3d/src/gpu_bbs3d/voxelmaps_io.cu

bbs3d/include/cpu_bbs3d/bbs3d.hpp

@@ -7,6 +7,8 @@
 #include <chrono>
 #include <Eigen/Dense>
 
+#include <discrete_transformation/discrete_transformation.hpp>
+
 namespace cpu {
 class VoxelMaps;
 
@@ -16,34 +18,6 @@ struct AngularInfo {
   Eigen::Vector3d min_rpy;
 };
 
-struct DiscreteTransformation {
-public:
-  DiscreteTransformation();
-  DiscreteTransformation(int score);
-  DiscreteTransformation(int score, int level, double resolution, double x, double y, double z, double roll, double pitch, double yaw);
-  ~DiscreteTransformation();
-
-  bool operator<(const DiscreteTransformation& rhs) const;
-
-  bool is_leaf() const;
-
-  Eigen::Matrix4d create_matrix() const;
-
-  std::vector<DiscreteTransformation> branch(const int child_level, const double child_res, const int v_rate, const AngularInfo& ang_info) const;
-
-  void calc_score(const std::vector<Eigen::Vector4i>& buckets, const int max_bucket_scan_count, const std::vector<Eigen::Vector3d>& points);
-
-  int score;
-  int level;
-  double resolution;
-  double x;
-  double y;
-  double z;
-  double roll;
-  double pitch;
-  double yaw;
-};
-
 class BBS3D {
 public:
   BBS3D();
@@ -118,7 +92,16 @@ class BBS3D {
 private:
   void calc_angular_info(std::vector<AngularInfo>& ang_info_vec);
 
-  std::vector<DiscreteTransformation> create_init_transset(const AngularInfo& init_ang_info);
+  std::vector<DiscreteTransformation<double>> create_init_transset(const AngularInfo& init_ang_info);
+
+  void calc_score(
+    DiscreteTransformation<double>& trans,
+    const double trans_res,
+    const Eigen::Vector3d& rpy_res,
+    const Eigen::Vector3d& min_rpy,
+    const std::vector<Eigen::Vector4i>& buckets,
+    const int max_bucket_scan_count,
+    const std::vector<Eigen::Vector3d>& points);
 
 private:
   Eigen::Matrix4d global_pose_;

bbs3d/include/discrete_transformation/discrete_transformation.hpp

@@ -0,0 +1,101 @@
+#pragma once
+#include <Eigen/Core>
+
+template <typename T>
+using Vector3 = Eigen::Matrix<T, 3, 1>;
+
+template <typename T>
+using Matrix4 = Eigen::Matrix<T, 4, 4>;
+
+template <typename T>
+class DiscreteTransformation {
+public:
+  DiscreteTransformation() : score(0), level(0), x(0), y(0), z(0), roll(0), pitch(0), yaw(0) {}
+
+  DiscreteTransformation(int score) : score(score), level(0), x(0), y(0), z(0), roll(0), pitch(0), yaw(0) {}
+
+  DiscreteTransformation(int score, int level, int x, int y, int z, int roll, int pitch, int yaw)
+  : score(score),
+    level(level),
+    x(x),
+    y(y),
+    z(z),
+    roll(roll),
+    pitch(pitch),
+    yaw(yaw) {}
+
+  ~DiscreteTransformation() {}
+
+  bool operator<(const DiscreteTransformation& rhs) const { return score < rhs.score; }
+
+  bool is_leaf() const { return level == 0; }
+
+  Matrix4<T> create_matrix(const T trans_res, const Vector3<T>& rpy_res, const Vector3<T>& min_rpy) {
+    Eigen::Translation<T, 3> translation(x * trans_res, y * trans_res, z * trans_res);
+    Eigen::AngleAxis<T> rollAngle(roll * rpy_res.x() + min_rpy.x(), Vector3<T>::UnitX());
+    Eigen::AngleAxis<T> pitchAngle(pitch * rpy_res.y() + min_rpy.y(), Vector3<T>::UnitY());
+    Eigen::AngleAxis<T> yawAngle(yaw * rpy_res.z() + min_rpy.z(), Vector3<T>::UnitZ());
+    return (translation * yawAngle * pitchAngle * rollAngle).matrix();
+  }
+
+  void branch(std::vector<DiscreteTransformation>& b, const int child_level, const int v_rate, const Eigen::Vector3i& num_division) {
+    for (int i = 0; i < v_rate; i++) {
+      for (int j = 0; j < v_rate; j++) {
+        for (int k = 0; k < v_rate; k++) {
+          for (int l = 0; l < num_division.x(); l++) {
+            for (int m = 0; m < num_division.y(); m++) {
+              for (int n = 0; n < num_division.z(); n++) {
+                b.emplace_back(DiscreteTransformation(
+                  0,
+                  child_level,
+                  x * v_rate + i,
+                  y * v_rate + j,
+                  z * v_rate + k,
+                  roll * num_division.x() + l,
+                  pitch * num_division.y() + m,
+                  yaw * num_division.z() + n));
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  std::vector<DiscreteTransformation> branch(const int child_level, const int v_rate, const Eigen::Vector3i& num_division) {
+    std::vector<DiscreteTransformation> b;
+    b.reserve(v_rate * v_rate * v_rate * num_division.x() * num_division.y() * num_division.z());
+    for (int i = 0; i < v_rate; i++) {
+      for (int j = 0; j < v_rate; j++) {
+        for (int k = 0; k < v_rate; k++) {
+          for (int l = 0; l < num_division.x(); l++) {
+            for (int m = 0; m < num_division.y(); m++) {
+              for (int n = 0; n < num_division.z(); n++) {
+                b.emplace_back(DiscreteTransformation(
+                  0,
+                  child_level,
+                  x * v_rate + i,
+                  y * v_rate + j,
+                  z * v_rate + k,
+                  roll * num_division.x() + l,
+                  pitch * num_division.y() + m,
+                  yaw * num_division.z() + n));
+              }
+            }
+          }
+        }
+      }
+    }
+    return b;
+  }
+
+public:
+  int score;
+  int level;
+  int x;
+  int y;
+  int z;
+  int roll;
+  int pitch;
+  int yaw;
+};

bbs3d/include/gpu_bbs3d/bbs3d.cuh

@@ -6,6 +6,8 @@
 #include <chrono>
 #include <Eigen/Dense>
 
+#include <discrete_transformation/discrete_transformation.hpp>
+
 // thrust
 #include <cuda_runtime.h>
 #include <thrust/host_vector.h>
@@ -20,33 +22,6 @@ struct AngularInfo {
   Eigen::Vector3f min_rpy;
 };
 
-struct DiscreteTransformation {
-public:
-  DiscreteTransformation();
-  DiscreteTransformation(int score);
-  DiscreteTransformation(int score, int level, float resolution, float x, float y, float z, float roll, float pitch, float yaw);
-  ~DiscreteTransformation();
-
-  bool operator<(const DiscreteTransformation& rhs) const;
-
-  bool is_leaf() const;
-
-  Eigen::Matrix4f create_matrix();
-
-  void branch(std::vector<DiscreteTransformation>& b, const int child_level, const float child_res, const int v_rate, const AngularInfo& ang_info);
-
-public:
-  int score;
-  int level;
-  float resolution;
-  float x;
-  float y;
-  float z;
-  float roll;
-  float pitch;
-  float yaw;
-};
-
 class BBS3D {
 public:
   BBS3D();
@@ -112,9 +87,11 @@ public:
 private:
   void calc_angular_info(std::vector<AngularInfo>& ang_info_vec);
 
-  std::vector<DiscreteTransformation> create_init_transset(const AngularInfo& init_ang_info);
+  std::vector<DiscreteTransformation<float>> create_init_transset(const AngularInfo& init_ang_info);
 
-  std::vector<DiscreteTransformation> calc_scores(const std::vector<DiscreteTransformation>& h_transset);
+  std::vector<DiscreteTransformation<float>> calc_scores(
+    const std::vector<DiscreteTransformation<float>>& h_transset,
+    thrust::device_vector<AngularInfo>& d_ang_info_vec);
 
   // pcd iof
   bool load_voxel_params(const std::string& voxelmaps_folder_path);

bbs3d/src/cpu_bbs3d/bbs3d.cpp

@@ -106,32 +106,22 @@ void BBS3D::calc_angular_info(std::vector<AngularInfo>& ang_info_vec) {
   }
 }
 
-std::vector<DiscreteTransformation> BBS3D::create_init_transset(const AngularInfo& init_ang_info) {
+std::vector<DiscreteTransformation<double>> BBS3D::create_init_transset(const AngularInfo& init_ang_info) {
   const int init_transset_size = (init_tx_range_.second - init_tx_range_.first + 1) * (init_ty_range_.second - init_ty_range_.first + 1) *
                                  (init_tz_range_.second - init_tz_range_.first + 1) * (init_ang_info.num_division.x()) *
                                  (init_ang_info.num_division.y()) * (init_ang_info.num_division.z());
 
   const int max_level = voxelmaps_ptr_->get_max_level();
-  const double init_trans_res = voxelmaps_ptr_->voxelmaps_res_[max_level];
 
-  std::vector<DiscreteTransformation> transset;
+  std::vector<DiscreteTransformation<double>> transset;
   transset.reserve(init_transset_size);
   for (int tx = init_tx_range_.first; tx <= init_tx_range_.second; tx++) {
     for (int ty = init_ty_range_.first; ty <= init_ty_range_.second; ty++) {
       for (int tz = init_tz_range_.first; tz <= init_tz_range_.second; tz++) {
         for (int roll = 0; roll < init_ang_info.num_division.x(); roll++) {
           for (int pitch = 0; pitch < init_ang_info.num_division.y(); pitch++) {
             for (int yaw = 0; yaw < init_ang_info.num_division.z(); yaw++) {
-              transset.emplace_back(DiscreteTransformation(
-                0,
-                max_level,
-                init_trans_res,
-                tx * init_trans_res,
-                ty * init_trans_res,
-                tz * init_trans_res,
-                roll * init_ang_info.rpy_res.x() + init_ang_info.min_rpy.x(),
-                pitch * init_ang_info.rpy_res.y() + init_ang_info.min_rpy.y(),
-                yaw * init_ang_info.rpy_res.z() + init_ang_info.min_rpy.z()));
+              transset.emplace_back(DiscreteTransformation<double>(0, max_level, tx, ty, tz, roll, pitch, yaw));
             }
           }
         }
@@ -141,6 +131,40 @@ std::vector<DiscreteTransformation> BBS3D::create_init_transset(const AngularInf
   return transset;
 }
 
+void BBS3D::calc_score(
+  DiscreteTransformation<double>& trans,
+  const double trans_res,
+  const Eigen::Vector3d& rpy_res,
+  const Eigen::Vector3d& min_rpy,
+  const std::vector<Eigen::Vector4i>& buckets,
+  const int max_bucket_scan_count,
+  const std::vector<Eigen::Vector3d>& points) {
+  const int num_buckets = buckets.size();
+  const double inv_res = 1.0 / trans_res;
+  Eigen::Transform<double, 3, Eigen::Affine> transform;
+  transform = trans.create_matrix(trans_res, rpy_res, min_rpy);
+
+  for (int i = 0; i < points.size(); i++) {
+    const Eigen::Vector3d transed_point = transform * points[i];
+    const Eigen::Vector3i coord = (transed_point.array() * inv_res).floor().cast<int>();
+    const std::uint32_t hash = (coord[0] * 73856093) ^ (coord[1] * 19349669) ^ (coord[2] * 83492791);
+
+    for (int j = 0; j < max_bucket_scan_count; j++) {
+      const std::uint32_t bucket_index = (hash + j) % num_buckets;
+      const Eigen::Vector4i& bucket = buckets[bucket_index];
+
+      if (bucket.x() != coord.x() || bucket.y() != coord.y() || bucket.z() != coord.z()) {
+        continue;
+      }
+
+      if (bucket.w() == 1) {
+        trans.score++;
+        break;
+      }
+    }
+  }
+}
+
 void BBS3D::localize() {
   // Calc localize time limit
   const auto start_time = std::chrono::system_clock::now();
@@ -150,7 +174,7 @@ void BBS3D::localize() {
   best_score_ = 0;
   const int score_threshold = std::floor(src_points_.size() * score_threshold_percentage_);
   int best_score = score_threshold;
-  DiscreteTransformation best_trans(best_score);
+  DiscreteTransformation<double> best_trans(best_score);
 
   // Preapre initial transset
   const int max_bucket_scan_count = voxelmaps_ptr_->get_max_bucket_scan_count();
@@ -161,12 +185,15 @@ void BBS3D::localize() {
 
   // Calc initial transset scores
   const auto& top_buckets = voxelmaps_ptr_->multi_buckets_[max_level];
+  double init_trans_res = voxelmaps_ptr_->voxelmaps_res_[max_level];
+  const Eigen::Vector3d& rpy_res = ang_info_vec[max_level].rpy_res;
+  const Eigen::Vector3d& min_rpy = ang_info_vec[max_level].min_rpy;
 #pragma omp parallel for num_threads(num_threads_)
   for (int i = 0; i < init_transset.size(); i++) {
-    init_transset[i].calc_score(top_buckets, max_bucket_scan_count, src_points_);
+    calc_score(init_transset[i], init_trans_res, rpy_res, min_rpy, top_buckets, max_bucket_scan_count, src_points_);
   }
 
-  std::priority_queue<DiscreteTransformation> trans_queue(init_transset.begin(), init_transset.end());
+  std::priority_queue<DiscreteTransformation<double>> trans_queue(init_transset.begin(), init_transset.end());
 
   while (!trans_queue.empty()) {
     if (use_timeout_ && std::chrono::system_clock::now() > time_limit) {
@@ -187,14 +214,17 @@ void BBS3D::localize() {
       best_score = trans.score;
     } else {
       const int child_level = trans.level - 1;
-      const double child_res = trans.resolution * inv_v_rate_;
-      auto children = trans.branch(child_level, child_res, static_cast<int>(v_rate_), ang_info_vec[child_level]);
+      const Eigen::Vector3i& num_division = ang_info_vec[child_level].num_division;
+      const Eigen::Vector3d& rpy_res = ang_info_vec[child_level].rpy_res;
+      const Eigen::Vector3d& min_rpy = ang_info_vec[child_level].min_rpy;
+      auto children = trans.branch(child_level, static_cast<int>(v_rate_), num_division);
 
       const auto& buckets = voxelmaps_ptr_->multi_buckets_[child_level];
+      double trans_res = voxelmaps_ptr_->voxelmaps_res_[child_level];
 
 #pragma omp parallel for num_threads(num_threads_)
       for (int i = 0; i < children.size(); i++) {
-        children[i].calc_score(buckets, max_bucket_scan_count, src_points_);
+        calc_score(children[i], trans_res, rpy_res, min_rpy, buckets, max_bucket_scan_count, src_points_);
       }
 
       for (const auto& child : children) {
@@ -218,7 +248,8 @@ void BBS3D::localize() {
     return;
   }
 
-  global_pose_ = best_trans.create_matrix();
+  double min_res = voxelmaps_ptr_->get_min_res();
+  global_pose_ = best_trans.create_matrix(min_res, ang_info_vec[0].rpy_res, ang_info_vec[0].min_rpy);
   best_score_ = best_score;
   has_timed_out_ = false;
   has_localized_ = true;