robust_kernel.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide
// SPDX-License-Identifier: MIT
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace small_gicp {
 
struct Huber {
public:
  struct Setting {
    double c = 1.0;  
  };
 
  Huber(const Setting& setting) : c(setting.c) {}
 
  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;  
};
 
struct Cauchy {
public:
  struct Setting {
    double c = 1.0;  
  };
 
  Cauchy(const Setting& setting) : c(setting.c) {}
 
  double weight(double e) const { return c / (c + e * e); }
 
public:
  const double c;  
};
 
template <typename Kernel, typename Factor>
struct RobustFactor {
public:
  struct Setting {
    typename Kernel::Setting robust_kernel;  
    typename Factor::Setting factor;         
  };
 
  RobustFactor(const Setting& setting = Setting()) : robust_kernel(setting.robust_kernel), factor(setting.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;
  }
 
  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;
  }
 
  bool inlier() const { return factor.inlier(); }
 
public:
  Kernel robust_kernel;  
  Factor factor;         
};
 
}  // namespace small_gicp