Program Listing for File block-pose-prior-error-term-v2.h¶
↰ Return to documentation for file (algorithms/ceres-error-terms/include/ceres-error-terms/block-pose-prior-error-term-v2.h
)
#ifndef CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_H_
#define CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_H_
#include <memory>
#include <Eigen/Core>
#include <aslam/common/pose-types.h>
#include <ceres-error-terms/common.h>
#include <ceres-error-terms/parameterization/quaternion-param-jpl.h>
#include <ceres/ceres.h>
#include <ceres/sized_cost_function.h>
#include <maplab-common/quaternion-math.h>
namespace ceres_error_terms {
// Note: this error term accepts rotations expressed as quaternions
// in JPL convention [x, y, z, w]. This convention corresponds to the internal
// coefficient storage of Eigen so you can directly pass pointer to your
// Eigen quaternion data, e.g. your_eigen_quaternion.coeffs().data().
class BlockPosePriorErrorTermV2 {
public:
BlockPosePriorErrorTermV2(
const aslam::Transformation& T_G_S_measured,
const Eigen::Matrix<double, 6, 6>& covariance);
~BlockPosePriorErrorTermV2() {}
template <typename T>
bool operator()(
const T* const q_G_M_jpl_p_G_M, const T* const q_B_M_jpl_p_M_B,
const T* const q_S_B_jpl_p_S_B, T* residuals) const;
static constexpr int kResidualBlockSize = 6;
static constexpr int kOrientationBlockSize = 4;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// Don't change the ordering of the enum elements, they have to be the
// same as the order of the parameter blocks.
enum {
kIdxBaseframePose,
kIdxVertexPose,
kIdxSensorPose,
};
Eigen::Matrix<double, 6, 6> sqrt_information_matrix_;
Eigen::Quaterniond q_S_G_measured_;
Eigen::Vector3d p_G_S_measured_;
};
} // namespace ceres_error_terms
#include "ceres-error-terms/block-pose-prior-error-term-v2-inl.h"
#endif // CERES_ERROR_TERMS_BLOCK_POSE_PRIOR_ERROR_TERM_V2_H_