robust kernel

include/small_gicp/factors/gicp_factor.hpp

@@ -12,8 +12,13 @@ namespace small_gicp {
 
 /// @brief GICP (distribution-to-distribution) per-point error factor.
 struct GICPFactor {
+  struct Setting {};
+
   /// @brief Constructor
-  GICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()), mahalanobis(Eigen::Matrix4d::Zero()) {}
+  GICPFactor(const Setting& setting = Setting())
+  : target_index(std::numeric_limits<size_t>::max()),
+    source_index(std::numeric_limits<size_t>::max()),
+    mahalanobis(Eigen::Matrix4d::Zero()) {}
 
   /// @brief Linearize the factor
   /// @param target       Target point cloud
@@ -25,7 +30,7 @@ struct GICPFactor {
   /// @param H            Linearized information matrix
   /// @param b            Linearized information vector
   /// @param e            Error at the linearization point
-  /// @return
+  /// @return             True if the point is inlier
   template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
   bool linearize(
     const TargetPointCloud& target,

include/small_gicp/factors/icp_factor.hpp

@@ -12,7 +12,9 @@ namespace small_gicp {
 
 /// @brief Point-to-point per-point error factor.
 struct ICPFactor {
-  ICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
+  struct Setting {};
+
+  ICPFactor(const Setting& setting = Setting()) : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
 
   template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
   bool linearize(

include/small_gicp/factors/plane_icp_factor.hpp

@@ -12,7 +12,9 @@ namespace small_gicp {
 
 /// @brief Point-to-plane per-point error factor.
 struct PointToPlaneICPFactor {
-  PointToPlaneICPFactor() : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
+  struct Setting {};
+
+  PointToPlaneICPFactor(const Setting& setting = Setting()) : target_index(std::numeric_limits<size_t>::max()), source_index(std::numeric_limits<size_t>::max()) {}
 
   template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
   bool linearize(

include/small_gicp/factors/robust_kernel.hpp

@@ -0,0 +1,106 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace small_gicp {
+
+/// @brief Huber robust kernel
+struct Huber {
+public:
+  /// @brief Huber robust kernel setting
+  struct Setting {
+    double c = 1.0;  ///< Kernel width
+  };
+
+  /// @brief Constructor
+  Huber(const Setting& setting) : c(setting.c) {}
+
+  /// @brief Compute the weight for an error
+  /// @param e Error
+  /// @return  Weight
+  double weight(double e) const {
+    const double e_abs = std::abs(e);
+    return e_abs < c ? 1.0 : c / e_abs;
+  }
+
+public:
+  const double c;  ///< Kernel width
+};
+
+/// @brief Cauchy robust kernel
+struct Cauchy {
+public:
+  /// @brief Huber robust kernel setting
+  struct Setting {
+    double c = 1.0;  ///< Kernel width
+  };
+
+  /// @brief Constructor
+  Cauchy(const Setting& setting) : c(setting.c) {}
+
+  /// @brief Compute the weight for an error
+  /// @param e Error
+  /// @return  Weight
+  double weight(double e) const { return c / (c + e * e); }
+
+public:
+  const double c;  ///< Kernel width
+};
+
+/// @brief Robustify a factor with a robust kernel
+/// @tparam Kernel  Robust kernel
+/// @tparam Factor  Factor
+template <typename Kernel, typename Factor>
+struct RobustFactor {
+public:
+  /// @brief Robust factor setting
+  struct Setting {
+    typename Kernel::Setting robust_kernel;  ///< Robust kernel setting
+    typename Factor::Setting factor;         ///< Factor setting
+  };
+
+  /// @brief Constructor
+  RobustFactor(const Setting& setting = Setting()) : robust_kernel(setting.robust_kernel), factor(setting.factor) {}
+
+  /// @brief Linearize the factor
+  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector>
+  bool linearize(
+    const TargetPointCloud& target,
+    const SourcePointCloud& source,
+    const TargetTree& target_tree,
+    const Eigen::Isometry3d& T,
+    size_t source_index,
+    const CorrespondenceRejector& rejector,
+    Eigen::Matrix<double, 6, 6>* H,
+    Eigen::Matrix<double, 6, 1>* b,
+    double* e) {
+    if (!factor.linearize(target, source, target_tree, T, source_index, rejector, H, b, e)) {
+      return false;
+    }
+
+    // Robustify the linearized factor
+    const double w = robust_kernel.weight(std::sqrt(*e));
+    *H *= w;
+    *b *= w;
+    *e *= w;
+
+    return true;
+  }
+
+  /// @brief Evaluate error
+  template <typename TargetPointCloud, typename SourcePointCloud>
+  double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const {
+    const double e = factor.error(target, source, T);
+    return robust_kernel.weight(std::sqrt(e)) * e;
+  }
+
+  /// @brief  Check if the factor is inlier
+  bool inlier() const { return factor.inlier(); }
+
+public:
+  Kernel robust_kernel;  ///< Robust kernel
+  Factor factor;         ///< Factor
+};
+
+}  // namespace small_gicp

include/small_gicp/registration/registration.hpp

@@ -15,7 +15,7 @@ namespace small_gicp {
 
 /// @brief Point cloud registration.
 template <
-  typename Factor,
+  typename PointFactor,
   typename Reduction,
   typename GeneralFactor = NullFactor,
   typename CorrespondenceRejector = DistanceRejector,
@@ -38,13 +38,16 @@ struct Registration {
       std::cerr << "warning: source point cloud is too small. |source|=" << traits::size(source) << std::endl;
     }
 
-    std::vector<Factor> factors(traits::size(source));
+    std::vector<PointFactor> factors(traits::size(source), PointFactor(point_factor));
     return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors, general_factor);
   }
 
 public:
+  using PointFactorSetting = typename PointFactor::Setting;
+
   TerminationCriteria criteria;     ///< Termination criteria
   CorrespondenceRejector rejector;  ///< Correspondence rejector
+  PointFactorSetting point_factor;  ///< Factor setting
   GeneralFactor general_factor;     ///< General factor
   Reduction reduction;              ///< Reduction
   Optimizer optimizer;              ///< Optimizer