Program Listing for File loop-closure-edge-error-term.h

↰ Return to documentation for file (algorithms/ceres-error-terms/include/ceres-error-terms/loop-closure-edge-error-term.h)

#ifndef CERES_ERROR_TERMS_LOOP_CLOSURE_EDGE_ERROR_TERM_H_
#define CERES_ERROR_TERMS_LOOP_CLOSURE_EDGE_ERROR_TERM_H_

#include <Eigen/Dense>

#include <maplab-common/pose_types.h>
#include <maplab-common/quaternion-math.h>

namespace ceres_error_terms {

class LoopClosureEdgeErrorTerm {
 public:
  LoopClosureEdgeErrorTerm(
      const Eigen::Matrix<double, 7, 1>& q_AB__A_p_AB,
      const Eigen::Matrix<double, 6, 6>& T_A_B_covariance)
      : q_AB__A_p_AB_(q_AB__A_p_AB) {
    Eigen::Matrix<double, 6, 6> L = T_A_B_covariance.llt().matrixL();
    Eigen::Matrix<double, 6, 6> inv_L = Eigen::Matrix<double, 6, 6>::Identity();

    L.template triangularView<Eigen::Lower>().solveInPlace(inv_L);
    sqrt_information_matrix_ = inv_L;
  }

  // Version where the vertices are in different mission frame of reference.
  template <typename T>
  bool operator()(
      const T* const T_G_MA, const T* const T_MA_IA, const T* const T_G_MB,
      const T* const T_MB_IB, const T* const switch_variable,
      T* residuals) const;

  // Version where both vertices are in the same mission frame of reference.
  template <typename T>
  bool operator()(
      const T* const T_M_IA, const T* const T_M_IB,
      const T* const switch_variable, T* residuals) const;

  static constexpr int kResidualBlockSize = 6;
  static constexpr int kSwitchVariableBlockSize = 1;
  static constexpr int kOrientationBlockSize = 4;
  static constexpr int kPositionBlockSize = 3;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  Eigen::Matrix<double, 7, 1> q_AB__A_p_AB_;
  Eigen::Matrix<double, 6, 6> sqrt_information_matrix_;
};

}  // namespace ceres_error_terms
#include "ceres-error-terms/loop-closure-edge-error-term-inl.h"
#endif  // CERES_ERROR_TERMS_LOOP_CLOSURE_EDGE_ERROR_TERM_H_