141 lines
4.3 KiB
C++
141 lines
4.3 KiB
C++
#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
|