File inverse_range_landmark_prior_factor.h

namespace sym

Functions

template<typename Scalar>
void InverseRangeLandmarkPriorFactor(const Scalar landmark_inverse_range, const Scalar inverse_range_prior, const Scalar weight, const Scalar sigma, const Scalar epsilon, Eigen::Matrix<Scalar, 1, 1> *const res = nullptr, Eigen::Matrix<Scalar, 1, 1> *const jacobian = nullptr, Eigen::Matrix<Scalar, 1, 1> *const hessian = nullptr, Eigen::Matrix<Scalar, 1, 1> *const rhs = nullptr)

Factor representing a Gaussian prior on the inverse range of a landmark

Args: landmark_inverse_range: The current inverse range estimate inverse_range_prior: The mean of the inverse range prior weight: The weight of the prior sigma: The standard deviation of the prior epsilon: Small positive value

Outputs: res: 1dof residual of the prior jacobian: (1x1) jacobian of res wrt arg landmark_inverse_range (1) hessian: (1x1) Gauss-Newton hessian for arg landmark_inverse_range (1) rhs: (1x1) Gauss-Newton rhs for arg landmark_inverse_range (1)