add RBF-kernel-based covariance estimation

Author: koide3

.clang-format

@@ -2,25 +2,30 @@
 Language:        Cpp
 # BasedOnStyle:  Google
 AccessModifierOffset: -2
-AlignAfterOpenBracket: Align
+AlignAfterOpenBracket: AlwaysBreak
+AlignConsecutiveMacros: false
 AlignConsecutiveAssignments: false
 AlignConsecutiveDeclarations: false
 AlignEscapedNewlines: Left
 AlignOperands:   true
 AlignTrailingComments: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
+AllowAllArgumentsOnNextLine: false
+AllowAllConstructorInitializersOnNextLine: false
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: Never
 AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: Empty
-AllowShortIfStatementsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: WithoutElse
 AllowShortLoopsOnASingleLine: true
 AlwaysBreakAfterDefinitionReturnType: None
 AlwaysBreakAfterReturnType: None
 AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: true
-BinPackArguments: true
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: false
 BinPackParameters: false
 BraceWrapping:
+  AfterCaseLabel:  false
   AfterClass:      false
   AfterControlStatement: false
   AfterEnum:       false
@@ -39,38 +44,46 @@ BraceWrapping:
 BreakBeforeBinaryOperators: None
 BreakBeforeBraces: Attach
 BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
 BreakBeforeTernaryOperators: true
 BreakConstructorInitializersBeforeComma: false
 BreakConstructorInitializers: BeforeColon
 BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: false
-ColumnLimit:     256
+BreakStringLiterals: true
+ColumnLimit:     180
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerAllOnOneLineOrOnePerLine: true
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
 Cpp11BracedListStyle: true
-DerivePointerAlignment: true
+DeriveLineEnding: true
+DerivePointerAlignment: false
 DisableFormat:   false
 ExperimentalAutoDetectBinPacking: false
 FixNamespaceComments: true
 ForEachMacros:
   - foreach
   - Q_FOREACH
   - BOOST_FOREACH
-IncludeBlocks:   Preserve
+IncludeBlocks:   Regroup
 IncludeCategories:
   - Regex:           '^<ext/.*\.h>'
     Priority:        2
+    SortPriority:    0
   - Regex:           '^<.*\.h>'
     Priority:        1
+    SortPriority:    0
   - Regex:           '^<.*'
     Priority:        2
+    SortPriority:    0
   - Regex:           '.*'
     Priority:        3
+    SortPriority:    0
 IncludeIsMainRegex: '([-_](test|unittest))?$'
+IncludeIsMainSourceRegex: ''
 IndentCaseLabels: true
+IndentGotoLabels: true
 IndentPPDirectives: None
 IndentWidth:     2
 IndentWrappedFunctionNames: false
@@ -81,33 +94,75 @@ MacroBlockBegin: ''
 MacroBlockEnd:   ''
 MaxEmptyLinesToKeep: 1
 NamespaceIndentation: None
+ObjCBinPackProtocolList: Never
 ObjCBlockIndentWidth: 2
 ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: false
+ObjCSpaceBeforeProtocolList: true
 PenaltyBreakAssignment: 2
 PenaltyBreakBeforeFirstCallParameter: 1
 PenaltyBreakComment: 300
 PenaltyBreakFirstLessLess: 120
 PenaltyBreakString: 1000
-PenaltyExcessCharacter: 0
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
 PenaltyReturnTypeOnItsOwnLine: 200
 PointerAlignment: Left
+RawStringFormats:
+  - Language:        Cpp
+    Delimiters:
+      - cc
+      - CC
+      - cpp
+      - Cpp
+      - CPP
+      - 'c++'
+      - 'C++'
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+  - Language:        TextProto
+    Delimiters:
+      - pb
+      - PB
+      - proto
+      - PROTO
+    EnclosingFunctions:
+      - EqualsProto
+      - EquivToProto
+      - PARSE_PARTIAL_TEXT_PROTO
+      - PARSE_TEST_PROTO
+      - PARSE_TEXT_PROTO
+      - ParseTextOrDie
+      - ParseTextProtoOrDie
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
 ReflowComments:  true
 SortIncludes:    false
