bundle_adjustment_factor_lsq.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam_points/util/gtsam_migration.hpp>
#include <gtsam_points/factors/bundle_adjustment_factor.hpp>
 
namespace gtsam_points {
 
class LsqBundleAdjustmentFactor : public BundleAdjustmentFactorBase {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using shared_ptr = gtsam_points::shared_ptr<LsqBundleAdjustmentFactor>;
 
  LsqBundleAdjustmentFactor();
  virtual ~LsqBundleAdjustmentFactor() override;
 
  virtual void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
  virtual size_t dim() const override { return 6; }
  virtual double error(const gtsam::Values& c) const override;
  virtual gtsam::GaussianFactor::shared_ptr linearize(const gtsam::Values& c) const override;
 
  virtual void add(const gtsam::Point3& pt, const gtsam::Key& key) override;
  virtual int num_points() const override { return global_num_points; }
 
private:
  void update_global_distribution(const gtsam::Values& values) const;
 
protected:
  struct FrameDistribution;
 
  std::unordered_map<gtsam::Key, std::shared_ptr<FrameDistribution>> frame_dists;
 
  int global_num_points;
  mutable Eigen::Vector3d global_mean;
  mutable Eigen::Matrix3d global_cov;
  mutable Eigen::Vector3d global_normal;
};
 
}  // namespace gtsam_points