symforce.codegen.slam_factors_codegen module#

Factors generally useful for visual SLAM problems (or visual odometry or structure-from-motion problems)

Currently contains reprojection factors for all camera models. For cameras that have backprojection, the factor operates on pixel coordinates in the source and target cameras. For cameras that do not have backprojection, it instead operates on a ray in the source camera frame and a pixel in the target camera.

inverse_range_landmark_prior_residual(landmark_inverse_range, inverse_range_prior, weight, sigma, epsilon)[source]#

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

Parameters:

  • landmark_inverse_range (float) – The current inverse range estimate

  • inverse_range_prior (float) – The mean of the inverse range prior

  • weight (float) – The weight of the prior

  • sigma (float) – The standard deviation of the prior

  • epsilon (float) – Small positive value

Return type:

Matrix11

Outputs:

res: 1dof residual of the prior

reprojection_delta(source_pose, source_calibration, target_pose, target_calibration, source_inverse_range, source_pixel, target_pixel, epsilon)[source]#

Reprojects the landmark into the target camera and returns the delta from the correspondence to the reprojection.

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).

Parameters:

  • source_pose (Pose3) – The pose of the source camera

  • source_calibration (CameraCal) – The source camera calibration

  • target_pose (Pose3) – The pose of the target camera

  • target_calibration (CameraCal) – The target camera calibration

  • source_inverse_range (float) – The inverse range of the landmark in the source camera

  • source_pixel (Matrix21) – The location of the landmark in the source camera

  • target_pixel (Matrix21) – The location of the correspondence in the target camera

  • epsilon (float) – Small positive value

  • camera_model_class – The subclass of CameraCal to use as the camera model

Return type:

Tuple[Matrix21, float]

Outputs:

res: 2dof pixel reprojection error valid: is valid projection or not

inverse_range_landmark_reprojection_error_residual(source_pose, source_calibration, target_pose, target_calibration, source_inverse_range, source_pixel, target_pixel, weight, epsilon, noise_model)[source]#

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 a ScalarNoiseModel. 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 ScalarNoiseModel.whiten_norm for more information on this.

Parameters:

  • source_pose (Pose3) – The pose of the source camera

  • source_calibration (CameraCal) – The source camera calibration

  • target_pose (Pose3) – The pose of the target camera

  • target_calibration (CameraCal) – The target camera calibration

  • source_inverse_range (float) – The inverse range of the landmark in the source camera

  • source_pixel (Matrix21) – The location of the landmark in the source camera

  • target_pixel (Matrix21) – The location of the correspondence in the target camera

  • weight (float) – The weight of the factor

  • epsilon (float) – Small positive value

  • noise_model (ScalarNoiseModel) – The noise model to use for whitening the residual

Return type:

Matrix21

Outputs:

res: 2dof residual of the reprojection

inverse_range_landmark_gnc_residual(source_pose, source_calibration, target_pose, target_calibration, source_inverse_range, source_pixel, target_pixel, weight, gnc_mu, gnc_scale, epsilon)[source]#

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 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 ScalarNoiseModel.whiten_norm for more information on this, and BarronNoiseModel for more information on the noise model.

Parameters:

  • source_pose (Pose3) – The pose of the source camera

  • source_calibration (CameraCal) – The source camera calibration

  • target_pose (Pose3) – The pose of the target camera

  • target_calibration (CameraCal) – The target camera calibration

  • source_inverse_range (float) – The inverse range of the landmark in the source camera

  • source_pixel (Matrix21) – The location of the landmark in the source camera

  • target_pixel (Matrix21) – The location of the correspondence in the target camera

  • weight (float) – The weight of the factor

  • gnc_mu (float) – The mu convexity parameter for the BarronNoiseModel

  • gnc_scale (float) – The scale parameter for the BarronNoiseModel

  • epsilon (float) – Small positive value

Return type:

Matrix21

Outputs:

res: 2dof residual of the reprojection

ray_reprojection_delta(source_pose, target_pose, target_calibration, source_inverse_range, p_camera_source, target_pixel, epsilon)[source]#

Reprojects the landmark ray into the target camera and returns the delta between the correspondence and the reprojection.

The landmark is specified as a 3D point or ray (will be normalized) in the source camera; 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).

Parameters:

  • source_pose (Pose3) – The pose of the source camera

  • target_pose (Pose3) – The pose of the target camera

  • target_calibration (CameraCal) – The target camera calibration

  • source_inverse_range (float) – The inverse range of the landmark in the source camera

  • p_camera_source (Matrix31) – The location of the landmark in the source camera coordinate, will be normalized

  • target_pixel (Matrix21) – The location of the correspondence in the target camera

  • epsilon (float) – Small positive value

Return type:

Tuple[Matrix21, float]

Outputs:

res: 2dof reprojection delta valid: is valid projection or not

inverse_range_landmark_ray_reprojection_error_residual(source_pose, target_pose, target_calibration, source_inverse_range, p_camera_source, target_pixel, weight, epsilon, noise_model)[source]#

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 a 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 ScalarNoiseModel.whiten_norm for more information on this, and BarronNoiseModel for more information on the noise model.

Parameters:

  • source_pose (Pose3) – The pose of the source camera

  • target_pose (Pose3) – The pose of the target camera

  • target_calibration (CameraCal) – The target spherical camera calibration

  • source_inverse_range (float) – The inverse range of the landmark in the source camera

  • p_camera_source (Matrix31) – The location of the landmark in the source camera coordinate, will be normalized

  • target_pixel (Matrix21) – The location of the correspondence in the target camera

  • weight (float) – The weight of the factor

  • gnc_mu – The mu convexity parameter for the BarronNoiseModel

  • gnc_scale – The scale parameter for the BarronNoiseModel

  • epsilon (float) – Small positive value

  • noise_model (ScalarNoiseModel) –

Return type:

Matrix21

Outputs:

res: 2dof whiten residual of the reprojection

inverse_range_landmark_ray_gnc_residual(source_pose, target_pose, target_calibration, source_inverse_range, p_camera_source, target_pixel, weight, gnc_mu, gnc_scale, epsilon)[source]#

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 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 ScalarNoiseModel.whiten_norm for more information on this, and BarronNoiseModel for more information on the noise model.

Parameters:

  • source_pose (Pose3) – The pose of the source camera

  • target_pose (Pose3) – The pose of the target camera

  • target_calibration (CameraCal) – The target spherical camera calibration

  • source_inverse_range (float) – The inverse range of the landmark in the source camera

  • p_camera_source (Matrix31) – The location of the landmark in the source camera coordinate, will be normalized

  • target_pixel (Matrix21) – The location of the correspondence in the target camera

  • weight (float) – The weight of the factor

  • gnc_mu (float) – The mu convexity parameter for the BarronNoiseModel

  • gnc_scale (float) – The scale parameter for the BarronNoiseModel

  • epsilon (float) – Small positive value

Return type:

Matrix21

Outputs:

res: 2dof whiten residual of the reprojection

generate(output_dir, config=None)[source]#

Generate the SLAM package for the given language.

Parameters:

  • output_dir (Path) – Directory to generate outputs into

  • config (CodegenConfig | None) – CodegenConfig, defaults to the default C++ config

Return type:

None