-SortUsingDeclarations: false
+SortUsingDeclarations: true
 SpaceAfterCStyleCast: false
-SpaceAfterTemplateKeyword: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
 SpaceBeforeAssignmentOperators: true
-SpaceBeforeParens: Never
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
 SpaceInEmptyParentheses: false
 SpacesBeforeTrailingComments: 2
 SpacesInAngles:  false
+SpacesInConditionalStatement: false
 SpacesInContainerLiterals: true
 SpacesInCStyleCastParentheses: false
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
 Standard:        Auto
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
 TabWidth:        8
+UseCRLF:         false
 UseTab:          Never
 ...
 

CMakeLists.txt

@@ -82,6 +82,8 @@ if(BUILD_VGICP_CUDA)
     src/fast_gicp/cuda/fast_vgicp_cuda.cu
     src/fast_gicp/cuda/brute_force_knn.cu
     src/fast_gicp/cuda/covariance_estimation.cu
+    src/fast_gicp/cuda/covariance_estimation_rbf.cu
+    src/fast_gicp/cuda/covariance_regularization.cu
     src/fast_gicp/cuda/gaussian_voxelmap.cu
     src/fast_gicp/cuda/find_voxel_correspondences.cu
     src/fast_gicp/cuda/compute_derivatives.cu

include/fast_gicp/cuda/covariance_estimation.cuh

@@ -8,8 +8,9 @@
 namespace fast_gicp {
 namespace cuda {
 
-void covariance_estimation(const thrust::device_vector<Eigen::Vector3f>& points, int k, const thrust::device_vector<int>& k_neighbors, thrust::device_vector<Eigen::Matrix3f>& covariances, RegularizationMethod method);
+void covariance_estimation(const thrust::device_vector<Eigen::Vector3f>& points, int k, const thrust::device_vector<int>& k_neighbors, thrust::device_vector<Eigen::Matrix3f>& covariances);
 
+void covariance_estimation_rbf(const thrust::device_vector<Eigen::Vector3f>& points, double kernel_width, double max_dist, thrust::device_vector<Eigen::Matrix3f>& covariances);
 }
   }  // namespace fast_gicp
 

include/fast_gicp/cuda/covariance_regularization.cuh

@@ -0,0 +1,16 @@
+#ifndef FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH
+#define FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH
+
+#include <Eigen/Core>
+#include <thrust/device_vector.h>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+namespace cuda {
+
+void covariance_regularization(thrust::device_vector<Eigen::Matrix3f>& covs, RegularizationMethod method);
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

include/fast_gicp/cuda/fast_vgicp_cuda.cuh

@@ -33,6 +33,7 @@ public:
   ~FastVGICPCudaCore();
 
   void set_resolution(double resolution);
+  void set_kernel_params(double kernel_width, double kernel_max_dist);
 
   void swap_source_and_target();
   void set_source_cloud(const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& cloud);
@@ -46,6 +47,9 @@ public:
   void calculate_source_covariances(RegularizationMethod method);
   void calculate_target_covariances(RegularizationMethod method);
 
+  void calculate_source_covariances_rbf(RegularizationMethod method);
+  void calculate_target_covariances_rbf(RegularizationMethod method);
+
   void get_source_covariances(std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>& covs) const;
   void get_target_covariances(std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>& covs) const;
 
@@ -62,6 +66,8 @@ public:
 
 public:
   double resolution;
+  double kernel_width;
+  double kernel_max_dist;
 
   std::unique_ptr<Points> source_points;
   std::unique_ptr<Points> target_points;

include/fast_gicp/gicp/fast_vgicp_cuda.hpp

