v01

include/basalt/linearization/landmark_block.hpp

@@ -0,0 +1,140 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+
+#include <basalt/optimization/accumulator.h>
+#include <basalt/vi_estimator/landmark_database.h>
+#include <basalt/linearization/block_diagonal.hpp>
+
+namespace basalt {
+
+template <class Scalar>
+struct RelPoseLin {
+  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
+  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;
+
+  Mat4 T_t_h;
+  Mat6 d_rel_d_h;
+  Mat6 d_rel_d_t;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template <typename Scalar>
+class LandmarkBlock {
+ public:
+  struct Options {
+    // use Householder instead of Givens for marginalization
+    bool use_householder = true;
+
+    // use_valid_projections_only: if true, set invalid projection's
+    // residual and jacobian to 0; invalid means z <= 0
+    bool use_valid_projections_only = true;
+
+    // if > 0, use huber norm with given threshold, else squared norm
+    Scalar huber_parameter = 0;
+
+    // Standard deviation of reprojection error to weight visual measurements
+    Scalar obs_std_dev = 1;
+
+    // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
+    // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
+    Scalar jacobi_scaling_eps = 1e-6;
+  };
+
+  enum State {
+    Uninitialized = 0,
+    Allocated,
+    NumericalFailure,
+    Linearized,
+    Marginalized
+  };
+
+  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+
+  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+
+  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;
+
+  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  using RowMatX =
+      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+
+  using RowMat3 = Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>;
+
+  virtual ~LandmarkBlock(){};
+
+  virtual bool isNumericalFailure() const = 0;
+  virtual void allocateLandmark(
+      Keypoint<Scalar>& lm,
+      const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
+                                         RelPoseLin<Scalar>>& relative_pose_lin,
+      const Calibration<Scalar>& calib, const AbsOrderMap& aom,
+      const Options& options,
+      const std::map<TimeCamId, size_t>* rel_order = nullptr) = 0;
+
+  // may set state to NumericalFailure --> linearization at this state is
+  // unusable. Numeric check is only performed for residuals that were
+  // considered to be used (valid), which depends on
+  // use_valid_projections_only setting.
+  virtual Scalar linearizeLandmark() = 0;
+  virtual void performQR() = 0;
+
+  // Sets damping and maintains upper triangular matrix for landmarks.
+  virtual void setLandmarkDamping(Scalar lambda) = 0;
+
+  // lambda < 0 means computing exact model cost change
+  virtual void backSubstitute(const VecX& pose_inc, Scalar& l_diff) = 0;
+
+  virtual void addQ2JpTQ2Jp_mult_x(VecX& res, const VecX& x_pose) const = 0;
+
+  virtual void addQ2JpTQ2r(VecX& res) const = 0;
+
+  virtual void addJp_diag2(VecX& res) const = 0;
+
+  virtual void addQ2JpTQ2Jp_blockdiag(
+      BlockDiagonalAccumulator<Scalar>& accu) const = 0;
+
+  virtual void scaleJl_cols() = 0;
+  virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0;
+  virtual void printStorage(const std::string& filename) const = 0;
+  virtual State getState() const = 0;
+
+  virtual size_t numReducedCams() const = 0;
+
+  virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
+                                  size_t start_idx) const = 0;
+
+  virtual void get_dense_Q2Jp_Q2r_rel(
+      MatX& Q2Jp, VecX& Q2r, size_t start_idx,
+      const std::map<TimeCamId, size_t>& rel_order) const = 0;
+
+  virtual void add_dense_H_b(DenseAccumulator<Scalar>& accum) const = 0;
+
+  virtual void add_dense_H_b(MatX& H, VecX& b) const = 0;
+
+  virtual void add_dense_H_b_rel(
+      MatX& H_rel, VecX& b_rel,
+      const std::map<TimeCamId, size_t>& rel_order) const = 0;
+
+  virtual const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
+      const = 0;
+
+  virtual Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
+      const std::map<TimeCamId, size_t>& rel_order) const = 0;
+
+  virtual void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const = 0;
+
+  virtual TimeCamId getHostKf() const = 0;
+
+  virtual size_t numQ2rows() const = 0;
+
+  // factory method
+  template <int POSE_SIZE>
+  static std::unique_ptr<LandmarkBlock<Scalar>> createLandmarkBlock();
+};
+
+}  // namespace basalt