general_factor.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 NullFactor {
  NullFactor() = default;
 
  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
  void update_linearized_system(
    const TargetPointCloud& target,
    const SourcePointCloud& source,
    const TargetTree& target_tree,
    const Eigen::Isometry3d& T,
    Eigen::Matrix<double, 6, 6>* H,
    Eigen::Matrix<double, 6, 1>* b,
    double* e) const {}
 
  template <typename TargetPointCloud, typename SourcePointCloud>
  void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {}
};
 
struct RestrictDoFFactor {
  RestrictDoFFactor() {
    lambda = 1e9;
    mask.setOnes();
  }
 
  void set_rotation_mask(const Eigen::Array3d& rot_mask) { mask.head<3>() = rot_mask; }
 
  void set_translation_mask(const Eigen::Array3d& trans_mask) { mask.tail<3>() = trans_mask; }
 
  template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
  void update_linearized_system(
    const TargetPointCloud& target,
    const SourcePointCloud& source,
    const TargetTree& target_tree,
    const Eigen::Isometry3d& T,
    Eigen::Matrix<double, 6, 6>* H,
    Eigen::Matrix<double, 6, 1>* b,
    double* e) const {
    *H += lambda * (mask - 1.0).abs().matrix().asDiagonal();
  }
 
  template <typename TargetPointCloud, typename SourcePointCloud>
  void update_error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, double* e) const {}
 
  double lambda;                    
  Eigen::Array<double, 6, 1> mask;  
};
 
}  // namespace small_gicp