integrated_matching_cost_factor.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <memory>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam_points/util/gtsam_migration.hpp>
 
namespace gtsam_points {
 
class IntegratedMatchingCostFactor : public gtsam::NonlinearFactor {
public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
  using shared_ptr = gtsam_points::shared_ptr<IntegratedMatchingCostFactor>;
 
  IntegratedMatchingCostFactor(gtsam::Key target_key, gtsam::Key source_key);
 
  IntegratedMatchingCostFactor(const gtsam::Pose3& fixed_target_pose, gtsam::Key source_key);
 
  virtual ~IntegratedMatchingCostFactor() override;
 
  virtual size_t dim() const override { return 6; }
 
  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
  virtual double error(const gtsam::Values& values) const override;
  virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& values) const override;
 
  const Eigen::Isometry3d& get_fixed_target_pose() const { return fixed_target_pose; }
 
public:
  Eigen::Isometry3d calc_delta(const gtsam::Values& values) const;
 
  virtual size_t memory_usage() const;
 
  virtual void update_correspondences(const Eigen::Isometry3d& delta) const = 0;
 
  virtual double evaluate(
    const Eigen::Isometry3d& delta,
    Eigen::Matrix<double, 6, 6>* H_target = nullptr,
    Eigen::Matrix<double, 6, 6>* H_source = nullptr,
    Eigen::Matrix<double, 6, 6>* H_target_source = nullptr,
    Eigen::Matrix<double, 6, 1>* b_target = nullptr,
    Eigen::Matrix<double, 6, 1>* b_source = nullptr) const = 0;
 
protected:
  bool is_binary;
  Eigen::Isometry3d fixed_target_pose;
};
}  // namespace gtsam_points