bundle_adjustment_factor_evm.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <memory>
#include <unordered_map>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_points/util/gtsam_migration.hpp>
#include <gtsam_points/factors/bundle_adjustment_factor.hpp>
 
namespace gtsam_points {
 
struct BALMFeature;
 
class EVMBundleAdjustmentFactorBase : public BundleAdjustmentFactorBase {
public:
  using shared_ptr = gtsam_points::shared_ptr<EVMBundleAdjustmentFactorBase>;
 
  EVMBundleAdjustmentFactorBase();
  virtual ~EVMBundleAdjustmentFactorBase() 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 void add(const gtsam::Point3& pt, const gtsam::Key& key) override;
 
  virtual int num_points() const override { return points.size(); }
 
  virtual void set_scale(double scale) override;
 
protected:
  template <int k>
  double calc_eigenvalue(const std::vector<Eigen::Vector3d>& transed_points, Eigen::MatrixXd* H = nullptr, Eigen::MatrixXd* J = nullptr) const;
 
  Eigen::MatrixXd calc_pose_derivatives(const std::vector<Eigen::Vector3d>& transed_points) const;
 
  gtsam::GaussianFactor::shared_ptr compose_factor(const Eigen::MatrixXd& H, const Eigen::MatrixXd& J, double error) const;
 
protected:
  double error_scale;
  std::vector<gtsam::Key> keys;
  std::vector<gtsam::Point3> points;
  std::unordered_map<gtsam::Key, int> key_index;
};
 
class PlaneEVMFactor : public EVMBundleAdjustmentFactorBase {
public:
  using shared_ptr = gtsam_points::shared_ptr<PlaneEVMFactor>;
 
  PlaneEVMFactor();
  virtual ~PlaneEVMFactor() override;
 
  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
  virtual double error(const gtsam::Values& c) const override;
  virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& c) const override;
 
  // TODO: Add feature parameter extraction method
};
 
class EdgeEVMFactor : public EVMBundleAdjustmentFactorBase {
public:
  using shared_ptr = gtsam_points::shared_ptr<EdgeEVMFactor>;
 
  EdgeEVMFactor();
  virtual ~EdgeEVMFactor() override;
 
  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
  virtual double error(const gtsam::Values& c) const override;
  virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& c) const override;
};
}  // namespace gtsam_points