File between_factor_pose3_translation_norm.h

namespace sym

Functions

template<typename Scalar>
void BetweenFactorPose3TranslationNorm(const sym::Pose3<Scalar> &a, const sym::Pose3<Scalar> &b, const Scalar translation_norm, const Eigen::Matrix<Scalar, 1, 1> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 1, 1> *const res = nullptr, Eigen::Matrix<Scalar, 1, 12> *const jacobian = nullptr, Eigen::Matrix<Scalar, 12, 12> *const hessian = nullptr, Eigen::Matrix<Scalar, 12, 1> *const rhs = nullptr)

Residual that penalizes the difference between translation_norm and (a.t - b.t).norm().

Args: sqrt_info: Square root information matrix to whiten residual. In this one dimensional case this is just 1/sigma. jacobian: (1x12) jacobian of res wrt args a (6), b (6) hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) rhs: (12x1) Gauss-Newton rhs for args a (6), b (6)