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

Outputs:

res: 1dof residual of the prior

Return type:

Matrix11

reprojection_delta(source_pose, source_calibration_storage, target_pose, target_calibration_storage, source_inverse_range, source_pixel, target_pixel, epsilon, camera_model_class)[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_storage (Matrix) – The storage vector of the source camera calibration

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

  • target_calibration_storage (Matrix) – The storage vector of 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 (Type[CameraCal]) – The subclass of CameraCal to use as the camera model

Outputs:

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

Return type:

Tuple[Matrix21, float]

inverse_range_landmark_reprojection_error_residual(source_pose, source_calibration_storage, target_pose, target_calibration_storage, source_inverse_range, source_pixel, target_pixel, weight, gnc_mu, gnc_scale, epsilon, camera_model_class)[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 Barron noise model. 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 the docstring for NoiseModel.whiten_norm for more information on this, and the docstring of BarronNoiseModel for more information on the noise model.

Parameters:

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

  • source_calibration_storage (Matrix) – The storage vector of the source camera calibration

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

  • target_calibration_storage (Matrix) – The storage vector of 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 Barron noise model

  • gnc_scale (float) – The scale parameter for the Barron noise model

  • epsilon (float) – Small positive value

  • camera_model_class (Type[CameraCal]) – The subclass of CameraCal to use as the camera model

Outputs:

res: 2dof residual of the reprojection

Return type:

Matrix21

ray_reprojection_delta(source_pose, target_pose, target_calibration_storage, source_inverse_range, p_camera_source, target_pixel, epsilon, target_camera_model_class)[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_storage (Matrix) – The storage vector of 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

  • target_camera_model_class (Type[CameraCal]) – The subclass of CameraCal to use as the target camera model

Outputs:

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

Return type:

Tuple[Matrix21, float]

inverse_range_landmark_ray_reprojection_error_residual(source_pose, target_pose, target_calibration_storage, source_inverse_range, p_camera_source, target_pixel, weight, gnc_mu, gnc_scale, epsilon, target_camera_model_class)[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 Barron noise model. 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 the docstring for NoiseModel.whiten_norm for more information on this, and the docstring of 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_storage (Matrix) – The storage vector of 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 Barron noise model

  • gnc_scale (float) – The scale parameter for the Barron noise model

  • epsilon (float) – Small positive value

  • target_camera_model_class (Type[CameraCal]) – The subclass of CameraCal to use as the target camera model

Outputs:

res: 2dof whiten residual of the reprojection

Return type:

Matrix21

generate(output_dir, config=None)[source]

Generate the SLAM package for the given language.

Parameters:

  • output_dir (Path) – Directory to generate outputs into

  • config (Optional[CodegenConfig]) – CodegenConfig, defaults to the default C++ config

Return type:

None