@@ -18,7 +18,7 @@ namespace cuda {
 class FastVGICPCudaCore;
 }
 
-enum class NearestNeighborMethod { CPU_PARALLEL_KDTREE, GPU_BRUTEFORCE };
+enum class NearestNeighborMethod { CPU_PARALLEL_KDTREE, GPU_BRUTEFORCE, GPU_RBF_KERNEL };
 
 /**
  * @brief Fast Voxelized GICP algorithm boosted with CUDA
@@ -47,6 +47,7 @@ class FastVGICPCuda : public LsqRegistration<PointSource, PointTarget> {
 
   void setCorrespondenceRandomness(int k);
   void setResolution(double resolution);
+  void setKernelCovarianceEstimationParams(double kernel_width, double max_dist);
   void setRegularizationMethod(RegularizationMethod method);
   void setNearesetNeighborSearchMethod(NearestNeighborMethod method);
 

include/fast_gicp/gicp/impl/fast_vgicp_cuda_impl.hpp

@@ -28,6 +28,7 @@ FastVGICPCuda<PointSource, PointTarget>::FastVGICPCuda() : LsqRegistration<Point
 
   vgicp_cuda_.reset(new cuda::FastVGICPCudaCore());
   vgicp_cuda_->set_resolution(voxel_resolution_);
+  vgicp_cuda_->set_kernel_params(0.5, 3.0);
 }
 
 template<typename PointSource, typename PointTarget>
@@ -41,6 +42,9 @@ void FastVGICPCuda<PointSource, PointTarget>::setResolution(double resolution) {
   vgicp_cuda_->set_resolution(resolution);
 }
 
+template <typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setKernelCovarianceEstimationParams(double kernel_width, double max_dist) {}
+
 template<typename PointSource, typename PointTarget>
 void FastVGICPCuda<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
   regularization_method_ = method;
@@ -84,12 +88,16 @@ void FastVGICPCuda<PointSource, PointTarget>::setInputSource(const PointCloudSou
     case NearestNeighborMethod::CPU_PARALLEL_KDTREE: {
       std::vector<int> neighbors = find_neighbors_parallel_kdtree<PointSource>(k_correspondences_, cloud);
       vgicp_cuda_->set_source_neighbors(k_correspondences_, neighbors);
+      vgicp_cuda_->calculate_source_covariances(regularization_method_);
     } break;
     case NearestNeighborMethod::GPU_BRUTEFORCE:
       vgicp_cuda_->find_source_neighbors(k_correspondences_);
+      vgicp_cuda_->calculate_source_covariances(regularization_method_);
+      break;
+    case NearestNeighborMethod::GPU_RBF_KERNEL:
+      vgicp_cuda_->calculate_source_covariances_rbf(regularization_method_);
       break;
   }
-  vgicp_cuda_->calculate_source_covariances(regularization_method_);
 }
 
 template<typename PointSource, typename PointTarget>
@@ -109,12 +117,16 @@ void FastVGICPCuda<PointSource, PointTarget>::setInputTarget(const PointCloudTar
     case NearestNeighborMethod::CPU_PARALLEL_KDTREE: {
       std::vector<int> neighbors = find_neighbors_parallel_kdtree<PointTarget>(k_correspondences_, cloud);
       vgicp_cuda_->set_target_neighbors(k_correspondences_, neighbors);
+      vgicp_cuda_->calculate_target_covariances(regularization_method_);
     } break;
     case NearestNeighborMethod::GPU_BRUTEFORCE:
       vgicp_cuda_->find_target_neighbors(k_correspondences_);
+      vgicp_cuda_->calculate_target_covariances(regularization_method_);
+      break;
+    case NearestNeighborMethod::GPU_RBF_KERNEL:
+      vgicp_cuda_->calculate_target_covariances_rbf(regularization_method_);
       break;
   }
-  vgicp_cuda_->calculate_target_covariances(regularization_method_);
   vgicp_cuda_->create_target_voxelmap();
 }
 

src/align.cpp

@@ -186,6 +186,12 @@ int main(int argc, char** argv) {
   // this would be a good choice if your PC has a weak CPU and a strong GPU (e.g., NVIDIA Jetson)
   vgicp_cuda.setNearesetNeighborSearchMethod(fast_gicp::NearestNeighborMethod::GPU_BRUTEFORCE);
   test(vgicp_cuda, target_cloud, source_cloud);
+
+  std::cout << "--- vgicp_cuda (gpu_rbf_kernel) ---" << std::endl;
+  // use RBF-kernel-based covariance estimation
+  // extremely fast but maybe a bit inaccurate
+  vgicp_cuda.setNearesetNeighborSearchMethod(fast_gicp::NearestNeighborMethod::GPU_RBF_KERNEL);
+  test(vgicp_cuda, target_cloud, source_cloud);
 #endif
 
   return 0;

src/fast_gicp/cuda/covariance_estimation.cu

@@ -40,55 +40,14 @@ namespace {
 
     thrust::device_ptr<Eigen::Matrix3f> covariances_ptr;
   };
+  }  // namespace
 
-  struct covariance_regularization_svd{
-    __host__ __device__ void operator()(Eigen::Matrix3f& cov) const {
-      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig;
-      eig.computeDirect(cov);
+  void covariance_estimation(const thrust::device_vector<Eigen::Vector3f>& points, int k, const thrust::device_vector<int>& k_neighbors, thrust::device_vector<Eigen::Matrix3f>& covariances) {
+    thrust::device_vector<int> d_indices(points.size());
+    thrust::sequence(d_indices.begin(), d_indices.end());
 
-      // why this doen't work...???
-      // cov = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().inverse();
-      Eigen::Matrix3f values = Eigen::Vector3f(1e-3, 1, 1).asDiagonal();
-      Eigen::Matrix3f v_inv = eig.eigenvectors().inverse();
-      cov = eig.eigenvectors() * values * v_inv;
-
-      // JacobiSVD is not supported on CUDA
-      // Eigen::JacobiSVD(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);
-      // Eigen::Vector3f values(1, 1, 1e-3);
-      // cov = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
-    }
-  };
-
-  struct covariance_regularization_frobenius {
-    __host__ __device__ void operator()(Eigen::Matrix3f& cov) const {
-      float lambda = 1e-3;
-      Eigen::Matrix3f C = cov + lambda * Eigen::Matrix3f::Identity();
-      Eigen::Matrix3f C_inv = C.inverse();
-      Eigen::Matrix3f C_norm = (C_inv / C_inv.norm()).inverse();
-      cov = C_norm;
-    }
-  };
-}
-
-void covariance_estimation(const thrust::device_vector<Eigen::Vector3f>& points, int k, const thrust::device_vector<int>& k_neighbors, thrust::device_vector<Eigen::Matrix3f>& covariances, RegularizationMethod method) {
-  thrust::device_vector<int> d_indices(points.size());
-  thrust::sequence(d_indices.begin(), d_indices.end());
-
-  covariances.resize(points.size());
-  thrust::for_each(d_indices.begin(), d_indices.end(), covariance_estimation_kernel(points, k, k_neighbors, covariances));
-
-  switch(method) {
-    default:
-      std::cerr << "unimplemented covariance regularization method was selected!!" << std::endl;
-      abort();
-    case RegularizationMethod::PLANE:
-      thrust::for_each(covariances.begin(), covariances.end(), covariance_regularization_svd());
-      break;
-    case RegularizationMethod::FROBENIUS:
-      thrust::for_each(covariances.begin(), covariances.end(), covariance_regularization_frobenius());
-      break;
+    covariances.resize(points.size());
+    thrust::for_each(d_indices.begin(), d_indices.end(), covariance_estimation_kernel(points, k, k_neighbors, covariances));
   }
-}
-
   }
 }