File inverse_range_landmark_polynomial_gnc_factor.h

namespace sym

Functions

template<typename Scalar>
void InverseRangeLandmarkPolynomialGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::Pose3<Scalar> &target_pose, const sym::PolynomialCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 3, 1> &p_camera_source, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)

Return the 2dof residual of reprojecting the landmark ray into the target spherical camera and comparing it against the correspondence.

The landmark is specified as a camera point in the source camera with an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera target_pose: The pose of the target camera target_calibration: The target spherical camera calibration source_inverse_range: The inverse range of the landmark in the source camera p_camera_source: The location of the landmark in the source camera coordinate, will be normalized target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof whiten residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)