File inverse_range_landmark_atan_gnc_factor.h

namespace sym

Functions

template<typename Scalar>
void InverseRangeLandmarkAtanGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::ATANCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::ATANCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, 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 into the target camera and comparing against the correspondence in the target camera.

The landmark is specified as a pixel in the source camera and 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 source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera 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 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)